From 099ba8a8ca8f8dfc05fd871402d1f081bb47c3c0 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 3 May 2022 13:52:09 -0400 Subject: [PATCH 01/14] 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/14] 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/14] 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/14] 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/14] 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/14] 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/14] 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/14] 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 4d275a45e64dc3c39b975e582bcdf05b8c248aa3 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 5 May 2022 11:23:51 -0400 Subject: [PATCH 09/14] remove key formatting from keys when defining vertices and edges to avoid syntax errors --- gtsam/inference/DotWriter.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/inference/DotWriter.cpp b/gtsam/inference/DotWriter.cpp index ad5330575..eac0c90f9 100644 --- a/gtsam/inference/DotWriter.cpp +++ b/gtsam/inference/DotWriter.cpp @@ -43,7 +43,7 @@ void DotWriter::drawVariable(Key key, const KeyFormatter& keyFormatter, const boost::optional& position, ostream* os) const { // Label the node with the label from the KeyFormatter - *os << " var" << keyFormatter(key) << "[label=\"" << keyFormatter(key) + *os << " var" << key << "[label=\"" << keyFormatter(key) << "\""; if (position) { *os << ", pos=\"" << position->x() << "," << position->y() << "!\""; @@ -65,13 +65,13 @@ void DotWriter::DrawFactor(size_t i, const boost::optional& position, static void ConnectVariables(Key key1, Key key2, const KeyFormatter& keyFormatter, ostream* os) { - *os << " var" << keyFormatter(key1) << "--" - << "var" << keyFormatter(key2) << ";\n"; + *os << " var" << key1 << "--" + << "var" << key2 << ";\n"; } static void ConnectVariableFactor(Key key, const KeyFormatter& keyFormatter, size_t i, ostream* os) { - *os << " var" << keyFormatter(key) << "--" + *os << " var" << key << "--" << "factor" << i << ";\n"; } From 51d1c27f2d8c829dfb4711739f2a0fd4d1cd3311 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 5 May 2022 11:24:26 -0400 Subject: [PATCH 10/14] add prior factor tests and remove TODO --- gtsam/nonlinear/Marginals.h | 2 +- gtsam/nonlinear/PriorFactor.h | 1 - gtsam/slam/tests/testPriorFactor.cpp | 40 +++++++++++++++++++++++++--- 3 files changed, 37 insertions(+), 6 deletions(-) diff --git a/gtsam/nonlinear/Marginals.h b/gtsam/nonlinear/Marginals.h index 028545d01..3c5aa9cab 100644 --- a/gtsam/nonlinear/Marginals.h +++ b/gtsam/nonlinear/Marginals.h @@ -121,7 +121,7 @@ public: /** Optimize the bayes tree */ VectorValues optimize() const; - + protected: /** Compute the Bayes Tree as a helper function to the constructor */ diff --git a/gtsam/nonlinear/PriorFactor.h b/gtsam/nonlinear/PriorFactor.h index c745f7bd9..a490162ac 100644 --- a/gtsam/nonlinear/PriorFactor.h +++ b/gtsam/nonlinear/PriorFactor.h @@ -94,7 +94,6 @@ namespace gtsam { Vector evaluateError(const T& x, boost::optional H = boost::none) const override { if (H) (*H) = Matrix::Identity(traits::GetDimension(x),traits::GetDimension(x)); // manifold equivalent of z-x -> Local(x,z) - // TODO(ASL) Add Jacobians. return -traits::Local(x, prior_); } diff --git a/gtsam/slam/tests/testPriorFactor.cpp b/gtsam/slam/tests/testPriorFactor.cpp index 2dc083cb2..d1a60e346 100644 --- a/gtsam/slam/tests/testPriorFactor.cpp +++ b/gtsam/slam/tests/testPriorFactor.cpp @@ -5,12 +5,16 @@ * @date Nov 4, 2014 */ -#include -#include #include +#include +#include +#include +#include using namespace std; +using namespace std::placeholders; using namespace gtsam; +using namespace imuBias; /* ************************************************************************* */ @@ -23,16 +27,44 @@ TEST(PriorFactor, ConstructorScalar) { // Constructor vector3 TEST(PriorFactor, ConstructorVector3) { SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 1.0); - PriorFactor factor(1, Vector3(1,2,3), model); + PriorFactor factor(1, Vector3(1, 2, 3), model); } // Constructor dynamic sized vector TEST(PriorFactor, ConstructorDynamicSizeVector) { - Vector v(5); v << 1, 2, 3, 4, 5; + Vector v(5); + v << 1, 2, 3, 4, 5; SharedNoiseModel model = noiseModel::Isotropic::Sigma(5, 1.0); PriorFactor factor(1, v, model); } +Vector callEvaluateError(const PriorFactor& factor, + const ConstantBias& bias) { + return factor.evaluateError(bias); +} + +// Test for imuBias::ConstantBias +TEST(PriorFactor, ConstantBias) { + Vector3 biasAcc(1, 2, 3); + Vector3 biasGyro(0.1, 0.2, 0.3); + ConstantBias bias(biasAcc, biasGyro); + + PriorFactor factor(1, bias, + noiseModel::Isotropic::Sigma(6, 0.1)); + Values values; + values.insert(1, bias); + + EXPECT_DOUBLES_EQUAL(0.0, factor.error(values), 1e-8); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); + + ConstantBias incorrectBias( + (Vector6() << 1.1, 2.1, 3.1, 0.2, 0.3, 0.4).finished()); + values.clear(); + values.insert(1, incorrectBias); + EXPECT_DOUBLES_EQUAL(3.0, factor.error(values), 1e-8); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); +} + /* ************************************************************************* */ int main() { TestResult tr; From 0cfeb8621c5605b0d32fb469f4aa8d1600c4c8fd Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 5 May 2022 14:35:10 -0400 Subject: [PATCH 11/14] fix tests and remove additional keyformatter calls --- .../tests/testDiscreteFactorGraph.cpp | 14 +++---- gtsam/inference/BayesNet-inst.h | 5 ++- gtsam/symbolic/tests/testSymbolicBayesNet.cpp | 18 ++++----- tests/testNonlinearFactorGraph.cpp | 40 +++++++++---------- 4 files changed, 39 insertions(+), 38 deletions(-) diff --git a/gtsam/discrete/tests/testDiscreteFactorGraph.cpp b/gtsam/discrete/tests/testDiscreteFactorGraph.cpp index 0a7d869ec..3d9621aff 100644 --- a/gtsam/discrete/tests/testDiscreteFactorGraph.cpp +++ b/gtsam/discrete/tests/testDiscreteFactorGraph.cpp @@ -415,16 +415,16 @@ TEST(DiscreteFactorGraph, DotWithNames) { "graph {\n" " size=\"5,5\";\n" "\n" - " varC[label=\"C\"];\n" - " varA[label=\"A\"];\n" - " varB[label=\"B\"];\n" + " var0[label=\"C\"];\n" + " var1[label=\"A\"];\n" + " var2[label=\"B\"];\n" "\n" " factor0[label=\"\", shape=point];\n" - " varC--factor0;\n" - " varA--factor0;\n" + " var0--factor0;\n" + " var1--factor0;\n" " factor1[label=\"\", shape=point];\n" - " varC--factor1;\n" - " varB--factor1;\n" + " var0--factor1;\n" + " var2--factor1;\n" "}\n"; EXPECT(actual == expected); } diff --git a/gtsam/inference/BayesNet-inst.h b/gtsam/inference/BayesNet-inst.h index afde5498d..e792b5c03 100644 --- a/gtsam/inference/BayesNet-inst.h +++ b/gtsam/inference/BayesNet-inst.h @@ -53,8 +53,9 @@ void BayesNet::dot(std::ostream& os, auto frontals = conditional->frontals(); const Key me = frontals.front(); auto parents = conditional->parents(); - for (const Key& p : parents) - os << " var" << keyFormatter(p) << "->var" << keyFormatter(me) << "\n"; + for (const Key& p : parents) { + os << " var" << p << "->var" << me << "\n"; + } } os << "}"; diff --git a/gtsam/symbolic/tests/testSymbolicBayesNet.cpp b/gtsam/symbolic/tests/testSymbolicBayesNet.cpp index 2e13be10e..7795d5b89 100644 --- a/gtsam/symbolic/tests/testSymbolicBayesNet.cpp +++ b/gtsam/symbolic/tests/testSymbolicBayesNet.cpp @@ -104,16 +104,16 @@ TEST(SymbolicBayesNet, Dot) { "digraph {\n" " size=\"5,5\";\n" "\n" - " vara1[label=\"a1\", pos=\"1,2!\", shape=box];\n" - " vara2[label=\"a2\", pos=\"2,2!\", shape=box];\n" - " varx1[label=\"x1\", pos=\"1,1!\"];\n" - " varx2[label=\"x2\", pos=\"2,1!\"];\n" - " varx3[label=\"x3\", pos=\"3,1!\"];\n" + " var6989586621679009793[label=\"a1\", pos=\"1,2!\", shape=box];\n" + " var6989586621679009794[label=\"a2\", pos=\"2,2!\", shape=box];\n" + " var8646911284551352321[label=\"x1\", pos=\"1,1!\"];\n" + " var8646911284551352322[label=\"x2\", pos=\"2,1!\"];\n" + " var8646911284551352323[label=\"x3\", pos=\"3,1!\"];\n" "\n" - " varx1->varx2\n" - " vara1->varx2\n" - " varx2->varx3\n" - " vara2->varx3\n" + " var8646911284551352321->var8646911284551352322\n" + " var6989586621679009793->var8646911284551352322\n" + " var8646911284551352322->var8646911284551352323\n" + " var6989586621679009794->var8646911284551352323\n" "}"); } diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp index 05a6e7f45..e1a88d616 100644 --- a/tests/testNonlinearFactorGraph.cpp +++ b/tests/testNonlinearFactorGraph.cpp @@ -335,21 +335,21 @@ TEST(NonlinearFactorGraph, dot) { "graph {\n" " size=\"5,5\";\n" "\n" - " varl1[label=\"l1\"];\n" - " varx1[label=\"x1\"];\n" - " varx2[label=\"x2\"];\n" + " var7782220156096217089[label=\"l1\"];\n" + " var8646911284551352321[label=\"x1\"];\n" + " var8646911284551352322[label=\"x2\"];\n" "\n" " factor0[label=\"\", shape=point];\n" - " varx1--factor0;\n" + " var8646911284551352321--factor0;\n" " factor1[label=\"\", shape=point];\n" - " varx1--factor1;\n" - " varx2--factor1;\n" + " var8646911284551352321--factor1;\n" + " var8646911284551352322--factor1;\n" " factor2[label=\"\", shape=point];\n" - " varx1--factor2;\n" - " varl1--factor2;\n" + " var8646911284551352321--factor2;\n" + " var7782220156096217089--factor2;\n" " factor3[label=\"\", shape=point];\n" - " varx2--factor3;\n" - " varl1--factor3;\n" + " var8646911284551352322--factor3;\n" + " var7782220156096217089--factor3;\n" "}\n"; const NonlinearFactorGraph fg = createNonlinearFactorGraph(); @@ -363,21 +363,21 @@ TEST(NonlinearFactorGraph, dot_extra) { "graph {\n" " size=\"5,5\";\n" "\n" - " varl1[label=\"l1\", pos=\"0,0!\"];\n" - " varx1[label=\"x1\", pos=\"1,0!\"];\n" - " varx2[label=\"x2\", pos=\"1,1.5!\"];\n" + " var7782220156096217089[label=\"l1\", pos=\"0,0!\"];\n" + " var8646911284551352321[label=\"x1\", pos=\"1,0!\"];\n" + " var8646911284551352322[label=\"x2\", pos=\"1,1.5!\"];\n" "\n" " factor0[label=\"\", shape=point];\n" - " varx1--factor0;\n" + " var8646911284551352321--factor0;\n" " factor1[label=\"\", shape=point];\n" - " varx1--factor1;\n" - " varx2--factor1;\n" + " var8646911284551352321--factor1;\n" + " var8646911284551352322--factor1;\n" " factor2[label=\"\", shape=point];\n" - " varx1--factor2;\n" - " varl1--factor2;\n" + " var8646911284551352321--factor2;\n" + " var7782220156096217089--factor2;\n" " factor3[label=\"\", shape=point];\n" - " varx2--factor3;\n" - " varl1--factor3;\n" + " var8646911284551352322--factor3;\n" + " var7782220156096217089--factor3;\n" "}\n"; const NonlinearFactorGraph fg = createNonlinearFactorGraph(); From 1d77ba55e400560e59dce39961d56ea59ccd240b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 5 May 2022 16:49:03 -0400 Subject: [PATCH 12/14] fix python test --- python/gtsam/tests/test_DiscreteBayesNet.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/python/gtsam/tests/test_DiscreteBayesNet.py b/python/gtsam/tests/test_DiscreteBayesNet.py index 74191dcc7..10c5db612 100644 --- a/python/gtsam/tests/test_DiscreteBayesNet.py +++ b/python/gtsam/tests/test_DiscreteBayesNet.py @@ -11,12 +11,12 @@ Author: Frank Dellaert # pylint: disable=no-name-in-module, invalid-name -import unittest import textwrap +import unittest import gtsam -from gtsam import (DiscreteBayesNet, DiscreteConditional, DiscreteFactorGraph, - DiscreteKeys, DiscreteDistribution, DiscreteValues, Ordering) +from gtsam import (DiscreteBayesNet, DiscreteConditional, DiscreteDistribution, + DiscreteFactorGraph, DiscreteKeys, DiscreteValues, Ordering) from gtsam.utils.test_case import GtsamTestCase # Some keys: @@ -152,10 +152,10 @@ class TestDiscreteBayesNet(GtsamTestCase): var4[label="4"]; var5[label="5"]; var6[label="6"]; - vara0[label="a0", pos="0,2!"]; + var6989586621679009792[label="a0", pos="0,2!"]; var4->var6 - vara0->var3 + var6989586621679009792->var3 var3->var5 var6->var5 }""" From e2a26346528f997e036bc52d00ffe91dbf9d3d9e Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sat, 7 May 2022 15:53:36 -0700 Subject: [PATCH 13/14] 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 14/14] 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