SRBA: Sparser Relative Bundle Adjustment
srba/models/observations_RangeBearing_3D.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 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 &params,
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
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends