Address comments

gtsam/base/OptionalJacobian.h

@@ -21,6 +21,7 @@
 #include <gtsam/config.h>      // Configuration from CMake
 #include <Eigen/Dense>
 #include <stdexcept>
+#include <string>
 
 #ifndef OPTIONALJACOBIAN_NOBOOST
 #include <boost/optional.hpp>
@@ -106,8 +107,11 @@ public:
     if (dynamic_ref.rows() == Rows && dynamic_ref.cols() == Cols && !dynamic_ref.IsRowMajor) {
       usurp(dynamic_ref.data());
     } else {
-      // It's never a good idea to throw in the constructor
-      throw std::invalid_argument("OptionalJacobian called with wrong dimensions or storage order.\n");
+      throw std::invalid_argument(
+          std::string("OptionalJacobian called with wrong dimensions or "
+                      "storage order.\n"
+                      "Expected: ") +
+          "(" + std::to_string(Rows) + ", " + std::to_string(Cols) + ")");
     }
   }
 

gtsam/geometry/geometry.i

@@ -623,7 +623,13 @@ virtual class Cal3DS2_Base {
 
   // Action on Point2
   gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
+  gtsam::Point2 uncalibrate(const gtsam::Point2& p,
+                            Eigen::Ref<Eigen::MatrixXd> Dcal,
+                            Eigen::Ref<Eigen::MatrixXd> Dp) const;
   gtsam::Point2 calibrate(const gtsam::Point2& p) const;
+  gtsam::Point2 calibrate(const gtsam::Point2& p,
+                          Eigen::Ref<Eigen::MatrixXd> Dcal,
+                          Eigen::Ref<Eigen::MatrixXd> Dp) const;
 
   // enabling serialization functionality
   void serialize() const;
@@ -851,10 +857,22 @@ class PinholeCamera {
   static gtsam::Point2 Project(const gtsam::Point3& cameraPoint);
   pair<gtsam::Point2, bool> projectSafe(const gtsam::Point3& pw) const;
   gtsam::Point2 project(const gtsam::Point3& point);
-  gtsam::Point2 project(const gtsam::Point3& point, Eigen::Ref<Eigen::MatrixXd> Dpose, Eigen::Ref<Eigen::MatrixXd> Dpoint, Eigen::Ref<Eigen::MatrixXd> Dcal);
+  gtsam::Point2 project(const gtsam::Point3& point,
+                        Eigen::Ref<Eigen::MatrixXd> Dpose,
+                        Eigen::Ref<Eigen::MatrixXd> Dpoint,
+                        Eigen::Ref<Eigen::MatrixXd> Dcal);
   gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const;
+  gtsam::Point3 backproject(const gtsam::Point2& p, double depth,
+                            Eigen::Ref<Eigen::MatrixXd> Dresult_dpose,
+                            Eigen::Ref<Eigen::MatrixXd> Dresult_dp,
+                            Eigen::Ref<Eigen::MatrixXd> Dresult_ddepth,
+                            Eigen::Ref<Eigen::MatrixXd> Dresult_dcal);
   double range(const gtsam::Point3& point);
+  double range(const gtsam::Point3& point, Eigen::Ref<Eigen::MatrixXd> Dcamera,
+               Eigen::Ref<Eigen::MatrixXd> Dpoint);
   double range(const gtsam::Pose3& pose);
+  double range(const gtsam::Pose3& pose, Eigen::Ref<Eigen::MatrixXd> Dcamera,
+               Eigen::Ref<Eigen::MatrixXd> Dpose);
 
   // enabling serialization functionality
   void serialize() const;

python/gtsam/tests/test_PinholeCamera.py

@@ -0,0 +1,58 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+PinholeCamera unit tests.
+Author: Fan Jiang
+"""
+import unittest
+from math import pi
+
+import numpy as np
+
+import gtsam
+from gtsam.utils.test_case import GtsamTestCase
+
+
+class TestPinholeCamera(GtsamTestCase):
+    def test_jacobian(self):
+        cam1 = gtsam.PinholeCameraCal3Bundler()
+        cam1.deserialize(
+            '22 serialization::archive 19 1 0\n0 0 0 0 0 1 0\n1 1 0\n2 9.99727427959442139e-01 6.30191620439291000e-03 2.24814359098672867e-02 5.97546668723225594e-03 -9.99876141548156738e-01 1.45585928112268448e-02 2.25703977048397064e-02 -1.44202867522835732e-02 -9.99641239643096924e-01 0 0 -5.81446531765312802e-02 -3.64078342172925590e-02 -5.63949743097517997e-01 1 0\n3 0 0 5.18692016601562500e+02 5.18692016601562500e+02 0.00000000000000000e+00 0.00000000000000000e+00 0.00000000000000000e+00 -1.14570140838623047e-01 -3.44798192381858826e-02 1.00000000000000008e-05\n'
+        )
+
+        # order is important because Eigen is column major!
+        Dpose = np.zeros((2, 6), order='F')
+        Dpoint = np.zeros((2, 3), order='F')
+        Dcal = np.zeros((2, 3), order='F')
+        cam1.project(np.array([0.43350768, 0.0305741, -1.93050155]), Dpose,
+                     Dpoint, Dcal)
+
+        self.gtsamAssertEquals(
+            Dpoint,
+            np.array([[358.22984814, -0.58837638, 128.85360812],
+                      [3.58714683, -371.03278204, -16.89571148]]))
+
+        Dpose_expected = np.array([[
+                -3.97279895, -553.22184659, -16.40213844, -361.03694491,
+                -0.98773192, 120.76242884
+            ],
+            [
+                512.13105406, 3.97279895, -171.21912901,
+                -0.98773192, -371.25308927, -11.56857932
+            ]])
+
+        self.gtsamAssertEquals(
+            Dpose,
+            Dpose_expected)
+
+        self.gtsamAssertEquals(
+            Dcal,
+            np.array([[0.33009787, 19.60464375, 2.214697],
+                      [-0.03162211, -1.87804997, -0.21215951]]))
+
+if __name__ == "__main__":
+    unittest.main()