Deprecated project with three derivatives, it's bogus: StereoCamera holds a pointer to a fixed calibration, and hence is similar to the new "PinholePose".

gtsam/geometry/StereoCamera.cpp

@@ -29,16 +29,15 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  StereoPoint2 StereoCamera::project(const Point3& point,
+  StereoPoint2 StereoCamera::project(const Point3& point) const {
-      OptionalJacobian<3,6> H1, OptionalJacobian<3,3> H2,
+    return project2(point);
-      OptionalJacobian<3,0> H3) const {
+  }
+
+  /* ************************************************************************* */
+  StereoPoint2 StereoCamera::project2(const Point3& point,
+      OptionalJacobian<3,6> H1, OptionalJacobian<3,3> H2) const {
 
-#ifdef STEREOCAMERA_CHAIN_RULE
-    const Point3 q = leftCamPose_.transform_to(point, H1, H2);
-#else
-    // omit derivatives
     const Point3 q = leftCamPose_.transform_to(point);
-#endif
 
     if ( q.z() <= 0 ) throw StereoCheiralityException();
 
@@ -56,12 +55,6 @@ namespace gtsam {
 
     // check if derivatives need to be computed
     if (H1 || H2) {
-#ifdef STEREOCAMERA_CHAIN_RULE
-      // just implement chain rule
-      Matrix3 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 v1 = v/fy, v2 = fx*v1, dx=d*x;
@@ -76,9 +69,6 @@ namespace gtsam {
                 fy*R(0, 1) - R(0, 2)*v , fy*R(1, 1) - R(1, 2)*v , fy*R(2, 1) - R(2, 2)*v;
         *H2 << d * (*H2);
       }
-#endif
-      if (H3)
-        throw std::runtime_error("StereoCamera::project does not support third derivative yet");
     }
 
     // finally translate
@@ -86,15 +76,23 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  Matrix3 StereoCamera::Dproject_to_stereo_camera1(const Point3& P) const {
+  StereoPoint2 StereoCamera::project(const Point3& point,
-    double d = 1.0 / P.z(), d2 = d*d;
+      OptionalJacobian<3,6> H1, OptionalJacobian<3,3> H2,
-    const Cal3_S2Stereo& K = *K_;
+      OptionalJacobian<3,0> H3) const {
-    double f_x = K.fx(), f_y = K.fy(), b = K.baseline();
+    if (H3)
-    Matrix3 m;
+    throw std::runtime_error(
-    m << f_x*d,   0.0, -d2*f_x* P.x(),
+        "StereoCamera::project does not support third derivative - BTW use project2");
-         f_x*d,   0.0, -d2*f_x*(P.x() - b),
+    return project2(point,H1,H2);
-           0.0, f_y*d, -d2*f_y* P.y();
+  }
-    return m;
+
+  /* ************************************************************************* */
+  Point3 StereoCamera::backproject(const StereoPoint2& z) const {
+    Vector measured = z.vector();
+    double Z = K_->baseline() * K_->fx() / (measured[0] - measured[1]);
+    double X = Z * (measured[0] - K_->px()) / K_->fx();
+    double Y = Z * (measured[2] - K_->py()) / K_->fy();
+    Point3 world_point = leftCamPose_.transform_from(Point3(X, Y, Z));
+    return world_point;
   }
 
 }

gtsam/geometry/StereoCamera.h

@@ -17,9 +17,6 @@
 
 #pragma once
 
-#include <boost/tuple/tuple.hpp>
-
-#include <gtsam/base/DerivedValue.h>
 #include <gtsam/geometry/Cal3_S2Stereo.h>
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/geometry/StereoPoint2.h>
@@ -127,33 +124,37 @@ public:
     return K_->baseline();
   }
 
-  /*
+  /// Project 3D point to StereoPoint2 (uL,uR,v)
-   * project 3D point and compute optional derivatives
+  StereoPoint2 project(const Point3& point) const;
+
+  /** Project 3D point and compute optional derivatives
+   * @param H1 derivative with respect to pose
+   * @param H2 derivative with respect to point
+   */
+  StereoPoint2 project2(const Point3& point, OptionalJacobian<3, 6> H1 =
+      boost::none, OptionalJacobian<3, 3> H2 = boost::none) const;
+
+  /// back-project a measurement
+  Point3 backproject(const StereoPoint2& z) const;
+
+  /// @}
+  /// @name Deprecated
+  /// @{
+
+  /** Project 3D point and compute optional derivatives
+   * @deprecated, use project2 - this class has fixed calibration
    * @param H1 derivative with respect to pose
    * @param H2 derivative with respect to point
    * @param H3 IGNORED (for calibration)
    */
-  StereoPoint2 project(const Point3& point, OptionalJacobian<3, 6> H1 =
+  StereoPoint2 project(const Point3& point, OptionalJacobian<3, 6> H1,
-      boost::none, OptionalJacobian<3, 3> H2 = boost::none,
+      OptionalJacobian<3, 3> H2 = boost::none, OptionalJacobian<3, 0> H3 =
-      OptionalJacobian<3, 0> H3 = boost::none) const;
+          boost::none) const;
-
-  /// back-project a measurement
-  Point3 backproject(const StereoPoint2& z) const {
-    Vector measured = z.vector();
-    double Z = K_->baseline() * K_->fx() / (measured[0] - measured[1]);
-    double X = Z * (measured[0] - K_->px()) / K_->fx();
-    double Y = Z * (measured[2] - K_->py()) / K_->fy();
-    Point3 world_point = leftCamPose_.transform_from(Point3(X, Y, Z));
-    return world_point;
-  }
 
   /// @}
 
 private:
 
-  /// utility function
-  Matrix3 Dproject_to_stereo_camera1(const Point3& P) const;
-
   friend class boost::serialization::access;
   template<class Archive>
   void serialize(Archive & ar, const unsigned int version) {

gtsam/geometry/tests/testStereoCamera.cpp

@@ -96,16 +96,12 @@ static StereoPoint2 project3(const Pose3& pose, const Point3& point, const Cal3_
 TEST( StereoCamera, Dproject)
 {
   Matrix expected1 = numericalDerivative31(project3, camPose, landmark, *K);
-  Matrix actual1; stereoCam.project(landmark, actual1, boost::none, boost::none);
+  Matrix actual1; stereoCam.project2(landmark, actual1, boost::none);
   CHECK(assert_equal(expected1,actual1,1e-7));
 
   Matrix expected2 = numericalDerivative32(project3,camPose, landmark, *K);
-  Matrix actual2; stereoCam.project(landmark, boost::none, actual2, boost::none);
+  Matrix actual2; stereoCam.project2(landmark, boost::none, actual2);
   CHECK(assert_equal(expected2,actual2,1e-7));
-
-  Matrix expected3 = numericalDerivative33(project3,camPose, landmark, *K);
-  Matrix actual3; stereoCam.project(landmark, boost::none, boost::none, actual3);
-//  CHECK(assert_equal(expected3,actual3,1e-8));
 }
 
 /* ************************************************************************* */