From 9a9b642d5dc2c9af2845e156075f76e3754f7d98 Mon Sep 17 00:00:00 2001 From: Chris Beall Date: Fri, 11 Mar 2011 21:56:14 +0000 Subject: [PATCH] moved StereoFactor typedef into visualSLAM namespace. StereoCamera improvements --- gtsam/geometry/StereoCamera.cpp | 12 ++++---- gtsam/geometry/StereoCamera.h | 53 ++++++++++++++++++++++++++------- gtsam/slam/StereoFactor.h | 7 +---- gtsam/slam/visualSLAM.h | 2 ++ 4 files changed, 51 insertions(+), 23 deletions(-) diff --git a/gtsam/geometry/StereoCamera.cpp b/gtsam/geometry/StereoCamera.cpp index 0ac08cee2..20c2fcefc 100644 --- a/gtsam/geometry/StereoCamera.cpp +++ b/gtsam/geometry/StereoCamera.cpp @@ -29,12 +29,12 @@ StereoCamera::StereoCamera(const Pose3& leftCamPose, const Cal3_S2Stereo& K) : /* ************************************************************************* */ StereoPoint2 StereoCamera::project(const Point3& point, - boost::optional Dproject_stereo_pose, - boost::optional Dproject_stereo_point) const { + boost::optional H1, + boost::optional H2) const { Point3 cameraPoint = leftCamPose_.transform_to(point); - if (Dproject_stereo_pose) { + if (H1) { Matrix D_cameraPoint_pose; Point3 cameraPoint = pose().transform_to(point, D_cameraPoint_pose, boost::none); @@ -43,10 +43,10 @@ StereoPoint2 StereoCamera::project(const Point3& point, Matrix D_intrinsic_pose = D_intrinsic_cameraPoint * D_cameraPoint_pose; Matrix D_projection_intrinsic = Duncalibrate2(calibration()); // 3x3 - *Dproject_stereo_pose = D_projection_intrinsic * D_intrinsic_pose; + *H1 = D_projection_intrinsic * D_intrinsic_pose; } - if (Dproject_stereo_point) { + if (H2) { Matrix D_cameraPoint_point; Point3 cameraPoint = pose().transform_to(point, boost::none, D_cameraPoint_point); @@ -55,7 +55,7 @@ StereoPoint2 StereoCamera::project(const Point3& point, Matrix D_intrinsic_point = D_intrinsic_cameraPoint * D_cameraPoint_point; Matrix D_projection_intrinsic = Duncalibrate2(calibration()); // 3x3 - *Dproject_stereo_point = D_projection_intrinsic * D_intrinsic_point; + *H2 = D_projection_intrinsic * D_intrinsic_point; } double d = 1.0 / cameraPoint.z(); diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index 9aebb5ebd..84ce0cfb5 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/gtsam/geometry/StereoCamera.h @@ -17,10 +17,10 @@ #pragma once +#include #include "boost/tuple/tuple.hpp" #include #include -#include #include namespace gtsam { @@ -52,9 +52,33 @@ namespace gtsam { return K_.baseline(); } + + /* + * project 3D point and compute optional derivatives + */ StereoPoint2 project(const Point3& point, - boost::optional Dproject_stereo_pose = boost::none, - boost::optional Dproject_stereo_point = boost::none) const; + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const; + + /* + * to accomodate tsam's assumption that K is estimated, too + */ + StereoPoint2 project(const Point3& point, + boost::optional H1, + boost::optional H1_k, + boost::optional H2) const { + return project(point, H1, H2); + } + + /* + * backproject using left camera calibration, up to scale only + * i.e. does not rely on baseline + */ + Point3 backproject(const Point2& projection, const double scale) const { + Point2 intrinsic = K_.calibrate(projection); + Point3 cameraPoint = Point3(intrinsic.x() * scale, intrinsic.y() * scale, scale);; + return pose().transform_from(cameraPoint); + } /** Dimensionality of the tangent space */ inline size_t dim() const { @@ -63,22 +87,22 @@ namespace gtsam { /** Exponential map around p0 */ inline StereoCamera expmap(const Vector& d) const { - return StereoCamera(pose().expmap(d),calibration()); + return StereoCamera(pose().expmap(d), calibration()); } Vector logmap(const StereoCamera &camera) const { - const Vector v1(leftCamPose_.logmap(camera.leftCamPose_)); - return v1; + const Vector v1(leftCamPose_.logmap(camera.leftCamPose_)); + return v1; } - bool equals (const StereoCamera &camera, double tol = 1e-9) const { - return leftCamPose_.equals(camera.leftCamPose_, tol) && - K_.equals(camera.K_, tol); + bool equals(const StereoCamera &camera, double tol = 1e-9) const { + return leftCamPose_.equals(camera.leftCamPose_, tol) && K_.equals( + camera.K_, tol); } void print(const std::string& s = "") const { - leftCamPose_.print(s + ".camera.") ; - K_.print(s + ".calibration.") ; + leftCamPose_.print(s + ".camera."); + K_.print(s + ".calibration."); } private: @@ -86,6 +110,13 @@ namespace gtsam { Matrix Dproject_to_stereo_camera1(const Point3& P) const; static Matrix Duncalibrate2(const Cal3_S2& K); + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(leftCamPose_); + ar & BOOST_SERIALIZATION_NVP(K_); + } + }; } diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h index f7e8cfb41..8a21617f9 100644 --- a/gtsam/slam/StereoFactor.h +++ b/gtsam/slam/StereoFactor.h @@ -18,15 +18,12 @@ #pragma once -#include #include #include namespace gtsam { -using namespace gtsam; - -template +template class GenericStereoFactor: public NonlinearFactor2 { private: @@ -114,7 +111,5 @@ private: } }; -// Typedef for general use -typedef GenericStereoFactor StereoFactor; } diff --git a/gtsam/slam/visualSLAM.h b/gtsam/slam/visualSLAM.h index cdbe1b651..e8cdd929e 100644 --- a/gtsam/slam/visualSLAM.h +++ b/gtsam/slam/visualSLAM.h @@ -27,6 +27,7 @@ #include #include #include +#include namespace gtsam { @@ -126,6 +127,7 @@ namespace gtsam { // Typedef for general use typedef GenericProjectionFactor ProjectionFactor; + typedef GenericStereoFactor StereoFactor; /** * Non-linear factor graph for vanilla visual SLAM