|
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/poses/CPose3DQuat.h> 00013 00014 namespace srba { 00023 template <> 00024 struct sensor_model<landmarks::Euclidean3D,observations::MonocularCamera> 00025 { 00026 // -------------------------------------------------------------------------------- 00027 // Typedefs for the sake of generality in the signature of methods below: 00028 // *DONT FORGET* to change these when writing new sensor models. 00029 // -------------------------------------------------------------------------------- 00030 typedef observations::MonocularCamera OBS_T; 00031 typedef landmarks::Euclidean3D LANDMARK_T; 00032 // -------------------------------------------------------------------------------- 00033 00034 static const size_t OBS_DIMS = OBS_T::OBS_DIMS; 00035 static const size_t LM_DIMS = LANDMARK_T::LM_DIMS; 00036 00037 typedef Eigen::Matrix<double,OBS_DIMS,LM_DIMS> TJacobian_dh_dx; 00038 typedef landmark_traits<LANDMARK_T>::array_landmark_t array_landmark_t; 00039 typedef OBS_T::TObservationParams TObservationParams; 00040 00041 00049 template <class POSE_T> 00050 static void observe_error( 00051 observation_traits<OBS_T>::array_obs_t & out_obs_err, 00052 const observation_traits<OBS_T>::array_obs_t & z_obs, 00053 const POSE_T & base_pose_wrt_observer, 00054 const landmark_traits<LANDMARK_T>::array_landmark_t & lm_pos, 00055 const OBS_T::TObservationParams & params) 00056 { 00057 double x,y,z; // wrt cam (local coords) 00058 base_pose_wrt_observer.composePoint(lm_pos[0],lm_pos[1],lm_pos[2], x,y,z); 00059 ASSERT_(z!=0) 00060 00061 // Pinhole model: 00062 observation_traits<OBS_T>::array_obs_t pred_obs; // prediction 00063 pred_obs[0] = params.camera_calib.cx() + params.camera_calib.fx() * x/z; 00064 pred_obs[1] = params.camera_calib.cy() + params.camera_calib.fy() * y/z; 00065 out_obs_err = z_obs - pred_obs; 00066 } 00067 00085 static bool eval_jacob_dh_dx( 00086 TJacobian_dh_dx & dh_dx, 00087 const array_landmark_t & xji_l, 00088 const TObservationParams & sensor_params) 00089 { 00090 // xji_l[0:2]=[X Y Z] 00091 // If the point is behind us, mark this Jacobian as invalid. This is probably a temporary situation until we get closer to the optimum. 00092 if (xji_l[2]<=0) 00093 return false; 00094 00095 const double pz_inv = 1.0/xji_l[2]; 00096 const double pz_inv2 = pz_inv*pz_inv; 00097 00098 const double cam_fx = sensor_params.camera_calib.fx(); 00099 const double cam_fy = sensor_params.camera_calib.fy(); 00100 00101 dh_dx.coeffRef(0,0)= cam_fx * pz_inv; 00102 dh_dx.coeffRef(0,1)= 0; 00103 dh_dx.coeffRef(0,2)= -cam_fx * xji_l[0] * pz_inv2; 00104 00105 dh_dx.coeffRef(1,0)= 0; 00106 dh_dx.coeffRef(1,1)= cam_fy * pz_inv; 00107 dh_dx.coeffRef(1,2)= -cam_fy * xji_l[1] * pz_inv2; 00108 00109 return true; 00110 } 00111 00121 static void inverse_sensor_model( 00122 landmark_traits<LANDMARK_T>::array_landmark_t & out_lm_pos, 00123 const observation_traits<OBS_T>::obs_data_t & obs, 00124 const OBS_T::TObservationParams & params) 00125 { 00126 // > +Z 00127 // / 00128 // / 00129 // +----> +X 00130 // | 00131 // | 00132 // V +Y 00133 // 00134 const double fx = params.camera_calib.fx(); 00135 const double fy = params.camera_calib.fy(); 00136 const double cx = params.camera_calib.cx(); 00137 const double cy = params.camera_calib.cy(); 00138 out_lm_pos[0] = (obs.px.x-cx)/fx; 00139 out_lm_pos[1] = (obs.px.y-cy)/fy; 00140 out_lm_pos[2] = 1; // Depth in Z (undefined for monocular cameras, just use any !=0 value) 00141 } 00142 00143 }; // end of struct sensor_model<landmarks::Euclidean3D,observations::MonocularCamera> 00144 00145 // ------------------------------------------------------------------------------------------------------------- 00146 00148 template <> 00149 struct sensor_model<landmarks::Euclidean3D,observations::StereoCamera> 00150 { 00151 // -------------------------------------------------------------------------------- 00152 // Typedefs for the sake of generality in the signature of methods below: 00153 // *DONT FORGET* to change these when writing new sensor models. 00154 // -------------------------------------------------------------------------------- 00155 typedef observations::StereoCamera OBS_T; 00156 typedef landmarks::Euclidean3D LANDMARK_T; 00157 // -------------------------------------------------------------------------------- 00158 00159 static const size_t OBS_DIMS = OBS_T::OBS_DIMS; 00160 static const size_t LM_DIMS = LANDMARK_T::LM_DIMS; 00161 00162 typedef Eigen::Matrix<double,OBS_DIMS,LM_DIMS> TJacobian_dh_dx; 00163 typedef landmark_traits<LANDMARK_T>::array_landmark_t array_landmark_t; 00164 typedef OBS_T::TObservationParams TObservationParams; 00165 00166 00174 template <class POSE_T> 00175 static void observe_error( 00176 observation_traits<OBS_T>::array_obs_t & out_obs_err, 00177 const observation_traits<OBS_T>::array_obs_t & z_obs, 00178 const POSE_T & base_pose_wrt_observer, 00179 const landmark_traits<LANDMARK_T>::array_landmark_t & lm_pos, 00180 const OBS_T::TObservationParams & params) 00181 { 00182 double lx,ly,lz; // wrt cam (local coords) 00183 base_pose_wrt_observer.composePoint(lm_pos[0],lm_pos[1],lm_pos[2], lx,ly,lz); 00184 ASSERT_(lz!=0) 00185 00186 observation_traits<OBS_T>::array_obs_t pred_obs; // prediction 00187 // Pinhole model: Left camera. 00188 const mrpt::utils::TCamera &lc = params.camera_calib.leftCamera; 00189 pred_obs[0] = lc.cx() + lc.fx() * lx/lz; 00190 pred_obs[1] = lc.cy() + lc.fy() * ly/lz; 00191 00192 // Project point relative to right-camera: 00193 const mrpt::poses::CPose3DQuat R2L = -params.camera_calib.rightCameraPose; // R2L = (-) Left-to-right_camera_pose 00194 00195 // base_wrt_right_cam = ( (-)L2R ) (+) L2Base 00196 //mrpt::poses::CPose3DQuat base_wrt_right_cam(mrpt::poses::UNINITIALIZED_POSE); 00197 //base_wrt_right_cam.composeFrom(R2L,mrpt::poses::CPose3DQuat(base_pose_wrt_observer)); 00198 00199 double rx,ry,rz; // wrt cam (local coords) 00200 //base_wrt_right_cam.composePoint(lx,ly,lz, rx,ry,rz); 00201 00202 // xji_l_right = R2L (+) Xji_l 00203 R2L.composePoint(lx,ly,lz, rx,ry,rz); 00204 ASSERT_(rz!=0) 00205 00206 // Pinhole model: Right camera. 00207 const mrpt::utils::TCamera &rc = params.camera_calib.rightCamera; 00208 pred_obs[2] = rc.cx() + rc.fx() * rx/rz; 00209 pred_obs[3] = rc.cy() + rc.fy() * ry/rz; 00210 out_obs_err = z_obs - pred_obs; 00211 } 00212 00230 static bool eval_jacob_dh_dx( 00231 TJacobian_dh_dx & dh_dx, 00232 const array_landmark_t & xji_l, 00233 const TObservationParams & sensor_params) 00234 { 00235 // xji_l[0:2]=[X Y Z] 00236 // If the point is behind us, mark this Jacobian as invalid. This is probably a temporary situation until we get closer to the optimum. 00237 if (xji_l[2]<=0) 00238 return false; 00239 00240 // Left camera: 00241 { 00242 const double pz_inv = 1.0/xji_l[2]; 00243 const double pz_inv2 = pz_inv*pz_inv; 00244 00245 const double cam_fx = sensor_params.camera_calib.leftCamera.fx(); 00246 const double cam_fy = sensor_params.camera_calib.leftCamera.fy(); 00247 00248 dh_dx.coeffRef(0,0)= cam_fx * pz_inv; 00249 dh_dx.coeffRef(0,1)= 0; 00250 dh_dx.coeffRef(0,2)= -cam_fx * xji_l[0] * pz_inv2; 00251 00252 dh_dx.coeffRef(1,0)= 0; 00253 dh_dx.coeffRef(1,1)= cam_fy * pz_inv; 00254 dh_dx.coeffRef(1,2)= -cam_fy * xji_l[1] * pz_inv2; 00255 } 00256 00257 // Right camera: 00258 array_landmark_t xji_l_right; // xji_l_right = R2L (+) Xji_l 00259 const mrpt::poses::CPose3DQuat R2L = -sensor_params.camera_calib.rightCameraPose; // R2L = (-) Left-to-right_camera_pose 00260 R2L.composePoint( 00261 xji_l[0],xji_l[1],xji_l[2], 00262 xji_l_right[0],xji_l_right[1],xji_l_right[2]); 00263 { 00264 const double pz_inv = 1.0/xji_l_right[2]; 00265 const double pz_inv2 = pz_inv*pz_inv; 00266 00267 const double cam_fx = sensor_params.camera_calib.rightCamera.fx(); 00268 const double cam_fy = sensor_params.camera_calib.rightCamera.fy(); 00269 00270 dh_dx.coeffRef(2,0)= cam_fx * pz_inv; 00271 dh_dx.coeffRef(2,1)= 0; 00272 dh_dx.coeffRef(2,2)= -cam_fx * xji_l_right[0] * pz_inv2; 00273 00274 dh_dx.coeffRef(3,0)= 0; 00275 dh_dx.coeffRef(3,1)= cam_fy * pz_inv; 00276 dh_dx.coeffRef(3,2)= -cam_fy * xji_l_right[1] * pz_inv2; 00277 } 00278 00279 return true; 00280 } 00281 00291 static void inverse_sensor_model( 00292 landmark_traits<LANDMARK_T>::array_landmark_t & out_lm_pos, 00293 const observation_traits<OBS_T>::obs_data_t & obs, 00294 const OBS_T::TObservationParams & params) 00295 { 00296 // > +Z 00297 // / 00298 // / 00299 // +----> +X 00300 // | 00301 // | 00302 // V +Y 00303 // 00304 const double fxl = params.camera_calib.leftCamera.fx(); 00305 const double fyl = params.camera_calib.leftCamera.fy(); 00306 const double cxl = params.camera_calib.leftCamera.cx(); 00307 const double cyl = params.camera_calib.leftCamera.cy(); 00308 const double disparity = std::max(0.001f, obs.left_px.x - obs.right_px.x); 00309 const double baseline = params.camera_calib.rightCameraPose.x(); 00310 ASSERT_(baseline!=0) 00311 const double Z = fxl*baseline/disparity; 00312 out_lm_pos[0] = (obs.left_px.x - cxl)*Z/fxl; 00313 out_lm_pos[1] = (obs.left_px.y - cyl)*Z/fyl; 00314 out_lm_pos[2] = Z; 00315 } 00316 00317 }; // end of struct sensor_model<landmarks::Euclidean3D,observations::StereoCamera> 00318 00319 // ------------------------------------------------------------------------------------------------------------- 00320 00322 template <> 00323 struct sensor_model<landmarks::Euclidean3D,observations::Cartesian_3D> 00324 { 00325 // -------------------------------------------------------------------------------- 00326 // Typedefs for the sake of generality in the signature of methods below: 00327 // *DONT FORGET* to change these when writing new sensor models. 00328 // -------------------------------------------------------------------------------- 00329 typedef observations::Cartesian_3D OBS_T; 00330 typedef landmarks::Euclidean3D LANDMARK_T; 00331 // -------------------------------------------------------------------------------- 00332 00333 static const size_t OBS_DIMS = OBS_T::OBS_DIMS; 00334 static const size_t LM_DIMS = LANDMARK_T::LM_DIMS; 00335 00336 typedef Eigen::Matrix<double,OBS_DIMS,LM_DIMS> TJacobian_dh_dx; 00337 typedef landmark_traits<LANDMARK_T>::array_landmark_t array_landmark_t; 00338 typedef OBS_T::TObservationParams TObservationParams; 00339 00340 00348 template <class POSE_T> 00349 static void observe_error( 00350 observation_traits<OBS_T>::array_obs_t & out_obs_err, 00351 const observation_traits<OBS_T>::array_obs_t & z_obs, 00352 const POSE_T & base_pose_wrt_observer, 00353 const landmark_traits<LANDMARK_T>::array_landmark_t & lm_pos, 00354 const OBS_T::TObservationParams & params) 00355 { 00356 MRPT_UNUSED_PARAM(params); 00357 double x,y,z; // wrt cam (local coords) 00358 base_pose_wrt_observer.composePoint(lm_pos[0],lm_pos[1],lm_pos[2], x,y,z); 00359 00360 observation_traits<OBS_T>::array_obs_t pred_obs; // prediction 00361 // Observations are simply the "local coords": 00362 pred_obs[0] = x; pred_obs[1] = y; pred_obs[2] = z; 00363 out_obs_err = z_obs - pred_obs; 00364 } 00365 00383 static bool eval_jacob_dh_dx( 00384 TJacobian_dh_dx & dh_dx, 00385 const array_landmark_t & xji_l, 00386 const TObservationParams & sensor_params) 00387 { 00388 MRPT_UNUSED_PARAM(xji_l); MRPT_UNUSED_PARAM(sensor_params); 00389 // xji_l[0:2]=[X Y Z] 00390 // This is probably the simplest Jacobian ever: 00391 dh_dx.setIdentity(); 00392 return true; 00393 } 00394 00404 static void inverse_sensor_model( 00405 landmark_traits<LANDMARK_T>::array_landmark_t & out_lm_pos, 00406 const observation_traits<OBS_T>::obs_data_t & obs, 00407 const OBS_T::TObservationParams & params) 00408 { 00409 MRPT_UNUSED_PARAM(params); 00410 out_lm_pos[0] = obs.pt.x; 00411 out_lm_pos[1] = obs.pt.y; 00412 out_lm_pos[2] = obs.pt.z; 00413 } 00414 00415 }; // end of struct sensor_model<landmarks::Euclidean3D,observations::Cartesian_3D> 00416 00417 // ------------------------------------------------------------------------------------------------------------- 00418 00420 template <> 00421 struct sensor_model<landmarks::Euclidean2D,observations::Cartesian_2D> 00422 { 00423 // -------------------------------------------------------------------------------- 00424 // Typedefs for the sake of generality in the signature of methods below: 00425 // *DONT FORGET* to change these when writing new sensor models. 00426 // -------------------------------------------------------------------------------- 00427 typedef observations::Cartesian_2D OBS_T; 00428 typedef landmarks::Euclidean2D LANDMARK_T; 00429 // -------------------------------------------------------------------------------- 00430 00431 static const size_t OBS_DIMS = OBS_T::OBS_DIMS; 00432 static const size_t LM_DIMS = LANDMARK_T::LM_DIMS; 00433 00434 typedef Eigen::Matrix<double,OBS_DIMS,LM_DIMS> TJacobian_dh_dx; 00435 typedef landmark_traits<LANDMARK_T>::array_landmark_t array_landmark_t; 00436 typedef OBS_T::TObservationParams TObservationParams; 00437 00438 00446 template <class POSE_T> 00447 static void observe_error( 00448 observation_traits<OBS_T>::array_obs_t & out_obs_err, 00449 const observation_traits<OBS_T>::array_obs_t & z_obs, 00450 const POSE_T & base_pose_wrt_observer, 00451 const landmark_traits<LANDMARK_T>::array_landmark_t & lm_pos, 00452 const OBS_T::TObservationParams & params) 00453 { 00454 double x,y; // wrt cam (local coords) 00455 base_pose_wrt_observer.composePoint(lm_pos[0],lm_pos[1], x,y); 00456 00457 observation_traits<OBS_T>::array_obs_t pred_obs; // prediction 00458 // Observations are simply the "local coords": 00459 pred_obs[0] = x; pred_obs[1] = y; 00460 out_obs_err = z_obs - pred_obs; 00461 } 00462 00480 static bool eval_jacob_dh_dx( 00481 TJacobian_dh_dx & dh_dx, 00482 const array_landmark_t & xji_l, 00483 const TObservationParams & sensor_params) 00484 { 00485 MRPT_UNUSED_PARAM(xji_l); MRPT_UNUSED_PARAM(sensor_params); 00486 // xji_l[0:1]=[X Y] 00487 // This is probably the simplest Jacobian ever: 00488 dh_dx.setIdentity(); 00489 return true; 00490 } 00491 00501 static void inverse_sensor_model( 00502 landmark_traits<LANDMARK_T>::array_landmark_t & out_lm_pos, 00503 const observation_traits<OBS_T>::obs_data_t & obs, 00504 const OBS_T::TObservationParams & params) 00505 { 00506 MRPT_UNUSED_PARAM(params); 00507 out_lm_pos[0] = obs.pt.x; 00508 out_lm_pos[1] = obs.pt.y; 00509 } 00510 00511 }; // end of struct sensor_model<landmarks::Euclidean2D,observations::Cartesian_2D> 00512 00513 // ------------------------------------------------------------------------------------------------------------- 00514 00516 template <> 00517 struct sensor_model<landmarks::Euclidean3D,observations::RangeBearing_3D> 00518 { 00519 // -------------------------------------------------------------------------------- 00520 // Typedefs for the sake of generality in the signature of methods below: 00521 // *DONT FORGET* to change these when writing new sensor models. 00522 // -------------------------------------------------------------------------------- 00523 typedef observations::RangeBearing_3D OBS_T; 00524 typedef landmarks::Euclidean3D LANDMARK_T; 00525 // -------------------------------------------------------------------------------- 00526 00527 static const size_t OBS_DIMS = OBS_T::OBS_DIMS; 00528 static const size_t LM_DIMS = LANDMARK_T::LM_DIMS; 00529 00530 typedef Eigen::Matrix<double,OBS_DIMS,LM_DIMS> TJacobian_dh_dx; 00531 typedef landmark_traits<LANDMARK_T>::array_landmark_t array_landmark_t; 00532 typedef OBS_T::TObservationParams TObservationParams; 00533 00534 00542 template <class POSE_T> 00543 static void observe_error( 00544 observation_traits<OBS_T>::array_obs_t & out_obs_err, 00545 const observation_traits<OBS_T>::array_obs_t & z_obs, 00546 const POSE_T & base_pose_wrt_observer, 00547 const landmark_traits<LANDMARK_T>::array_landmark_t & lm_pos, 00548 const OBS_T::TObservationParams & params) 00549 { 00550 mrpt::math::TPoint3D l; // wrt sensor (local coords) 00551 base_pose_wrt_observer.composePoint(lm_pos[0],lm_pos[1],lm_pos[2], l.x,l.y,l.z); 00552 00553 static const mrpt::poses::CPose3D origin; 00554 double range,yaw,pitch; 00555 origin.sphericalCoordinates( 00556 l, // In: point 00557 range,yaw,pitch // Out: spherical coords 00558 ); 00559 00560 observation_traits<OBS_T>::array_obs_t pred_obs; // prediction 00561 pred_obs[0] = range; 00562 pred_obs[1] = yaw; 00563 pred_obs[2] = pitch; 00564 out_obs_err = z_obs - pred_obs; 00565 } 00566 00584 static bool eval_jacob_dh_dx( 00585 TJacobian_dh_dx & dh_dx, 00586 const array_landmark_t & xji_l, 00587 const TObservationParams & sensor_params) 00588 { 00589 MRPT_UNUSED_PARAM(sensor_params); 00590 // xji_l[0:2]=[X Y Z] 00591 mrpt::math::CMatrixDouble33 dh_dx_(mrpt::math::UNINITIALIZED_MATRIX); 00592 00593 static const mrpt::poses::CPose3DQuat origin; 00594 double range,yaw,pitch; 00595 origin.sphericalCoordinates( 00596 mrpt::math::TPoint3D(xji_l[0],xji_l[1],xji_l[2]), // In: point 00597 range,yaw,pitch, // Out: spherical coords 00598 &dh_dx_, // dh_dx 00599 NULL // dh_dp (not needed) 00600 ); 00601 00602 dh_dx = dh_dx_; 00603 return true; 00604 } 00605 00615 static void inverse_sensor_model( 00616 landmark_traits<LANDMARK_T>::array_landmark_t & out_lm_pos, 00617 const observation_traits<OBS_T>::obs_data_t & obs, 00618 const OBS_T::TObservationParams & params) 00619 { 00620 MRPT_UNUSED_PARAM(params); 00621 const double chn_y = cos(obs.yaw), shn_y = sin(obs.yaw); 00622 const double chn_p = cos(obs.pitch), shn_p = sin(obs.pitch); 00623 // The new point, relative to the sensor: 00624 out_lm_pos[0] = obs.range * chn_y * chn_p; 00625 out_lm_pos[1] = obs.range * shn_y * chn_p; 00626 out_lm_pos[2] = -obs.range * shn_p; 00627 } 00628 00629 }; // end of struct sensor_model<landmarks::Euclidean3D,observations::RangeBearing_3D> 00630 00631 // ------------------------------------------------------------------------------------------------------------- 00632 00634 template <> 00635 struct sensor_model<landmarks::Euclidean2D,observations::RangeBearing_2D> 00636 { 00637 // -------------------------------------------------------------------------------- 00638 // Typedefs for the sake of generality in the signature of methods below: 00639 // *DONT FORGET* to change these when writing new sensor models. 00640 // -------------------------------------------------------------------------------- 00641 typedef observations::RangeBearing_2D OBS_T; 00642 typedef landmarks::Euclidean2D LANDMARK_T; 00643 // -------------------------------------------------------------------------------- 00644 00645 static const size_t OBS_DIMS = OBS_T::OBS_DIMS; 00646 static const size_t LM_DIMS = LANDMARK_T::LM_DIMS; 00647 00648 typedef Eigen::Matrix<double,OBS_DIMS,LM_DIMS> TJacobian_dh_dx; 00649 typedef landmark_traits<LANDMARK_T>::array_landmark_t array_landmark_t; 00650 typedef OBS_T::TObservationParams TObservationParams; 00651 00652 00660 template <class POSE_T> 00661 static void observe_error( 00662 observation_traits<OBS_T>::array_obs_t & out_obs_err, 00663 const observation_traits<OBS_T>::array_obs_t & z_obs, 00664 const POSE_T & base_pose_wrt_observer, 00665 const landmark_traits<LANDMARK_T>::array_landmark_t & lm_pos, 00666 const OBS_T::TObservationParams & params) 00667 { 00668 MRPT_UNUSED_PARAM(params); 00669 mrpt::math::TPoint2D l; // wrt sensor (local coords) 00670 base_pose_wrt_observer.composePoint(lm_pos[0],lm_pos[1], l.x,l.y); 00671 00672 const double range = hypot(l.x,l.y); 00673 const double yaw = atan2(l.y,l.x); 00674 00675 observation_traits<OBS_T>::array_obs_t pred_obs; // prediction 00676 pred_obs[0] = range; 00677 pred_obs[1] = yaw; 00678 out_obs_err = z_obs - pred_obs; 00679 } 00680 00698 static bool eval_jacob_dh_dx( 00699 TJacobian_dh_dx & dh_dx, 00700 const array_landmark_t & xji_l, 00701 const TObservationParams & sensor_params) 00702 { 00703 MRPT_UNUSED_PARAM(sensor_params); 00704 // xji_l[0:1]=[X Y] 00705 const double r = hypot(xji_l[0], xji_l[1]); 00706 if (r==0) return false; 00707 00708 const double r_inv = 1.0/r; 00709 const double r_inv2 = r_inv*r_inv; 00710 dh_dx(0,0) = xji_l[0] * r_inv; 00711 dh_dx(0,1) = xji_l[1] * r_inv; 00712 dh_dx(1,0) = -xji_l[1] * r_inv2; 00713 dh_dx(1,1) = xji_l[0] * r_inv2; 00714 return true; 00715 } 00716 00726 static void inverse_sensor_model( 00727 landmark_traits<LANDMARK_T>::array_landmark_t & out_lm_pos, 00728 const observation_traits<OBS_T>::obs_data_t & obs, 00729 const OBS_T::TObservationParams & params) 00730 { 00731 MRPT_UNUSED_PARAM(params); 00732 const double chn_y = cos(obs.yaw), shn_y = sin(obs.yaw); 00733 // The new point, relative to the sensor: 00734 out_lm_pos[0] = obs.range * chn_y; 00735 out_lm_pos[1] = obs.range * shn_y; 00736 } 00737 00738 }; // end of sensor_model<landmarks::Euclidean2D,observations::RangeBearing_2D> 00739 00740 // ------------------------------------------------------------------------------------------------------------- 00741 00743 template <> 00744 struct sensor_model<landmarks::RelativePoses2D,observations::RelativePoses_2D> 00745 { 00746 // -------------------------------------------------------------------------------- 00747 // Typedefs for the sake of generality in the signature of methods below: 00748 // *DONT FORGET* to change these when writing new sensor models. 00749 // -------------------------------------------------------------------------------- 00750 typedef observations::RelativePoses_2D OBS_T; 00751 typedef landmarks::RelativePoses2D LANDMARK_T; 00752 // -------------------------------------------------------------------------------- 00753 00754 static const size_t OBS_DIMS = OBS_T::OBS_DIMS; 00755 static const size_t LM_DIMS = LANDMARK_T::LM_DIMS; 00756 00757 typedef Eigen::Matrix<double,OBS_DIMS,LM_DIMS> TJacobian_dh_dx; 00758 typedef landmark_traits<LANDMARK_T>::array_landmark_t array_landmark_t; 00759 typedef OBS_T::TObservationParams TObservationParams; 00760 00761 00769 template <class POSE_T> 00770 static void observe_error( 00771 observation_traits<OBS_T>::array_obs_t & out_obs_err, 00772 const observation_traits<OBS_T>::array_obs_t & z_obs, 00773 const POSE_T & base_pose_wrt_observer, 00774 const landmark_traits<LANDMARK_T>::array_landmark_t & lm_pos, 00775 const OBS_T::TObservationParams & params) 00776 { 00777 MRPT_UNUSED_PARAM(lm_pos); MRPT_UNUSED_PARAM(params); 00778 // Relative pose observation: 00779 // OUT_OBS_ERR = - pseudo-log( PREDICTED_REL_POSE \ominus Z_OBS ) 00780 const POSE_T h = POSE_T(z_obs[0],z_obs[1],z_obs[2]) - base_pose_wrt_observer; 00781 00782 out_obs_err[0] = h.x(); 00783 out_obs_err[1] = h.y(); 00784 out_obs_err[2] = h.phi(); 00785 } 00786 00804 static bool eval_jacob_dh_dx( 00805 TJacobian_dh_dx & dh_dx, 00806 const array_landmark_t & xji_l, 00807 const TObservationParams & sensor_params) 00808 { 00809 MRPT_UNUSED_PARAM(xji_l); MRPT_UNUSED_PARAM(sensor_params); 00810 // h(z_obs \ominus p) = pseudo-log(z_obs \ominus p) 00811 // with p: relative pose in SE(2) 00812 dh_dx.setIdentity(); 00813 return true; 00814 } 00815 00825 static void inverse_sensor_model( 00826 landmark_traits<LANDMARK_T>::array_landmark_t & out_lm_pos, 00827 const observation_traits<OBS_T>::obs_data_t & obs, 00828 const OBS_T::TObservationParams & params) 00829 { 00830 MRPT_UNUSED_PARAM(params); 00831 out_lm_pos[0] = obs.x; 00832 out_lm_pos[1] = obs.y; 00833 out_lm_pos[2] = obs.yaw; 00834 } 00835 00836 }; // end of sensor_model<landmarks::RelativePoses2D,observations::RelativePoses_2D> 00837 00838 // ------------------------------------------------------------------------------------------------------------- 00839 00841 template <> 00842 struct sensor_model<landmarks::RelativePoses3D,observations::RelativePoses_3D> 00843 { 00844 // -------------------------------------------------------------------------------- 00845 // Typedefs for the sake of generality in the signature of methods below: 00846 // *DONT FORGET* to change these when writing new sensor models. 00847 // -------------------------------------------------------------------------------- 00848 typedef observations::RelativePoses_3D OBS_T; 00849 typedef landmarks::RelativePoses3D LANDMARK_T; 00850 // -------------------------------------------------------------------------------- 00851 00852 static const size_t OBS_DIMS = OBS_T::OBS_DIMS; 00853 static const size_t LM_DIMS = LANDMARK_T::LM_DIMS; 00854 00855 typedef Eigen::Matrix<double,OBS_DIMS,LM_DIMS> TJacobian_dh_dx; 00856 typedef landmark_traits<LANDMARK_T>::array_landmark_t array_landmark_t; 00857 typedef OBS_T::TObservationParams TObservationParams; 00858 00859 00867 template <class POSE_T> 00868 static void observe_error( 00869 observation_traits<OBS_T>::array_obs_t & out_obs_err, 00870 const observation_traits<OBS_T>::array_obs_t & z_obs, 00871 const POSE_T & base_pose_wrt_observer, 00872 const landmark_traits<LANDMARK_T>::array_landmark_t & lm_pos, 00873 const OBS_T::TObservationParams & params) 00874 { 00875 // Relative pose observation: 00876 // OUT_OBS_ERR = - pseudo-log( PREDICTED_REL_POSE \ominus Z_OBS ) 00877 const POSE_T h = POSE_T(z_obs[0],z_obs[1],z_obs[2],z_obs[3],z_obs[4],z_obs[5]) - base_pose_wrt_observer; 00878 00879 mrpt::poses::SE_traits<3>::pseudo_ln(h,out_obs_err); 00880 } 00881 00899 static bool eval_jacob_dh_dx( 00900 TJacobian_dh_dx & dh_dx, 00901 const array_landmark_t & xji_l, 00902 const TObservationParams & sensor_params) 00903 { 00904 MRPT_UNUSED_PARAM(xji_l); MRPT_UNUSED_PARAM(sensor_params); 00905 // h(z_obs \ominus p) = xji_l (the relative pose in SE(3)) 00906 dh_dx.setIdentity(); 00907 return true; 00908 } 00909 00919 static void inverse_sensor_model( 00920 landmark_traits<LANDMARK_T>::array_landmark_t & out_lm_pos, 00921 const observation_traits<OBS_T>::obs_data_t & obs, 00922 const OBS_T::TObservationParams & params) 00923 { 00924 MRPT_UNUSED_PARAM(params); 00925 out_lm_pos[0] = obs.x; out_lm_pos[1] = obs.y; out_lm_pos[2] = obs.z; 00926 out_lm_pos[3] = obs.yaw; out_lm_pos[4] = obs.pitch; out_lm_pos[5] = obs.roll; 00927 } 00928 00929 }; // end of struct sensor_model<landmarks::RelativePoses3D,observations::RelativePoses_3D> 00930 00931 // ------------------------------------------------------------------------------------------------------------- 00932 00935 } // end NS
1.7.6.1
