From 099ba8a8ca8f8dfc05fd871402d1f081bb47c3c0 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 3 May 2022 13:52:09 -0400 Subject: [PATCH 01/10] Add block Jacobi preconditioner wrap --- gtsam/linear/linear.i | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i index f1bc92f69..943b661d8 100644 --- a/gtsam/linear/linear.i +++ b/gtsam/linear/linear.i @@ -671,6 +671,10 @@ virtual class DummyPreconditionerParameters : gtsam::PreconditionerParameters { DummyPreconditionerParameters(); }; +virtual class BlockJacobiPreconditionerParameters : gtsam::PreconditionerParameters { + BlockJacobiPreconditionerParameters(); +}; + #include virtual class PCGSolverParameters : gtsam::ConjugateGradientParameters { PCGSolverParameters(); From 3571420010a332743293a8903daeba0afc927d90 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 3 May 2022 13:54:25 -0400 Subject: [PATCH 02/10] Added PinholePose in wrap and FromPose3 in EssentialMatrix --- gtsam/geometry/geometry.i | 71 ++++++++++++++++++++++++++++++++++++- gtsam/nonlinear/nonlinear.i | 16 +++++++++ 2 files changed, 86 insertions(+), 1 deletion(-) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 415aa0dc4..63fd7056a 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -547,6 +547,12 @@ class EssentialMatrix { // Standard Constructors EssentialMatrix(const gtsam::Rot3& aRb, const gtsam::Unit3& aTb); + // Constructors from Pose3 + gtsam::EssentialMatrix FromPose3(const gtsam::Pose3& _1P2_); + + gtsam::EssentialMatrix FromPose3(const gtsam::Pose3& _1P2_, + Eigen::Ref H); + // Testable void print(string s = "") const; bool equals(const gtsam::EssentialMatrix& pose, double tol) const; @@ -904,6 +910,12 @@ class PinholeCamera { Eigen::Ref Dresult_dp, Eigen::Ref Dresult_ddepth, Eigen::Ref Dresult_dcal); + + gtsam::Point2 reprojectionError(const gtsam::Point3& pw, const gtsam::Point2& measured, + Eigen::Ref Dpose, + Eigen::Ref Dpoint, + Eigen::Ref Dcal); + double range(const gtsam::Point3& point); double range(const gtsam::Point3& point, Eigen::Ref Dcamera, Eigen::Ref Dpoint); @@ -914,7 +926,58 @@ class PinholeCamera { // enabling serialization functionality void serialize() const; }; - + +#include +template +class PinholePose { + // Standard Constructors and Named Constructors + PinholePose(); + PinholePose(const gtsam::PinholePose other); + PinholePose(const gtsam::Pose3& pose); + PinholePose(const gtsam::Pose3& pose, const CALIBRATION* K); + static This Level(const gtsam::Pose2& pose, double height); + static This Lookat(const gtsam::Point3& eye, const gtsam::Point3& target, + const gtsam::Point3& upVector, const CALIBRATION* K); + + // Testable + void print(string s = "PinholePose") const; + bool equals(const This& camera, double tol) const; + + // Standard Interface + gtsam::Pose3 pose() const; + CALIBRATION calibration() const; + + // Manifold + This retract(Vector d) const; + Vector localCoordinates(const This& T2) const; + size_t dim() const; + static size_t Dim(); + + // Transformations and measurement functions + 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; +}; + #include class Similarity2 { // Standard Constructors @@ -971,6 +1034,12 @@ typedef gtsam::PinholeCamera PinholeCameraCal3Unified; typedef gtsam::PinholeCamera PinholeCameraCal3Bundler; typedef gtsam::PinholeCamera PinholeCameraCal3Fisheye; +typedef gtsam::PinholePose PinholePoseCal3_S2; +typedef gtsam::PinholePose PinholePoseCal3DS2; +typedef gtsam::PinholePose PinholePoseCal3Unified; +typedef gtsam::PinholePose PinholePoseCal3Bundler; +typedef gtsam::PinholePose PinholePoseCal3Fisheye; + template class CameraSet { CameraSet(); diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 3fff71978..30181e08d 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -226,6 +226,10 @@ class Values { void insert(size_t j, const gtsam::PinholeCamera& camera); void insert(size_t j, const gtsam::PinholeCamera& camera); void insert(size_t j, const gtsam::PinholeCamera& camera); + void insert(size_t j, const gtsam::PinholePose& camera); + void insert(size_t j, const gtsam::PinholePose& camera); + void insert(size_t j, const gtsam::PinholePose& camera); + void insert(size_t j, const gtsam::PinholePose& camera); void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); void insert(size_t j, const gtsam::NavState& nav_state); void insert(size_t j, double c); @@ -269,6 +273,10 @@ class Values { void update(size_t j, const gtsam::PinholeCamera& camera); void update(size_t j, const gtsam::PinholeCamera& camera); void update(size_t j, const gtsam::PinholeCamera& camera); + void update(size_t j, const gtsam::PinholePose& camera); + void update(size_t j, const gtsam::PinholePose& camera); + void update(size_t j, const gtsam::PinholePose& camera); + void update(size_t j, const gtsam::PinholePose& camera); void update(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); void update(size_t j, const gtsam::NavState& nav_state); void update(size_t j, Vector vector); @@ -310,6 +318,10 @@ class Values { void insert_or_assign(size_t j, const gtsam::PinholeCamera& camera); void insert_or_assign(size_t j, const gtsam::PinholeCamera& camera); void insert_or_assign(size_t j, const gtsam::PinholeCamera& camera); + void insert_or_assign(size_t j, const gtsam::PinholePose& camera); + void insert_or_assign(size_t j, const gtsam::PinholePose& camera); + void insert_or_assign(size_t j, const gtsam::PinholePose& camera); + void insert_or_assign(size_t j, const gtsam::PinholePose& camera); void insert_or_assign(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); void insert_or_assign(size_t j, const gtsam::NavState& nav_state); void insert_or_assign(size_t j, Vector vector); @@ -351,6 +363,10 @@ class Values { gtsam::PinholeCamera, gtsam::PinholeCamera, gtsam::PinholeCamera, + gtsam::PinholePose, + gtsam::PinholePose, + gtsam::PinholePose, + gtsam::PinholePose, gtsam::imuBias::ConstantBias, gtsam::NavState, Vector, From d4eadbaf20099791e9fbae43075652535be31370 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 3 May 2022 13:54:45 -0400 Subject: [PATCH 03/10] Added wrapping for Shonan constructor --- gtsam/sfm/sfm.i | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index bf9a73ac5..901270d9c 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -9,6 +9,8 @@ class SfmTrack { SfmTrack(); SfmTrack(const gtsam::Point3& pt); const Point3& point3() const; + + Point3 p; double r; double g; @@ -34,6 +36,9 @@ class SfmData { static gtsam::SfmData FromBundlerFile(string filename); static gtsam::SfmData FromBalFile(string filename); + std::vector tracks; + std::vector> cameras; + void addTrack(const gtsam::SfmTrack& t); void addCamera(const gtsam::SfmCamera& cam); size_t numberTracks() const; @@ -184,6 +189,10 @@ class ShonanAveraging2 { }; class ShonanAveraging3 { + ShonanAveraging3( + const std::vector>& measurements, + const gtsam::ShonanAveragingParameters3& parameters = + gtsam::ShonanAveragingParameters3()); ShonanAveraging3(string g2oFile); ShonanAveraging3(string g2oFile, const gtsam::ShonanAveragingParameters3& parameters); From acaf6a273ac8d91d411004f514bf758f7619023d Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 3 May 2022 13:55:08 -0400 Subject: [PATCH 04/10] Wrap GeneralSFMFactor for PinholePose --- gtsam/slam/slam.i | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index 4e943253e..8e1e06d5b 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -90,6 +90,22 @@ typedef gtsam::GeneralSFMFactor, gtsam::Point3> GeneralSFMFactorCal3Unified; +typedef gtsam::GeneralSFMFactor, + gtsam::Point3> + GeneralSFMFactorPoseCal3_S2; +typedef gtsam::GeneralSFMFactor, + gtsam::Point3> + GeneralSFMFactorPoseCal3DS2; +typedef gtsam::GeneralSFMFactor, + gtsam::Point3> + GeneralSFMFactorPoseCal3Bundler; +typedef gtsam::GeneralSFMFactor, + gtsam::Point3> + GeneralSFMFactorPoseCal3Fisheye; +typedef gtsam::GeneralSFMFactor, + gtsam::Point3> + GeneralSFMFactorPoseCal3Unified; + template virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor { From fcec6839d3562c4b34b62243f5ff13b126bad3fc Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 3 May 2022 13:56:41 -0400 Subject: [PATCH 05/10] added SfmTracks and SfmCameras so we can easily access them in Python --- python/gtsam/preamble/sfm.h | 11 ++++++++++- python/gtsam/specializations/sfm.h | 13 +++++++++++++ 2 files changed, 23 insertions(+), 1 deletion(-) diff --git a/python/gtsam/preamble/sfm.h b/python/gtsam/preamble/sfm.h index a34e73058..778375b6b 100644 --- a/python/gtsam/preamble/sfm.h +++ b/python/gtsam/preamble/sfm.h @@ -9,4 +9,13 @@ * automatic STL binding, such that the raw objects can be accessed in Python. * Without this they will be automatically converted to a Python object, and all * mutations on Python side will not be reflected on C++. - */ \ No newline at end of file + */ + +#include +#include + +PYBIND11_MAKE_OPAQUE( + std::vector); + +PYBIND11_MAKE_OPAQUE( + std::vector); \ No newline at end of file diff --git a/python/gtsam/specializations/sfm.h b/python/gtsam/specializations/sfm.h index 6de15217f..a7a1e822f 100644 --- a/python/gtsam/specializations/sfm.h +++ b/python/gtsam/specializations/sfm.h @@ -14,3 +14,16 @@ py::bind_vector > >( m_, "BinaryMeasurementsUnit3"); py::bind_map(m_, "KeyPairDoubleMap"); + +py::bind_vector< + std::vector >( + m_, "SfmTracks"); + +py::bind_vector< + std::vector >( + m_, "SfmCameras"); + +py::bind_vector< + std::vector>>( + m_, "SfmMeasurementVector" + ); From 86b9761b5b4dcc4f6a361df376645c7ddf6f5cfc Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 3 May 2022 13:57:48 -0400 Subject: [PATCH 06/10] Also copy the preambles and specializations --- python/CMakeLists.txt | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index d3cbff32d..0d143354d 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -107,6 +107,14 @@ file(GLOB GTSAM_PYTHON_UTIL_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam/utils/*.py" foreach(util_file ${GTSAM_PYTHON_UTIL_FILES}) configure_file(${util_file} "${GTSAM_MODULE_PATH}/utils/${test_file}" COPYONLY) endforeach() +file(GLOB GTSAM_PYTHON_PREAMBLE_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam/preamble/*.h") +foreach(util_file ${GTSAM_PYTHON_PREAMBLE_FILES}) + configure_file(${util_file} "${GTSAM_MODULE_PATH}/preamble/${test_file}" COPYONLY) +endforeach() +file(GLOB GTSAM_PYTHON_SPECIALIZATION_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam/specializations/*.h") +foreach(util_file ${GTSAM_PYTHON_SPECIALIZATION_FILES}) + configure_file(${util_file} "${GTSAM_MODULE_PATH}/specializations/${test_file}" COPYONLY) +endforeach() # Common directory for data/datasets stored with the package. # This will store the data in the Python site package directly. From 21b8365d7bda979b21bbcf949f7ea78682e05030 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 3 May 2022 16:02:00 -0400 Subject: [PATCH 07/10] Address comments --- gtsam/geometry/geometry.i | 32 ++++++++++++++++---------------- gtsam/sfm/SfmData.h | 8 ++++++-- gtsam/sfm/sfm.i | 8 ++++---- 3 files changed, 26 insertions(+), 22 deletions(-) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 63fd7056a..0dc23c160 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -927,6 +927,16 @@ class PinholeCamera { void serialize() const; }; +// Forward declaration of PinholeCameraCalX is defined here. +#include +// Some typedefs for common camera types +// PinholeCameraCal3_S2 is the same as SimpleCamera above +typedef gtsam::PinholeCamera PinholeCameraCal3_S2; +typedef gtsam::PinholeCamera PinholeCameraCal3DS2; +typedef gtsam::PinholeCamera PinholeCameraCal3Unified; +typedef gtsam::PinholeCamera PinholeCameraCal3Bundler; +typedef gtsam::PinholeCamera PinholeCameraCal3Fisheye; + #include template class PinholePose { @@ -978,6 +988,12 @@ class PinholePose { void serialize() const; }; +typedef gtsam::PinholePose PinholePoseCal3_S2; +typedef gtsam::PinholePose PinholePoseCal3DS2; +typedef gtsam::PinholePose PinholePoseCal3Unified; +typedef gtsam::PinholePose PinholePoseCal3Bundler; +typedef gtsam::PinholePose PinholePoseCal3Fisheye; + #include class Similarity2 { // Standard Constructors @@ -1024,22 +1040,6 @@ class Similarity3 { double scale() const; }; -// Forward declaration of PinholeCameraCalX is defined here. -#include -// Some typedefs for common camera types -// PinholeCameraCal3_S2 is the same as SimpleCamera above -typedef gtsam::PinholeCamera PinholeCameraCal3_S2; -typedef gtsam::PinholeCamera PinholeCameraCal3DS2; -typedef gtsam::PinholeCamera PinholeCameraCal3Unified; -typedef gtsam::PinholeCamera PinholeCameraCal3Bundler; -typedef gtsam::PinholeCamera PinholeCameraCal3Fisheye; - -typedef gtsam::PinholePose PinholePoseCal3_S2; -typedef gtsam::PinholePose PinholePoseCal3DS2; -typedef gtsam::PinholePose PinholePoseCal3Unified; -typedef gtsam::PinholePose PinholePoseCal3Bundler; -typedef gtsam::PinholePose PinholePoseCal3Fisheye; - template class CameraSet { CameraSet(); diff --git a/gtsam/sfm/SfmData.h b/gtsam/sfm/SfmData.h index afce12205..430e107ad 100644 --- a/gtsam/sfm/SfmData.h +++ b/gtsam/sfm/SfmData.h @@ -77,10 +77,14 @@ struct GTSAM_EXPORT SfmData { size_t numberCameras() const { return cameras.size(); } /// The track formed by series of landmark measurements - SfmTrack track(size_t idx) const { return tracks[idx]; } + const SfmTrack& track(size_t idx) const { return tracks[idx]; } /// The camera pose at frame index `idx` - SfmCamera camera(size_t idx) const { return cameras[idx]; } + const SfmCamera& camera(size_t idx) const { return cameras[idx]; } + + /// Getters + const std::vector& cameraList() const { return cameras; } + const std::vector& trackList() const { return tracks; } /** * @brief Create projection factors using keys i and P(j) diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index 901270d9c..0a61874c1 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -36,15 +36,15 @@ class SfmData { static gtsam::SfmData FromBundlerFile(string filename); static gtsam::SfmData FromBalFile(string filename); - std::vector tracks; - std::vector> cameras; + std::vector& trackList() const; + std::vector>& cameraList() const; void addTrack(const gtsam::SfmTrack& t); void addCamera(const gtsam::SfmCamera& cam); size_t numberTracks() const; size_t numberCameras() const; - gtsam::SfmTrack track(size_t idx) const; - gtsam::PinholeCamera camera(size_t idx) const; + gtsam::SfmTrack& track(size_t idx) const; + gtsam::PinholeCamera& camera(size_t idx) const; gtsam::NonlinearFactorGraph generalSfmFactors( const gtsam::SharedNoiseModel& model = From 28b216b72fc0e1cc95a13e2998f2972c377783d0 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 3 May 2022 16:24:09 -0400 Subject: [PATCH 08/10] Correct the binding for BinaryMeasurement --- gtsam/slam/dataset.h | 1 + python/CMakeLists.txt | 2 ++ python/gtsam/preamble/sfm.h | 6 +++++- python/gtsam/specializations/sfm.h | 2 ++ 4 files changed, 10 insertions(+), 1 deletion(-) diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index dc450a9f7..7c4d7fea7 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -223,6 +223,7 @@ parse3DFactors(const std::string &filename, size_t maxIndex = 0); using BinaryMeasurementsUnit3 = std::vector>; +using BinaryMeasurementsRot3 = std::vector>; #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 inline boost::optional GTSAM_DEPRECATED diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 0d143354d..0c8dc314b 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -48,6 +48,7 @@ set(ignore gtsam::Rot3Vector gtsam::KeyVector gtsam::BinaryMeasurementsUnit3 + gtsam::BinaryMeasurementsRot3 gtsam::DiscreteKey gtsam::KeyPairDoubleMap) @@ -137,6 +138,7 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) gtsam::KeyVector gtsam::FixedLagSmootherKeyTimestampMapValue gtsam::BinaryMeasurementsUnit3 + gtsam::BinaryMeasurementsRot3 gtsam::CameraSetCal3_S2 gtsam::CameraSetCal3Bundler gtsam::CameraSetCal3Unified diff --git a/python/gtsam/preamble/sfm.h b/python/gtsam/preamble/sfm.h index 778375b6b..c74f37109 100644 --- a/python/gtsam/preamble/sfm.h +++ b/python/gtsam/preamble/sfm.h @@ -18,4 +18,8 @@ PYBIND11_MAKE_OPAQUE( std::vector); PYBIND11_MAKE_OPAQUE( - std::vector); \ No newline at end of file + std::vector); +PYBIND11_MAKE_OPAQUE( + std::vector>); +PYBIND11_MAKE_OPAQUE( + std::vector>); diff --git a/python/gtsam/specializations/sfm.h b/python/gtsam/specializations/sfm.h index a7a1e822f..90a2b8417 100644 --- a/python/gtsam/specializations/sfm.h +++ b/python/gtsam/specializations/sfm.h @@ -13,6 +13,8 @@ py::bind_vector > >( m_, "BinaryMeasurementsUnit3"); +py::bind_vector > >( + m_, "BinaryMeasurementsRot3"); py::bind_map(m_, "KeyPairDoubleMap"); py::bind_vector< From e2a26346528f997e036bc52d00ffe91dbf9d3d9e Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sat, 7 May 2022 15:53:36 -0700 Subject: [PATCH 09/10] Finally debugged the root cause! --- python/gtsam/preamble/sfm.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/python/gtsam/preamble/sfm.h b/python/gtsam/preamble/sfm.h index c74f37109..8ff0ea82e 100644 --- a/python/gtsam/preamble/sfm.h +++ b/python/gtsam/preamble/sfm.h @@ -11,7 +11,8 @@ * mutations on Python side will not be reflected on C++. */ -#include +// Including can cause some serious hard-to-debug bugs!!! +// #include #include PYBIND11_MAKE_OPAQUE( From 02dce469d185f105f46eb42c147712ece2ff2f3d Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sat, 7 May 2022 15:55:06 -0700 Subject: [PATCH 10/10] Add fallback entry for MATLAB --- gtsam/sfm/sfm.i | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index 0a61874c1..19e21b10c 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -96,6 +96,13 @@ class BinaryMeasurementsUnit3 { void push_back(const gtsam::BinaryMeasurement& measurement); }; +class BinaryMeasurementsRot3 { + BinaryMeasurementsRot3(); + size_t size() const; + gtsam::BinaryMeasurement at(size_t idx) const; + void push_back(const gtsam::BinaryMeasurement& measurement); +}; + #include // TODO(frank): copy/pasta below until we have integer template paremeters in