diff --git a/.cproject b/.cproject
index 26815d331..4526f371e 100644
--- a/.cproject
+++ b/.cproject
@@ -1507,10 +1507,10 @@
true
true
-
+
make
-j2
- tests/timeCalibratedCamera.run
+ tests/timeStereoCamera.run
true
true
true
diff --git a/gtsam/geometry/Makefile.am b/gtsam/geometry/Makefile.am
index 34b307717..c0cfb66fc 100644
--- a/gtsam/geometry/Makefile.am
+++ b/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)
diff --git a/gtsam/geometry/SimpleCamera.cpp b/gtsam/geometry/SimpleCamera.cpp
index c87f72e3e..5c9859e60 100644
--- a/gtsam/geometry/SimpleCamera.cpp
+++ b/gtsam/geometry/SimpleCamera.cpp
@@ -49,17 +49,13 @@ namespace gtsam {
Point2 intrinsic = calibrated_.project(point, H1, H2);
if (!H1 && !H2)
return K_.uncalibrate(intrinsic);
-
- Matrix D_projection_intrinsic;
- Point2 projection = K_.uncalibrate(intrinsic, boost::none, D_projection_intrinsic);
-
- if (H1) {
- *H1 = D_projection_intrinsic * (*H1);
+ else {
+ Matrix D_projection_intrinsic;
+ Point2 projection = K_.uncalibrate(intrinsic, boost::none, D_projection_intrinsic);
+ if (H1) *H1 = D_projection_intrinsic * (*H1);
+ if (H2) *H2 = D_projection_intrinsic * (*H2);
+ return projection;
}
- if (H2) {
- *H2 = D_projection_intrinsic * (*H2);
- }
- return projection;
}
Point3 SimpleCamera::backproject(const Point2& projection, const double scale) const {
diff --git a/gtsam/geometry/StereoCamera.cpp b/gtsam/geometry/StereoCamera.cpp
index e652930bf..b1e5f0db4 100644
--- a/gtsam/geometry/StereoCamera.cpp
+++ b/gtsam/geometry/StereoCamera.cpp
@@ -1,18 +1,18 @@
/* ----------------------------------------------------------------------------
- * 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)
+*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
+*See LICENSE for the license information
- * -------------------------------------------------------------------------- */
+*-------------------------------------------------------------------------- */
/**
- * @file StereoCamera.h
- * @brief A Stereo Camera based on two Simple Cameras
- * @author Chris Beall
+*@file StereoCamera.h
+*@brief A Stereo Camera based on two Simple Cameras
+*@author Chris Beall
*/
#include
@@ -22,45 +22,77 @@ using namespace gtsam;
namespace gtsam {
-/* ************************************************************************* */
-StereoCamera::StereoCamera(const Pose3& leftCamPose, const Cal3_S2Stereo::shared_ptr K) :
- leftCamPose_(leftCamPose), K_(K) {
-}
-
-/* ************************************************************************* */
-StereoPoint2 StereoCamera::project(const Point3& point,
- boost::optional H1,
- boost::optional 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;
+ /* ************************************************************************* */
+ StereoCamera::StereoCamera(const Pose3& leftCamPose,
+ const Cal3_S2Stereo::shared_ptr K) :
+ leftCamPose_(leftCamPose), K_(K) {
}
- const Cal3_S2Stereo& K = *K_;
- double f_x = K.fx(), f_y = K.fy(), b=K.baseline();
- double d = 1.0 / cameraPoint.z();
- 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);
-}
+ /* ************************************************************************* */
+ StereoPoint2 StereoCamera::project(const Point3& point,
+ boost::optional H1, boost::optional H2) const {
-/* ************************************************************************* */
-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, -f_x *P.x() * d2,
- f_x*d, 0.0, -f_x *(P.x() - b) * d2,
- 0.0, f_y*d, -f_y*P.y() * d2);
-}
+#ifdef STEREOCAMERA_CHAIN_RULE
+ const Point3 q = leftCamPose_.transform_to(point, H1, H2);
+#else
+ // omit derivatives
+ const Point3 q = leftCamPose_.transform_to(point);
+#endif
+
+ // get calibration
+ 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()
+ );
+ }
}
diff --git a/gtsam/geometry/tests/timeStereoCamera.cpp b/gtsam/geometry/tests/timeStereoCamera.cpp
new file mode 100644
index 000000000..08da1b0e3
--- /dev/null
+++ b/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
+#include
+
+#include
+
+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;
+}