SRBA: Sparser Relative Bundle Adjustment
srba/models/observations_Cartesian_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 Cartesian_2D
00021     {
00022         static const size_t  OBS_DIMS = 2; 
00023         
00025         struct obs_data_t
00026         {
00027             mrpt::math::TPoint2D  pt;
00028 
00030             template <class ARRAY>
00031             inline void getAsArray(ARRAY &obs) const {
00032                 obs[0] = pt.x; obs[1] = pt.y;
00033             }
00034         };
00035 
00038         struct TObservationParams
00039         {
00040             // This type of observations has no further parameters.
00041         };
00042     };
00043 
00045     template <> struct landmark_matcher<Cartesian_2D>
00046     {
00047         template <class POSE>
00048         static bool find_relative_pose(
00049             const mrpt::aligned_containers<Cartesian_2D::obs_data_t>::vector_t & new_kf_obs,
00050             const mrpt::aligned_containers<Cartesian_2D::obs_data_t>::vector_t & old_kf_obs,
00051             const Cartesian_2D::TObservationParams &params,
00052             POSE &pose_new_kf_wrt_old_kf)
00053         {
00054             ASSERT_(new_kf_obs.size()==old_kf_obs.size())
00055             const size_t N=new_kf_obs.size();
00056             mrpt::utils::TMatchingPairList matches;
00057             matches.reserve(N);
00058             for (size_t i=0;i<N;i++)
00059                 matches.push_back( mrpt::utils::TMatchingPair(i,i, old_kf_obs[i].pt.x,old_kf_obs[i].pt.y,0,  new_kf_obs[i].pt.x,new_kf_obs[i].pt.y,0 ) );
00060             // Least-square optimal transformation:
00061             if (POSE::rotation_dimensions==2)
00062             { // SE(2)
00063                 mrpt::math::TPose2D found_pose;
00064                 if (!mrpt::tfest::se2_l2(matches,found_pose))
00065                     return false;
00066                 pose_new_kf_wrt_old_kf = POSE( mrpt::poses::CPose2D(found_pose));
00067             }
00068             else
00069             {  // SE(3)
00070                 mrpt::poses::CPose3DQuat found_pose;
00071                 double found_scale;
00072                 if (!mrpt::tfest::se3_l2(matches,found_pose,found_scale))
00073                     return false;
00074                 pose_new_kf_wrt_old_kf = POSE(found_pose);
00075             }
00076             return true;
00077         }
00078     };
00079 
00081 }
00082 } // end NS
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends