Add camera and Calibration Jacobians in Python #1171

ProfFan · 2022-04-15T17:57:36Z

ProfFan · 2022-04-15T18:00:13Z

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,3), order='F')
Dpoint = np.zeros((2,3), order='F')
Dcal = np.zeros((2,6), order='F')
cam1.project(np.array([ 0.43350768,  0.0305741 , -1.93050155]), Dpose, Dpoint, Dcal)

dellaert

That’s amazing! Add a test in python?
Should we merge this after adding a large number of overloads?

gtsam/base/OptionalJacobian.h

dellaert · 2022-04-16T18:52:47Z

gtsam/geometry/geometry.i

@@ -850,6 +851,7 @@ 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);


Ok, that’s very cool!

ProfFan · 2022-04-17T20:05:04Z

@dellaert I added some overloads, but it's too cumbersome to add them all. I think maybe @shteren1 could send a PR with wrapping of those he needs?

dellaert

See comments. Regarding wrapping everything:
Could we just do all camera derivatives as promised by PR? That's not all derivatives in whole of GTSAM, but a good start/example. Just part of the camera derivatives is neither here nor there I think.

python/gtsam/tests/test_PinholeCamera.py

dellaert

Approving modulo some style comments.

As far as modernizing the test, should be easy, but I understand if you're strapped for time.

dellaert · 2022-04-17T21:19:17Z

gtsam/geometry/geometry.i

@@ -584,7 +584,13 @@ class Cal3_S2 {

  // Action on Point2
  gtsam::Point2 calibrate(const gtsam::Point2& p) const;
+  gtsam::Point2 calibrate(const gtsam::Point2& p,


Cal3_S2 is not a camera, so thank you for the extra work :-) But please update PR comment (do that anyway, it's very terse) to say you also provide calibration derivatives.

PS, speaking about extra work: why not just do this in the base class? Is that because it is virtual and we can't handle?

Most are done in the Base class, but some of them actually are different than the base class. I decided that I should not touch anything that works, so I just kept all the original and added overloads.

dellaert · 2022-04-17T21:23:33Z

python/gtsam/tests/test_Cal3Unified.py

+        Dp = np.zeros((2, 2), order='F')
+        camera.calibrate(img_point, Dcal, Dp)
+
+        self.gtsamAssertEquals(Dcal, np.array(


numpy arrays should be tested with np.testing.assert...

internally gtsamAssertEquals also deal with NP arrays

dellaert · 2022-04-17T21:24:38Z

python/gtsam/tests/test_Cal3Unified.py

@@ -139,6 +139,17 @@ def test_jacobian(self):
        self.gtsamAssertEquals(z, np.zeros(2))
        self.gtsamAssertEquals(H @ H.T, 4*np.eye(2))

+        Dcal = np.zeros((2, 10), order='F')


Now that we have derivatives, we no longer have to jump through the linearize hoop above for the earlier test. Can you read and update the test?

I think they test for different derivatives though

dellaert · 2022-04-17T21:24:52Z

python/gtsam/tests/test_PinholeCamera.py

+        Dcal = np.zeros((2, 3), order='F')
+        cam1.project(np.array([1, 1, 1]), Dpose, Dpoint, Dcal)
+
+        self.gtsamAssertEquals(Dpoint, np.array([[1, 0, -1], [0, 1, -1]]))


np.testing...

ProfFan · 2022-04-17T23:39:13Z

I'll merge first, don't really have time for another round...

dellaert · 2022-04-18T00:04:15Z

Okidoke. Could you announce on the Google group, though? This is pretty cool and I’d like you to get the credit :-)

shteren1 · 2022-04-18T05:34:39Z

@ProfFan Thank you very much! I will create a PR soon with additional jacobians wrappings for python.
I think GTSAM is a great alternative for scipy.optimize.least_squares or JAX now when it comes to general NLSQ optimizations in python.
and it'll only get better if you implement the JIT mechanism mentioned at #875.

Add camera Jacobian

939416c

ProfFan requested a review from dellaert April 15, 2022 17:57

ProfFan mentioned this pull request Apr 15, 2022

Jacobian access in python wrapper #1170

Closed

Use implicit conversion

f3c9b39

dellaert reviewed Apr 16, 2022

View reviewed changes

Address comments

923c57e

dellaert reviewed Apr 17, 2022

View reviewed changes

python/gtsam/tests/test_PinholeCamera.py Show resolved Hide resolved

python/gtsam/tests/test_PinholeCamera.py Outdated Show resolved Hide resolved

python/gtsam/tests/test_PinholeCamera.py Show resolved Hide resolved

ProfFan added 6 commits April 17, 2022 16:49

Use templated Eigen::Ref wrapper

807d63c

Added Jacobians for all calibrations

e6420cf

Use rations numbers in test

693f05b

Add stereo jacobians

4c8260b

Add Cal3Unified unit test

f212ac3

Remove print

da09271

ProfFan requested a review from dellaert April 17, 2022 21:06

dellaert approved these changes Apr 17, 2022

View reviewed changes

ProfFan changed the title ~~Add camera Jacobian~~ Add camera and Calibration Jacobians in Python Apr 17, 2022

ProfFan merged commit 3cf7442 into develop Apr 17, 2022

ProfFan deleted the feature/jacobian_camera branch April 17, 2022 23:39

ProfFan restored the feature/jacobian_camera branch April 19, 2022 00:02

dellaert deleted the feature/jacobian_camera branch April 23, 2022 13:03

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Add camera and Calibration Jacobians in Python #1171

Add camera and Calibration Jacobians in Python #1171

ProfFan commented Apr 15, 2022

ProfFan commented Apr 15, 2022 •

edited

Loading

dellaert left a comment

dellaert Apr 16, 2022

ProfFan commented Apr 17, 2022

dellaert left a comment

dellaert left a comment

dellaert Apr 17, 2022

ProfFan Apr 17, 2022

dellaert Apr 17, 2022

ProfFan Apr 17, 2022

dellaert Apr 17, 2022

ProfFan Apr 17, 2022

dellaert Apr 17, 2022

ProfFan commented Apr 17, 2022

dellaert commented Apr 18, 2022

shteren1 commented Apr 18, 2022

Add camera and Calibration Jacobians in Python #1171

Add camera and Calibration Jacobians in Python #1171

Conversation

ProfFan commented Apr 15, 2022

ProfFan commented Apr 15, 2022 • edited Loading

dellaert left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

ProfFan commented Apr 17, 2022

dellaert left a comment

Choose a reason for hiding this comment

dellaert left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

ProfFan commented Apr 17, 2022

dellaert commented Apr 18, 2022

shteren1 commented Apr 18, 2022

ProfFan commented Apr 15, 2022 •

edited

Loading