diff --git a/gtsam/base/OptionalJacobian.h b/gtsam/base/OptionalJacobian.h index 07801df7a..c9a960a89 100644 --- a/gtsam/base/OptionalJacobian.h +++ b/gtsam/base/OptionalJacobian.h @@ -20,6 +20,8 @@ #pragma once #include // Configuration from CMake #include +#include +#include #ifndef OPTIONALJACOBIAN_NOBOOST #include @@ -96,6 +98,24 @@ 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! + */ + template + OptionalJacobian(Eigen::Ref dynamic_ref) : + map_(nullptr) { + if (dynamic_ref.rows() == Rows && dynamic_ref.cols() == Cols && !dynamic_ref.IsRowMajor) { + usurp(dynamic_ref.data()); + } else { + throw std::invalid_argument( + std::string("OptionalJacobian called with wrong dimensions or " + "storage order.\n" + "Expected: ") + + "(" + std::to_string(Rows) + ", " + std::to_string(Cols) + ")"); + } + } + #ifndef OPTIONALJACOBIAN_NOBOOST /// Constructor with boost::none just makes empty diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 858270c00..7818058a3 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; @@ -623,7 +629,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; @@ -680,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; @@ -706,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; @@ -769,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; @@ -807,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 @@ -824,6 +867,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,9 +894,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::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; @@ -921,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; }; 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""" diff --git a/python/gtsam/tests/test_PinholeCamera.py b/python/gtsam/tests/test_PinholeCamera.py new file mode 100644 index 000000000..392d48d3f --- /dev/null +++ b/python/gtsam/tests/test_PinholeCamera.py @@ -0,0 +1,46 @@ +""" +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): + """ + Tests if we can correctly get the camera Jacobians in Python + """ + def test_jacobian(self): + cam1 = gtsam.PinholeCameraCal3Bundler() + + # 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([1, 1, 1]), Dpose, Dpoint, Dcal) + + self.gtsamAssertEquals(Dpoint, np.array([[1, 0, -1], [0, 1, -1]])) + + self.gtsamAssertEquals( + Dpose, + 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.]])) + + +if __name__ == "__main__": + unittest.main()