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_3D 00021 { 00022 static const size_t OBS_DIMS = 3; 00023 00025 struct obs_data_t 00026 { 00027 double range; 00028 double yaw; 00029 double pitch; 00030 00032 template <class ARRAY> 00033 inline void getAsArray(ARRAY &obs) const { 00034 obs[0] = range; obs[1] = yaw; obs[2] = pitch; 00035 } 00036 }; 00037 00040 struct TObservationParams 00041 { 00042 // This type of observations has no further parameters. 00043 }; 00044 }; 00045 00047 template <> struct landmark_matcher<RangeBearing_3D> 00048 { 00049 template <class POSE> 00050 static bool find_relative_pose( 00051 const mrpt::aligned_containers<RangeBearing_3D::obs_data_t>::vector_t & new_kf_obs, 00052 const mrpt::aligned_containers<RangeBearing_3D::obs_data_t>::vector_t & old_kf_obs, 00053 const RangeBearing_3D::TObservationParams ¶ms, 00054 POSE &pose_new_kf_wrt_old_kf) 00055 { 00056 ASSERT_(new_kf_obs.size()==old_kf_obs.size()) 00057 const size_t N=new_kf_obs.size(); 00058 mrpt::utils::TMatchingPairList matches; 00059 matches.reserve(N); 00060 for (size_t i=0;i<N;i++) 00061 matches.push_back( mrpt::utils::TMatchingPair(i,i, 00062 old_kf_obs[i].range * cos(old_kf_obs[i].yaw) * cos(old_kf_obs[i].pitch), old_kf_obs[i].range * sin(old_kf_obs[i].yaw) * cos(old_kf_obs[i].pitch), -old_kf_obs[i].range * sin(old_kf_obs[i].pitch), 00063 new_kf_obs[i].range * cos(new_kf_obs[i].yaw) * cos(new_kf_obs[i].pitch), new_kf_obs[i].range * sin(new_kf_obs[i].yaw) * cos(new_kf_obs[i].pitch), -new_kf_obs[i].range * sin(new_kf_obs[i].pitch) )); 00064 // Least-square optimal transformation: 00065 if (POSE::rotation_dimensions==2) 00066 { // SE(2) 00067 mrpt::math::TPose2D found_pose; 00068 if (!mrpt::tfest::se2_l2(matches,found_pose)) 00069 return false; 00070 pose_new_kf_wrt_old_kf = POSE( mrpt::poses::CPose2D(found_pose)); 00071 } 00072 else 00073 { // SE(3) 00074 mrpt::poses::CPose3DQuat found_pose; 00075 double found_scale; 00076 if (!mrpt::tfest::se3_l2(matches,found_pose,found_scale)) 00077 return false; 00078 pose_new_kf_wrt_old_kf = POSE(found_pose); 00079 } 00080 return true; 00081 } 00082 }; 00084 } 00085 } // end NS