moved StereoFactor typedef into visualSLAM namespace. StereoCamera improvements

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<Matrix&> Dproject_stereo_pose,
+		boost::optional<Matrix&> H1,
-		boost::optional<Matrix&> Dproject_stereo_point) const {
+		boost::optional<Matrix&> 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();

gtsam/geometry/StereoCamera.h

@@ -17,10 +17,10 @@
 
 #pragma once
 
+#include <gtsam/base/Lie.h>
 #include "boost/tuple/tuple.hpp"
 #include <gtsam/geometry/Cal3_S2Stereo.h>
 #include <gtsam/geometry/Pose3.h>
-#include <gtsam/base/Lie.h>
 #include <gtsam/geometry/StereoPoint2.h>
 
 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<Matrix&> Dproject_stereo_pose = boost::none,
+				boost::optional<Matrix&> H1 = boost::none,
-				boost::optional<Matrix&> Dproject_stereo_point = boost::none) const;
+				boost::optional<Matrix&> H2 = boost::none) const;
+
+		/*
+		 * to accomodate tsam's assumption that K is estimated, too
+		 */
+		StereoPoint2 project(const Point3& point,
+					boost::optional<Matrix&> H1,
+					boost::optional<Matrix&> H1_k,
+					boost::optional<Matrix&> 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_));
+			const Vector v1(leftCamPose_.logmap(camera.leftCamPose_));
-				return v1;
+			return v1;
 		}
 
-		bool equals (const StereoCamera &camera, double tol = 1e-9) const {
+		bool equals(const StereoCamera &camera, double tol = 1e-9) const {
-			return leftCamPose_.equals(camera.leftCamPose_, tol) &&
+			return leftCamPose_.equals(camera.leftCamPose_, tol) && K_.equals(
-				   K_.equals(camera.K_, tol);
+					camera.K_, tol);
 		}
 
 		void print(const std::string& s = "") const {
-			leftCamPose_.print(s + ".camera.") ;
+			leftCamPose_.print(s + ".camera.");
-			K_.print(s + ".calibration.") ;
+			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<class Archive>
+		void serialize(Archive & ar, const unsigned int version) {
+			ar & BOOST_SERIALIZATION_NVP(leftCamPose_);
+			ar & BOOST_SERIALIZATION_NVP(K_);
+		}
+
 	};
 
 }

gtsam/slam/StereoFactor.h

@@ -18,15 +18,12 @@
 
 #pragma once
 
-#include <gtsam/slam/visualSLAM.h>
 #include <gtsam/geometry/Cal3_S2Stereo.h>
 #include <gtsam/geometry/StereoCamera.h>
 
 namespace gtsam {
 
-using namespace gtsam;
+template<class VALUES, class KEY1, class KEY2>
-
-template<class VALUES=visualSLAM::Values, class KEY1=visualSLAM::PoseKey, class KEY2=visualSLAM::PointKey>
 class GenericStereoFactor: public NonlinearFactor2<VALUES, KEY1, KEY2> {
 private:
 
@@ -114,7 +111,5 @@ private:
 	}
 };
 
-// Typedef for general use
-typedef GenericStereoFactor<visualSLAM::Values, visualSLAM::PoseKey, visualSLAM::PointKey> StereoFactor;
 
 }

gtsam/slam/visualSLAM.h

@@ -27,6 +27,7 @@
 #include <gtsam/nonlinear/NonlinearEquality.h>
 #include <gtsam/nonlinear/NonlinearOptimizer.h>
 #include <gtsam/slam/PriorFactor.h>
+#include <gtsam/slam/StereoFactor.h>
 
 namespace gtsam {
 
@@ -126,6 +127,7 @@ namespace gtsam {
   
   // Typedef for general use
   typedef GenericProjectionFactor<Values, PointKey, PoseKey> ProjectionFactor;
+  typedef GenericStereoFactor<Values, PoseKey, PointKey> StereoFactor;
 
   /**
    * Non-linear factor graph for vanilla visual SLAM