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 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 ¶ms, 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