SRBA: Sparser Relative Bundle Adjustment
|
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 ¶ms, 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