Optimized StereoCamera.project derivative

.cproject

@@ -1507,10 +1507,10 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="tests/timeCalibratedCamera.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="tests/timeStereoCamera.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
-				<buildTarget>tests/timeCalibratedCamera.run</buildTarget>
+				<buildTarget>tests/timeStereoCamera.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>

gtsam/geometry/Makefile.am

@@ -33,7 +33,7 @@ sources += projectiveGeometry.cpp tensorInterface.cpp
 check_PROGRAMS += tests/testTensors tests/testHomography2 tests/testFundamental
 
 # Timing tests
-noinst_PROGRAMS = tests/timeRot3 tests/timePose3 tests/timeCalibratedCamera
+noinst_PROGRAMS = tests/timeRot3 tests/timePose3 tests/timeCalibratedCamera tests/timeStereoCamera
 
 # Rot3M and Rot3Q both use Rot3.h, they do not have individual header files
 allsources = $(sources)

gtsam/geometry/SimpleCamera.cpp

@@ -49,17 +49,13 @@ namespace gtsam {
 		Point2 intrinsic = calibrated_.project(point, H1, H2);
 		if (!H1 && !H2)
 			return K_.uncalibrate(intrinsic);
-
+		else {
-		Matrix D_projection_intrinsic;
+			Matrix D_projection_intrinsic;
-		Point2 projection = K_.uncalibrate(intrinsic, boost::none, D_projection_intrinsic);
+			Point2 projection = K_.uncalibrate(intrinsic, boost::none, D_projection_intrinsic);
-
+			if (H1) *H1 = D_projection_intrinsic * (*H1);
-		if (H1) {
+			if (H2) *H2 = D_projection_intrinsic * (*H2);
-			*H1 = D_projection_intrinsic * (*H1);
+			return projection;
 		}
-		if (H2) {
-			*H2 = D_projection_intrinsic * (*H2);
-		}
-		return projection;
 	}
 
 	Point3 SimpleCamera::backproject(const Point2& projection, const double scale) const {

gtsam/geometry/StereoCamera.cpp

@@ -1,18 +1,18 @@
 /* ----------------------------------------------------------------------------
 
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+*GTSAM Copyright 2010, Georgia Tech Research Corporation,
- * Atlanta, Georgia 30332-0415
+*Atlanta, Georgia 30332-0415
- * All Rights Reserved
+*All Rights Reserved
- * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+*Authors: Frank Dellaert, et al. (see THANKS for the full author list)
 
- * See LICENSE for the license information
+*See LICENSE for the license information
 
- * -------------------------------------------------------------------------- */
+*-------------------------------------------------------------------------- */
 
 /**
- * @file    StereoCamera.h
+*@file    StereoCamera.h
- * @brief   A Stereo Camera based on two Simple Cameras
+*@brief   A Stereo Camera based on two Simple Cameras
- * @author  Chris Beall
+*@author  Chris Beall
  */
 
 #include <gtsam/geometry/StereoCamera.h>
@@ -22,45 +22,77 @@ using namespace gtsam;
 
 namespace gtsam {
 
-/* ************************************************************************* */
+	/* ************************************************************************* */
-StereoCamera::StereoCamera(const Pose3& leftCamPose, const Cal3_S2Stereo::shared_ptr K) :
+	StereoCamera::StereoCamera(const Pose3& leftCamPose,
-	leftCamPose_(leftCamPose), K_(K) {
+			const Cal3_S2Stereo::shared_ptr K) :
-}
+			leftCamPose_(leftCamPose), K_(K) {
-
-/* ************************************************************************* */
-StereoPoint2 StereoCamera::project(const Point3& point,
-		boost::optional<Matrix&> H1,
-		boost::optional<Matrix&> H2) const {
-
-	Point3 cameraPoint = leftCamPose_.transform_to(point, H1, H2);
-
-	if(H1 || H2) {
-		Matrix D_project_point = Dproject_to_stereo_camera1(cameraPoint); // 3x3 Jacobian
-
-	  if (H1)
-	  	*H1 =  D_project_point * *H1;
-	  if (H2)
-	  	*H2 =  D_project_point * *H2;
 	}
 
-	const Cal3_S2Stereo& K = *K_;
+	/* ************************************************************************* */
-	double f_x = K.fx(), f_y = K.fy(), b=K.baseline();
+	StereoPoint2 StereoCamera::project(const Point3& point,
-	double d = 1.0 / cameraPoint.z();
+			boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
-	double uL = K.px() + d * f_x * cameraPoint.x();
-	double uR = K.px() + d * f_x * (cameraPoint.x() - b);
-	double v = K.py() + d * f_y * cameraPoint.y();
-	return StereoPoint2(uL, uR, v);
-}
 
-/* ************************************************************************* */
+#ifdef STEREOCAMERA_CHAIN_RULE
-Matrix StereoCamera::Dproject_to_stereo_camera1(const Point3& P) const {
+		const Point3 q = leftCamPose_.transform_to(point, H1, H2);
-	double d = 1.0 / P.z(), d2 = d * d;
+#else
-	const Cal3_S2Stereo& K = *K_;
+		// omit derivatives
-	double f_x = K.fx(), f_y = K.fy(), b=K.baseline();
+		const Point3 q = leftCamPose_.transform_to(point);
-	return Matrix_(3, 3,
+#endif
-			f_x*d, 0.0, -f_x *P.x() * d2,
+
-			f_x*d, 0.0, -f_x *(P.x() - b) * d2,
+		// get calibration
-			0.0, f_y*d, -f_y*P.y() * d2);
+		const Cal3_S2Stereo& K = *K_;
-}
+		const double fx = K.fx(), fy = K.fy(), b = K.baseline();
+
+		// calculate scaled but not translated image coordinates
+		const double d = 1.0 / q.z();
+		const double x = q.x(), y = q.y();
+		const double uL = d*fx*x;
+		const double uR = d*fx*(x - b);
+		const double v  = d*fy*y;
+
+		// check if derivatives need to be computed
+		if (H1 || H2) {
+#ifdef STEREOCAMERA_CHAIN_RULE
+			// just implement chain rule
+			Matrix D_project_point = Dproject_to_stereo_camera1(q); // 3x3 Jacobian
+			if (H1) *H1 = D_project_point*(*H1);
+			if (H2) *H2 = D_project_point*(*H2);
+#else
+			// optimized version, see StereoCamera.nb
+			if (H1) {
+				const double z = q.z(), fxz = fx*z, v1 = v/fy, v2 = fx*v1;
+				const double dfx = d*fx, dx = d*x;
+				*H1 = Matrix_(3, 6,
+							  uL*v1, -d*(uL*x + fxz),     v2, -dfx,   0.0, d*uL,
+							  uR*v1, -d*(uR*x + fxz),     v2, -dfx,   0.0, d*uR,
+						fy + v*v1,           -dx*v, -dx*fy,  0.0, -d*fy, d*v
+					);
+			}
+			if (H2) {
+				const Matrix R(leftCamPose_.rotation().matrix());
+				*H2 = d * Matrix_(3, 3,
+						 fx*R(0, 0) - R(0, 2)*uL, fx*R(1, 0) - R(1, 2)*uL, fx*R(2, 0) - R(2, 2)*uL,
+						 fx*R(0, 0) - R(0, 2)*uR, fx*R(1, 0) - R(1, 2)*uR, fx*R(2, 0) - R(2, 2)*uR,
+						 fy*R(0, 1) - R(0, 2)*v , fy*R(1, 1) - R(1, 2)*v , fy*R(2, 1) - R(2, 2)*v
+				 );
+			}
+#endif
+		}
+
+		// finally translate
+		return StereoPoint2(K.px() + uL, K.px() + uR, K.py() + v);
+	}
+
+	/* ************************************************************************* */
+	Matrix StereoCamera::Dproject_to_stereo_camera1(const Point3& P) const {
+		double d = 1.0 / P.z(), d2 = d*d;
+		const Cal3_S2Stereo& K = *K_;
+		double f_x = K.fx(), f_y = K.fy(), b = K.baseline();
+		return Matrix_(3, 3,
+				 f_x*d,   0.0, -d2*f_x* P.x(),
+				 f_x*d,   0.0, -d2*f_x*(P.x() - b),
+				   0.0, f_y*d, -d2*f_y* P.y()
+		);
+	}
 
 }

gtsam/geometry/tests/timeStereoCamera.cpp

@@ -0,0 +1,51 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    timeStereoCamera.cpp
+ * @brief   time StereoCamera derivatives
+ * @author  Frank Dellaert
+ */
+
+#include <time.h>
+#include <iostream>
+
+#include <gtsam/geometry/StereoCamera.h>
+
+using namespace std;
+using namespace gtsam;
+
+int main()
+{
+	int n = 100000;
+
+	const Pose3 pose1(Matrix_(3,3,
+			1., 0., 0.,
+			0.,-1., 0.,
+			0., 0.,-1.
+	),
+	Point3(0,0,0.5));
+
+	const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5));
+	const StereoCamera camera(pose1, K);
+	const Point3 point1(-0.08,-0.08, 0.0);
+
+	Matrix computed1, computed2;
+	long timeLog = clock();
+	for(int i = 0; i < n; i++)
+		camera.project(point1, computed1, computed2);
+	long timeLog2 = clock();
+	double seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC;
+	cout << ((double)n/seconds) << " calls/second" << endl;
+	cout << ((double)seconds*1000000/n) << " musecs/call" << endl;
+
+	return 0;
+}