SRBA: Sparser Relative Bundle Adjustment
srba/models/observations_RelativePoses_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 RelativePoses_2D
00021     {
00022         static const size_t  OBS_DIMS = 3; 
00023         
00025         struct obs_data_t
00026         {
00027             double x,y;   
00028             double yaw;   
00029 
00031             template <class ARRAY>
00032             inline void getAsArray(ARRAY &obs) const {
00033                 obs[0]=x; obs[1]=y; obs[2]=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<RelativePoses_2D>
00047     {
00048         template <class POSE>
00049         static bool find_relative_pose(
00050             const mrpt::aligned_containers<RelativePoses_2D::obs_data_t>::vector_t & new_kf_obs,
00051             const mrpt::aligned_containers<RelativePoses_2D::obs_data_t>::vector_t & old_kf_obs,
00052             const RelativePoses_2D::TObservationParams &params,
00053             POSE &pose_new_kf_wrt_old_kf)
00054         {
00055             ASSERT_(new_kf_obs.size()==old_kf_obs.size())
00056             // Find the observation related to one of the two KFs connected by this new edge: it should have an exact (0,...,0) in its relative pose:
00057             for (size_t i=0;i<new_kf_obs.size();i++)
00058             {
00059                 const RelativePoses_2D::obs_data_t & kf0 = new_kf_obs[i], &kf1 = old_kf_obs[i];
00060                 if ( (kf0.x!=0 || kf0.y!=0 || kf0.yaw!=0) && 
00061                      (kf1.x!=0 || kf1.y!=0 || kf1.yaw!=0) )
00062                      continue; // skip.
00063                 const mrpt::poses::CPose2D new_obs(kf0.x,kf0.y,kf0.yaw);
00064                 const mrpt::poses::CPose2D old_obs(kf1.x,kf1.y,kf1.yaw);
00065                 pose_new_kf_wrt_old_kf = POSE(old_obs-new_obs);
00066                 return true;
00067             }
00068             return false; // None found (should not happen?)
00069         }
00070     };
00072 }
00073 } // end NS
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends