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 Cartesian_3D 00021 { 00022 static const size_t OBS_DIMS = 3; 00023 00025 struct obs_data_t 00026 { 00027 mrpt::math::TPoint3D pt; 00028 00030 template <class ARRAY> 00031 inline void getAsArray(ARRAY &obs) const { 00032 obs[0] = pt.x; obs[1] = pt.y; obs[2] = pt.z; 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_3D> 00046 { 00047 template <class POSE> 00048 static bool find_relative_pose( 00049 const mrpt::aligned_containers<Cartesian_3D::obs_data_t>::vector_t & new_kf_obs, 00050 const mrpt::aligned_containers<Cartesian_3D::obs_data_t>::vector_t & old_kf_obs, 00051 const Cartesian_3D::TObservationParams ¶ms, 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,old_kf_obs[i].pt.z, new_kf_obs[i].pt.x,new_kf_obs[i].pt.y,new_kf_obs[i].pt.z ) ); 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 }; 00080 } 00081 } // end NS