SRBA: Sparser Relative Bundle Adjustment
srba/models/observations_RangeBearing_2D.h
00001 /* +---------------------------------------------------------------------------+
00002    |                     Mobile Robot Programming Toolkit (MRPT)               |
00003    |                          http://www.mrpt.org/                             |
00004    |                                                                           |
00005    | Copyright (c) 2005-2015, Individual contributors, see AUTHORS file        |
00006    | See: http://www.mrpt.org/Authors - All rights reserved.                   |
00007    | Released under BSD License. See details in http://www.mrpt.org/License    |
00008    +---------------------------------------------------------------------------+ */
00009 
00010 #pragma once
00011 
00012 #include <mrpt/tfest.h>  // for use in landmark_matcher<>
00013 
00014 namespace srba {
00015 namespace observations {
00020     struct RangeBearing_2D
00021     {
00022         static const size_t  OBS_DIMS = 2; 
00023         
00025         struct obs_data_t
00026         {
00027             double range; 
00028             double yaw;   
00029 
00031             template <class ARRAY>
00032             inline void getAsArray(ARRAY &obs) const {
00033                 obs[0] = range; obs[1] = yaw;
00034             }
00035         };
00036 
00039         struct TObservationParams
00040         {
00041             // This type of observations has no further parameters.
00042         };
00043     };
00044 
00046     template <> struct landmark_matcher<RangeBearing_2D>
00047     {
00048         template <class POSE>
00049         static bool find_relative_pose(
00050             const mrpt::aligned_containers<RangeBearing_2D::obs_data_t>::vector_t & new_kf_obs,
00051             const mrpt::aligned_containers<RangeBearing_2D::obs_data_t>::vector_t & old_kf_obs,
00052             const RangeBearing_2D::TObservationParams &params,
00053             POSE &pose_new_kf_wrt_old_kf)
00054         {
00055             ASSERT_(new_kf_obs.size()==old_kf_obs.size())
00056             const size_t N=new_kf_obs.size();
00057             mrpt::utils::TMatchingPairList matches;
00058             matches.reserve(N);
00059             for (size_t i=0;i<N;i++)
00060                 matches.push_back( mrpt::utils::TMatchingPair(i,i, 
00061                     old_kf_obs[i].range * cos(old_kf_obs[i].yaw),old_kf_obs[i].range * sin(old_kf_obs[i].yaw),0,  
00062                     new_kf_obs[i].range * cos(new_kf_obs[i].yaw),new_kf_obs[i].range * sin(new_kf_obs[i].yaw),0 ) );
00063             // Least-square optimal transformation:
00064             if (POSE::rotation_dimensions==2)
00065             { // SE(2)
00066                 mrpt::math::TPose2D found_pose;
00067                 if (!mrpt::tfest::se2_l2(matches,found_pose))
00068                     return false;
00069                 pose_new_kf_wrt_old_kf = POSE( mrpt::poses::CPose2D(found_pose));
00070             }
00071             else
00072             {  // SE(3)
00073                 mrpt::poses::CPose3DQuat found_pose;
00074                 double found_scale;
00075                 if (!mrpt::tfest::se3_l2(matches,found_pose,found_scale))
00076                     return false;
00077                 pose_new_kf_wrt_old_kf = POSE(found_pose);
00078             }
00079             return true;
00080         }
00081     };
00083 }
00084 } // end NS
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends