StereoCamera cleanup and improvements

gtsam/geometry/Cal3_S2Stereo.h

@@ -30,6 +30,9 @@ namespace gtsam {
 		double b_;
 
 	public:
+
+		typedef boost::shared_ptr<Cal3_S2Stereo> shared_ptr;  ///< shared pointer to stereo calibration object
+
 		/**
 		 * default calibration leaves coordinates unchanged
 		 */

gtsam/geometry/StereoCamera.cpp

@@ -23,7 +23,7 @@ using namespace gtsam;
 namespace gtsam {
 
 /* ************************************************************************* */
-StereoCamera::StereoCamera(const Pose3& leftCamPose, const Cal3_S2Stereo& K) :
+StereoCamera::StereoCamera(const Pose3& leftCamPose, const Cal3_S2Stereo::shared_ptr K) :
 	leftCamPose_(leftCamPose), K_(K) {
 }
 
@@ -32,49 +32,40 @@ StereoPoint2 StereoCamera::project(const Point3& point,
 		boost::optional<Matrix&> H1,
 		boost::optional<Matrix&> H2) const {
 
-	Point3 cameraPoint = leftCamPose_.transform_to(point);
+	Point3 cameraPoint = leftCamPose_.transform_to(point, H1, H2);
-
-	if (H1) {
-		Matrix D_cameraPoint_pose;
-		Point3 cameraPoint = pose().transform_to(point, D_cameraPoint_pose,
-				boost::none);
 
+	if(H1 || H2) {
 		Matrix D_intrinsic_cameraPoint = Dproject_to_stereo_camera1(cameraPoint); // 3x3 Jacobian
-		Matrix D_intrinsic_pose = D_intrinsic_cameraPoint * D_cameraPoint_pose;
+		Matrix D_projection_intrinsic = Duncalibrate2(K_); // 3x3
 
-		Matrix D_projection_intrinsic = Duncalibrate2(calibration()); // 3x3
+	  if (H1) {
-		*H1 = D_projection_intrinsic * D_intrinsic_pose;
+	  	//Matrix D_intrinsic_pose = D_intrinsic_cameraPoint * *H1;
-	}
+	  	*H1 = D_projection_intrinsic * D_intrinsic_cameraPoint * *H1;
+	  }
 
-	if (H2) {
+	  if (H2) {
-		Matrix D_cameraPoint_point;
+	  	//Matrix D_intrinsic_point = D_intrinsic_cameraPoint * *H2;
-		Point3 cameraPoint = pose().transform_to(point, boost::none,
+	  	*H2 = D_projection_intrinsic * D_intrinsic_cameraPoint * *H2;
-				D_cameraPoint_point);
+	  }
-
-		Matrix D_intrinsic_cameraPoint = Dproject_to_stereo_camera1(cameraPoint); // 3x3 Jacobian
-		Matrix D_intrinsic_point = D_intrinsic_cameraPoint * D_cameraPoint_point;
-
-		Matrix D_projection_intrinsic = Duncalibrate2(calibration()); // 3x3
-		*H2 = D_projection_intrinsic * D_intrinsic_point;
 	}
 
 	double d = 1.0 / cameraPoint.z();
-	double uL = K_.px() + d * K_.fx() * cameraPoint.x();
+	double uL = K_->px() + d * K_->fx() * cameraPoint.x();
-	double uR = K_.px() + d * K_.fx() * (cameraPoint.x() - K_.baseline());
+	double uR = K_->px() + d * K_->fx() * (cameraPoint.x() - K_->baseline());
-	double v = K_.py() + d * K_.fy() * cameraPoint.y();
+	double v = K_->py() + d * K_->fy() * cameraPoint.y();
 	return StereoPoint2(uL, uR, v);
 }
 
 /* ************************************************************************* */
 Matrix StereoCamera::Dproject_to_stereo_camera1(const Point3& P) const {
 	double d = 1.0 / P.z(), d2 = d * d;
-	return Matrix_(3, 3, d, 0.0, -P.x() * d2, d, 0.0, -(P.x() - K_.baseline()) * d2,
+	return Matrix_(3, 3, d, 0.0, -P.x() * d2, d, 0.0, -(P.x() - K_->baseline()) * d2,
 			0.0, d, -P.y() * d2);
 }
 
 /* ************************************************************************* */
-Matrix StereoCamera::Duncalibrate2(const Cal3_S2& K) {
+Matrix StereoCamera::Duncalibrate2(const Cal3_S2Stereo::shared_ptr K) {
-	Vector calibration = K.vector();
+	Vector calibration = K->vector();
 	Vector calibration2(3);
 	calibration2(0) = calibration(0);
 	calibration2(1) = calibration(0);

gtsam/geometry/StereoCamera.h

@@ -32,15 +32,15 @@ namespace gtsam {
 
 	private:
 		Pose3 leftCamPose_;
-		Cal3_S2Stereo K_;
+		Cal3_S2Stereo::shared_ptr K_;
 
 	public:
 		StereoCamera() {
 		}
 
-		StereoCamera(const Pose3& leftCamPose, const Cal3_S2Stereo& K);
+		StereoCamera(const Pose3& leftCamPose, const Cal3_S2Stereo::shared_ptr K);
 
-		const Cal3_S2Stereo& calibration() const {
+		const Cal3_S2Stereo::shared_ptr calibration() const {
 			return K_;
 		}
 
@@ -49,7 +49,7 @@ namespace gtsam {
 		}
 
 		const double baseline() const {
-			return K_.baseline();
+			return K_->baseline();
 		}
 
 
@@ -75,16 +75,16 @@ namespace gtsam {
 		 * i.e. does not rely on baseline
 		 */
 		Point3 backproject(const Point2& projection, const double scale) const {
-			Point2 intrinsic = K_.calibrate(projection);
+			Point2 intrinsic = K_->calibrate(projection);
 			Point3 cameraPoint = Point3(intrinsic.x() * scale, intrinsic.y() * scale, scale);;
 			return pose().transform_from(cameraPoint);
 		}
 
 		Point3 backproject(const StereoPoint2& z) const {
 			Vector measured = z.vector();
-			double Z = K_.baseline()*K_.fx()/(measured[0]-measured[1]);
+			double Z = K_->baseline()*K_->fx()/(measured[0]-measured[1]);
-			double X = Z *(measured[0]- K_.px()) / K_.fx();
+			double X = Z *(measured[0]- K_->px()) / K_->fx();
-			double Y = Z *(measured[2]- K_.py()) / K_.fy();
+			double Y = Z *(measured[2]- K_->py()) / K_->fy();
 			Point3 world_point = leftCamPose_.transform_from(Point3(X, Y, Z));
 			return world_point;
 		}
@@ -101,7 +101,7 @@ namespace gtsam {
 
 		/** Exponential map around p0 */
 		inline StereoCamera expmap(const Vector& d) const {
-			return StereoCamera(pose().expmap(d), calibration());
+			return StereoCamera(pose().expmap(d), K_);
 		}
 
 		Vector logmap(const StereoCamera &camera) const {
@@ -110,8 +110,8 @@ namespace gtsam {
 		}
 
 		bool equals(const StereoCamera &camera, double tol = 1e-9) const {
-			return leftCamPose_.equals(camera.leftCamPose_, tol) && K_.equals(
+			return leftCamPose_.equals(camera.leftCamPose_, tol) && K_->equals(
-					camera.K_, tol);
+					*camera.K_, tol);
 		}
 
 		Pose3 between(const StereoCamera &camera,
@@ -122,13 +122,13 @@ namespace gtsam {
 
 		void print(const std::string& s = "") const {
 			leftCamPose_.print(s + ".camera.");
-			K_.print(s + ".calibration.");
+			K_->print(s + ".calibration.");
 		}
 
 	private:
 		/** utility functions */
 		Matrix Dproject_to_stereo_camera1(const Point3& P) const;
-		static Matrix Duncalibrate2(const Cal3_S2& K);
+		static Matrix Duncalibrate2(const Cal3_S2Stereo::shared_ptr K);
 
 		friend class boost::serialization::access;
 		template<class Archive>

gtsam/geometry/tests/testStereoCamera.cpp

@@ -36,7 +36,7 @@ TEST( StereoCamera, project)
 {
 	// create a Stereo camera at the origin with focal length 1500, baseline 0.5m
 	// and principal point 320, 240 (for a hypothetical 640x480 sensor)
-	Cal3_S2Stereo K(1500, 1500, 0, 320, 240, 0.5);
+	Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5));
 	StereoCamera stereoCam(Pose3(), K);
 
 	// point X Y Z in meters
@@ -77,7 +77,7 @@ Pose3 camera1(Matrix_(3,3,
 		       ),
 	      Point3(0,0,6.25));
 
-Cal3_S2Stereo K(1500, 1500, 0, 320, 240, 0.5);
+Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5));
 StereoCamera stereoCam(Pose3(), K);
 
 // point X Y Z in meters
@@ -115,7 +115,7 @@ TEST( StereoCamera, backproject2)
 	       -0.804435942,	-0.592650676,	-0.0405925523,
 	       0.0732045588,	-0.0310882277,	-0.996832359);
   Point3 t(53.5239823, 23.7866016, -4.42379876);
-  Cal3_S2Stereo K(1733.75, 1733.75, 0, 689.645, 508.835, 0.0699612);
+  Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1733.75, 1733.75, 0, 689.645, 508.835, 0.0699612));
   StereoCamera camera(Pose3(R,t), K);
   StereoPoint2 z(184.812, 129.068, 714.768);
 

gtsam/slam/StereoFactor.h

@@ -86,7 +86,7 @@ public:
 	/** h(x)-z */
 	Vector evaluateError(const Pose3& pose, const Point3& point,
 			boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
-		StereoCamera stereoCam(pose, *K_);
+		StereoCamera stereoCam(pose, K_);
 		return (stereoCam.project(point, H1, H2) - z_).vector();
 	}
 

gtsam/slam/tests/testStereoFactor.cpp

@@ -40,7 +40,7 @@ Pose3 camera1(Matrix_(3,3,
 		       ),
 	      Point3(0,0,6.25));
 
-Cal3_S2Stereo K(1500, 1500, 0, 320, 240, 0.5);
+Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5));
 StereoCamera stereoCam(Pose3(), K);
 
 // point X Y Z in meters