From 923c57e68ba7619d174166b865a1223c53691c4a Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sun, 17 Apr 2022 16:03:00 -0400 Subject: [PATCH] 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()