From 939416c3d0005843cefbbfec8ce308e8f374d21e Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Fri, 15 Apr 2022 13:57:01 -0400 Subject: [PATCH 1/9] Add camera Jacobian --- gtsam/geometry/PinholePose.h | 13 +++++++++++++ gtsam/geometry/geometry.i | 2 ++ 2 files changed, 15 insertions(+) diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h index 7b92be5d5..4bfbc8b2d 100644 --- a/gtsam/geometry/PinholePose.h +++ b/gtsam/geometry/PinholePose.h @@ -121,6 +121,19 @@ public: return _project(pw, Dpose, Dpoint, Dcal); } + /// project, but for Python use + Point2 project(const Point3& pw, Eigen::Ref Dpose, Eigen::Ref Dpoint, Eigen::Ref Dcal) const { + Eigen::Matrix Dpose_; + Eigen::Matrix Dpoint_; + Eigen::Matrix Dcal_; + + auto ret = _project(pw, Dpose_, Dpoint_, Dcal_); + Dpose = Dpose_; + Dpoint = Dpoint_; + Dcal = Dcal_; + return ret; + } + /// project a 3D point from world coordinates into the image Point2 reprojectionError(const Point3& pw, const Point2& measured, OptionalJacobian<2, 6> Dpose = boost::none, OptionalJacobian<2, 3> Dpoint = boost::none, diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 858270c00..46efebde3 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -824,6 +824,7 @@ template class PinholeCamera { // Standard Constructors and Named Constructors PinholeCamera(); + PinholeCamera(const gtsam::PinholeCamera other); PinholeCamera(const gtsam::Pose3& pose); PinholeCamera(const gtsam::Pose3& pose, const CALIBRATION& K); static This Level(const CALIBRATION& K, const gtsam::Pose2& pose, @@ -850,6 +851,7 @@ class PinholeCamera { static gtsam::Point2 Project(const gtsam::Point3& cameraPoint); pair projectSafe(const gtsam::Point3& pw) const; gtsam::Point2 project(const gtsam::Point3& point); + gtsam::Point2 project(const gtsam::Point3& point, Eigen::Ref Dpose, Eigen::Ref Dpoint, Eigen::Ref Dcal); gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const; double range(const gtsam::Point3& point); double range(const gtsam::Pose3& pose); From f3c9b3967b572bbd1b117fce674e19257effe151 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sat, 16 Apr 2022 13:51:18 -0400 Subject: [PATCH 2/9] Use implicit conversion --- gtsam/base/OptionalJacobian.h | 15 +++++++++++++++ gtsam/geometry/PinholePose.h | 13 ------------- 2 files changed, 15 insertions(+), 13 deletions(-) diff --git a/gtsam/base/OptionalJacobian.h b/gtsam/base/OptionalJacobian.h index 07801df7a..8a8891446 100644 --- a/gtsam/base/OptionalJacobian.h +++ b/gtsam/base/OptionalJacobian.h @@ -20,6 +20,7 @@ #pragma once #include // Configuration from CMake #include +#include #ifndef OPTIONALJACOBIAN_NOBOOST #include @@ -96,6 +97,20 @@ public: usurp(dynamic->data()); } + /** + * @brief Constructor from an Eigen::Ref *value*. Will not usurp if dimension is wrong + * @note This is important so we don't overwrite someone else's memory! + */ + OptionalJacobian(Eigen::Ref dynamic_ref) : + map_(nullptr) { + 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"); + } + } + #ifndef OPTIONALJACOBIAN_NOBOOST /// Constructor with boost::none just makes empty diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h index 4bfbc8b2d..7b92be5d5 100644 --- a/gtsam/geometry/PinholePose.h +++ b/gtsam/geometry/PinholePose.h @@ -121,19 +121,6 @@ public: return _project(pw, Dpose, Dpoint, Dcal); } - /// project, but for Python use - Point2 project(const Point3& pw, Eigen::Ref Dpose, Eigen::Ref Dpoint, Eigen::Ref Dcal) const { - Eigen::Matrix Dpose_; - Eigen::Matrix Dpoint_; - Eigen::Matrix Dcal_; - - auto ret = _project(pw, Dpose_, Dpoint_, Dcal_); - Dpose = Dpose_; - Dpoint = Dpoint_; - Dcal = Dcal_; - return ret; - } - /// project a 3D point from world coordinates into the image Point2 reprojectionError(const Point3& pw, const Point2& measured, OptionalJacobian<2, 6> Dpose = boost::none, OptionalJacobian<2, 3> Dpoint = boost::none, From 923c57e68ba7619d174166b865a1223c53691c4a Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sun, 17 Apr 2022 16:03:00 -0400 Subject: [PATCH 3/9] Address comments --- gtsam/base/OptionalJacobian.h | 8 +++- gtsam/geometry/geometry.i | 20 +++++++- python/gtsam/tests/test_PinholeCamera.py | 58 ++++++++++++++++++++++++ 3 files changed, 83 insertions(+), 3 deletions(-) create mode 100644 python/gtsam/tests/test_PinholeCamera.py diff --git a/gtsam/base/OptionalJacobian.h b/gtsam/base/OptionalJacobian.h index 8a8891446..a990ffc0e 100644 --- a/gtsam/base/OptionalJacobian.h +++ b/gtsam/base/OptionalJacobian.h @@ -21,6 +21,7 @@ #include // Configuration from CMake #include #include +#include #ifndef OPTIONALJACOBIAN_NOBOOST #include @@ -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) + ")"); } } diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 46efebde3..fd96bcc02 100644 --- a/gtsam/geometry/geometry.i +++ b/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 Dcal, + Eigen::Ref Dp) const; gtsam::Point2 calibrate(const gtsam::Point2& p) const; + gtsam::Point2 calibrate(const gtsam::Point2& p, + Eigen::Ref Dcal, + Eigen::Ref Dp) const; // enabling serialization functionality void serialize() const; @@ -851,10 +857,22 @@ class PinholeCamera { static gtsam::Point2 Project(const gtsam::Point3& cameraPoint); pair projectSafe(const gtsam::Point3& pw) const; gtsam::Point2 project(const gtsam::Point3& point); - gtsam::Point2 project(const gtsam::Point3& point, Eigen::Ref Dpose, Eigen::Ref Dpoint, Eigen::Ref Dcal); + gtsam::Point2 project(const gtsam::Point3& point, + Eigen::Ref Dpose, + Eigen::Ref Dpoint, + Eigen::Ref Dcal); gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const; + gtsam::Point3 backproject(const gtsam::Point2& p, double depth, + Eigen::Ref Dresult_dpose, + Eigen::Ref Dresult_dp, + Eigen::Ref Dresult_ddepth, + Eigen::Ref Dresult_dcal); double range(const gtsam::Point3& point); + double range(const gtsam::Point3& point, Eigen::Ref Dcamera, + Eigen::Ref Dpoint); double range(const gtsam::Pose3& pose); + double range(const gtsam::Pose3& pose, Eigen::Ref Dcamera, + Eigen::Ref Dpose); // enabling serialization functionality void serialize() const; diff --git a/python/gtsam/tests/test_PinholeCamera.py b/python/gtsam/tests/test_PinholeCamera.py new file mode 100644 index 000000000..ceed8638d --- /dev/null +++ b/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() From 807d63c7bfe6a5d59833b4b4740c2050c8f18863 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sun, 17 Apr 2022 16:49:52 -0400 Subject: [PATCH 4/9] Use templated Eigen::Ref wrapper --- gtsam/base/OptionalJacobian.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/gtsam/base/OptionalJacobian.h b/gtsam/base/OptionalJacobian.h index a990ffc0e..c9a960a89 100644 --- a/gtsam/base/OptionalJacobian.h +++ b/gtsam/base/OptionalJacobian.h @@ -102,7 +102,8 @@ public: * @brief Constructor from an Eigen::Ref *value*. Will not usurp if dimension is wrong * @note This is important so we don't overwrite someone else's memory! */ - OptionalJacobian(Eigen::Ref dynamic_ref) : + template + OptionalJacobian(Eigen::Ref dynamic_ref) : map_(nullptr) { if (dynamic_ref.rows() == Rows && dynamic_ref.cols() == Cols && !dynamic_ref.IsRowMajor) { usurp(dynamic_ref.data()); From e6420cfcb7727c69f9461a03c5361fe1ee35ea4f Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sun, 17 Apr 2022 16:50:07 -0400 Subject: [PATCH 5/9] Added Jacobians for all calibrations --- gtsam/geometry/geometry.i | 24 ++++++++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index fd96bcc02..75c0847a4 100644 --- a/gtsam/geometry/geometry.i +++ b/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, + Eigen::Ref Dcal, + Eigen::Ref Dp) const; gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; + gtsam::Point2 uncalibrate(const gtsam::Point2& p, + Eigen::Ref Dcal, + Eigen::Ref Dp) const; // Standard Interface double fx() const; @@ -686,7 +692,13 @@ virtual class Cal3Unified : gtsam::Cal3DS2_Base { // Note: the signature of this functions differ from the functions // with equal name in the base class. gtsam::Point2 calibrate(const gtsam::Point2& p) const; + gtsam::Point2 calibrate(const gtsam::Point2& p, + Eigen::Ref Dcal, + Eigen::Ref Dp) const; gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; + gtsam::Point2 uncalibrate(const gtsam::Point2& p, + Eigen::Ref Dcal, + Eigen::Ref Dp) const; // enabling serialization functionality void serialize() const; @@ -712,7 +724,13 @@ class Cal3Fisheye { // Action on Point2 gtsam::Point2 calibrate(const gtsam::Point2& p) const; + gtsam::Point2 calibrate(const gtsam::Point2& p, + Eigen::Ref Dcal, + Eigen::Ref Dp) const; gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; + gtsam::Point2 uncalibrate(const gtsam::Point2& p, + Eigen::Ref Dcal, + Eigen::Ref Dp) const; // Standard Interface double fx() const; @@ -775,7 +793,13 @@ class Cal3Bundler { // Action on Point2 gtsam::Point2 calibrate(const gtsam::Point2& p) const; + gtsam::Point2 calibrate(const gtsam::Point2& p, + Eigen::Ref Dcal, + Eigen::Ref Dp) const; gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; + gtsam::Point2 uncalibrate(const gtsam::Point2& p, + Eigen::Ref Dcal, + Eigen::Ref Dp) const; // Standard Interface double fx() const; From 693f05b04a50e3cfaf1a13365e05dc2a297e58a4 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sun, 17 Apr 2022 16:50:23 -0400 Subject: [PATCH 6/9] Use rations numbers in test --- python/gtsam/tests/test_PinholeCamera.py | 35 ++++++++---------------- 1 file changed, 12 insertions(+), 23 deletions(-) diff --git a/python/gtsam/tests/test_PinholeCamera.py b/python/gtsam/tests/test_PinholeCamera.py index ceed8638d..a5282d5b0 100644 --- a/python/gtsam/tests/test_PinholeCamera.py +++ b/python/gtsam/tests/test_PinholeCamera.py @@ -18,41 +18,30 @@ from gtsam.utils.test_case import GtsamTestCase class TestPinholeCamera(GtsamTestCase): + """ + Tests if we can correctly get the camera Jacobians in Python + """ 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) + cam1.project(np.array([1, 1, 1]), 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(Dpoint, np.array([[1, 0, -1], [0, 1, -1]])) + print(repr(Dpose), repr(Dcal)) self.gtsamAssertEquals( Dpose, - Dpose_expected) + np.array([ + [1., -2., 1., -1., 0., 1.], # + [2., -1., -1., 0., -1., 1.] + ])) + + self.gtsamAssertEquals(Dcal, np.array([[1., 2., 4.], [1., 2., 4.]])) - self.gtsamAssertEquals( - Dcal, - np.array([[0.33009787, 19.60464375, 2.214697], - [-0.03162211, -1.87804997, -0.21215951]])) if __name__ == "__main__": unittest.main() From 4c8260bc0e76bd16072539943dc67b8f8fd49059 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sun, 17 Apr 2022 17:04:07 -0400 Subject: [PATCH 7/9] Add stereo jacobians --- gtsam/geometry/geometry.i | 24 +++++++++++++++++++++++- 1 file changed, 23 insertions(+), 1 deletion(-) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 75c0847a4..7818058a3 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -837,12 +837,25 @@ class CalibratedCamera { // Action on Point3 gtsam::Point2 project(const gtsam::Point3& point) const; + gtsam::Point2 project(const gtsam::Point3& point, + Eigen::Ref Dcamera, + Eigen::Ref Dpoint); + gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const; + gtsam::Point3 backproject(const gtsam::Point2& p, double depth, + Eigen::Ref Dresult_dpose, + Eigen::Ref Dresult_dp, + Eigen::Ref Dresult_ddepth); + static gtsam::Point2 Project(const gtsam::Point3& cameraPoint); // Standard Interface gtsam::Pose3 pose() const; double range(const gtsam::Point3& point) const; + double range(const gtsam::Point3& point, Eigen::Ref Dcamera, + Eigen::Ref Dpoint); double range(const gtsam::Pose3& pose) const; + double range(const gtsam::Pose3& point, Eigen::Ref Dcamera, + Eigen::Ref Dpose); double range(const gtsam::CalibratedCamera& camera) const; // enabling serialization functionality @@ -965,9 +978,18 @@ class StereoCamera { static size_t Dim(); // Transformations and measurement functions - gtsam::StereoPoint2 project(const gtsam::Point3& point); + gtsam::StereoPoint2 project(const gtsam::Point3& point) const; gtsam::Point3 backproject(const gtsam::StereoPoint2& p) const; + // project with Jacobian + gtsam::StereoPoint2 project2(const gtsam::Point3& point, + Eigen::Ref H1, + Eigen::Ref H2) const; + + gtsam::Point3 backproject2(const gtsam::StereoPoint2& p, + Eigen::Ref H1, + Eigen::Ref H2) const; + // enabling serialization functionality void serialize() const; }; From f212ac363abe391c9b67b269032e3c60fae86bf9 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sun, 17 Apr 2022 17:04:19 -0400 Subject: [PATCH 8/9] Add Cal3Unified unit test --- python/gtsam/tests/test_Cal3Unified.py | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/python/gtsam/tests/test_Cal3Unified.py b/python/gtsam/tests/test_Cal3Unified.py index bafbacfa4..630109d66 100644 --- a/python/gtsam/tests/test_Cal3Unified.py +++ b/python/gtsam/tests/test_Cal3Unified.py @@ -139,6 +139,17 @@ class TestCal3Unified(GtsamTestCase): self.gtsamAssertEquals(z, np.zeros(2)) self.gtsamAssertEquals(H @ H.T, 4*np.eye(2)) + Dcal = np.zeros((2, 10), order='F') + Dp = np.zeros((2, 2), order='F') + camera.calibrate(img_point, Dcal, Dp) + + self.gtsamAssertEquals(Dcal, np.array( + [[ 0., 0., 0., -1., 0., 0., 0., 0., 0., 0.], + [ 0., 0., 0., 0., -1., 0., 0., 0., 0., 0.]])) + self.gtsamAssertEquals(Dp, np.array( + [[ 1., -0.], + [-0., 1.]])) + @unittest.skip("triangulatePoint3 currently seems to require perspective projections.") def test_triangulation(self): """Estimate spatial point from image measurements""" From da09271d97f8d193f4934787c66d5bb3c3f888b4 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sun, 17 Apr 2022 17:04:40 -0400 Subject: [PATCH 9/9] Remove print --- python/gtsam/tests/test_PinholeCamera.py | 1 - 1 file changed, 1 deletion(-) diff --git a/python/gtsam/tests/test_PinholeCamera.py b/python/gtsam/tests/test_PinholeCamera.py index a5282d5b0..392d48d3f 100644 --- a/python/gtsam/tests/test_PinholeCamera.py +++ b/python/gtsam/tests/test_PinholeCamera.py @@ -32,7 +32,6 @@ class TestPinholeCamera(GtsamTestCase): self.gtsamAssertEquals(Dpoint, np.array([[1, 0, -1], [0, 1, -1]])) - print(repr(Dpose), repr(Dcal)) self.gtsamAssertEquals( Dpose, np.array([