From d0279d2738548b8e25aca2b9a39c172b99a561a6 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 31 Jan 2022 11:57:31 -0500 Subject: [PATCH 01/20] Add pybind11/stl.h and get it compiling --- gtsam/base/base.i | 36 ----------------------------- python/gtsam/gtsam.tpl | 1 + python/gtsam/specializations/base.h | 3 --- 3 files changed, 1 insertion(+), 39 deletions(-) diff --git a/gtsam/base/base.i b/gtsam/base/base.i index 9838f97d3..f4184fa12 100644 --- a/gtsam/base/base.i +++ b/gtsam/base/base.i @@ -41,45 +41,9 @@ class DSFMap { std::map sets(); }; -class IndexPairSet { - IndexPairSet(); - // common STL methods - size_t size() const; - bool empty() const; - void clear(); - - // structure specific methods - void insert(gtsam::IndexPair key); - bool erase(gtsam::IndexPair key); // returns true if value was removed - bool count(gtsam::IndexPair key) const; // returns true if value exists -}; - -class IndexPairVector { - IndexPairVector(); - IndexPairVector(const gtsam::IndexPairVector& other); - - // common STL methods - size_t size() const; - bool empty() const; - void clear(); - - // structure specific methods - gtsam::IndexPair at(size_t i) const; - void push_back(gtsam::IndexPair key) const; -}; gtsam::IndexPairVector IndexPairSetAsArray(gtsam::IndexPairSet& set); -class IndexPairSetMap { - IndexPairSetMap(); - // common STL methods - size_t size() const; - bool empty() const; - void clear(); - - // structure specific methods - gtsam::IndexPairSet at(gtsam::IndexPair& key); -}; #include bool linear_independent(Matrix A, Matrix B, double tol); diff --git a/python/gtsam/gtsam.tpl b/python/gtsam/gtsam.tpl index b760e4eb5..bdeb2bccc 100644 --- a/python/gtsam/gtsam.tpl +++ b/python/gtsam/gtsam.tpl @@ -11,6 +11,7 @@ {include_boost} #include +#include #include #include #include diff --git a/python/gtsam/specializations/base.h b/python/gtsam/specializations/base.h index 9eefdeca8..995753da1 100644 --- a/python/gtsam/specializations/base.h +++ b/python/gtsam/specializations/base.h @@ -11,7 +11,4 @@ * and saves one copy operation. */ -py::bind_map(m_, "IndexPairSetMap"); -py::bind_vector(m_, "IndexPairVector"); - py::bind_vector >(m_, "JacobianVector"); From 394bb82dba39cb8b8f456612957cc1f8025dd19b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 31 Jan 2022 12:14:31 -0500 Subject: [PATCH 02/20] remove unnecessary includes --- python/gtsam/preamble/basis.h | 2 -- python/gtsam/preamble/discrete.h | 2 -- python/gtsam/preamble/inference.h | 2 -- 3 files changed, 6 deletions(-) diff --git a/python/gtsam/preamble/basis.h b/python/gtsam/preamble/basis.h index 56a07cfdd..d07a75f6f 100644 --- a/python/gtsam/preamble/basis.h +++ b/python/gtsam/preamble/basis.h @@ -10,5 +10,3 @@ * Without this they will be automatically converted to a Python object, and all * mutations on Python side will not be reflected on C++. */ - -#include diff --git a/python/gtsam/preamble/discrete.h b/python/gtsam/preamble/discrete.h index 608508c32..598ab626d 100644 --- a/python/gtsam/preamble/discrete.h +++ b/python/gtsam/preamble/discrete.h @@ -11,6 +11,4 @@ * mutations on Python side will not be reflected on C++. */ -#include - PYBIND11_MAKE_OPAQUE(gtsam::DiscreteKeys); diff --git a/python/gtsam/preamble/inference.h b/python/gtsam/preamble/inference.h index 320e0ac71..bd71c0817 100644 --- a/python/gtsam/preamble/inference.h +++ b/python/gtsam/preamble/inference.h @@ -11,5 +11,3 @@ * mutations on Python side will not be reflected on C++. */ -#include - From 19287ad5c889b08cf64855fe48b7a89bd0d4e174 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 31 Jan 2022 12:15:43 -0500 Subject: [PATCH 03/20] fix geometry modules --- gtsam/geometry/geometry.i | 54 ------------------------- python/gtsam/preamble/geometry.h | 12 ++---- python/gtsam/specializations/geometry.h | 7 ---- 3 files changed, 3 insertions(+), 70 deletions(-) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 1e42966f8..97ffe6f18 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -29,38 +29,6 @@ class Point2 { void serialize() const; }; -class Point2Pairs { - Point2Pairs(); - size_t size() const; - bool empty() const; - gtsam::Point2Pair at(size_t n) const; - void push_back(const gtsam::Point2Pair& point_pair); -}; - -// std::vector -class Point2Vector { - // Constructors - Point2Vector(); - Point2Vector(const gtsam::Point2Vector& v); - - // Capacity - size_t size() const; - size_t max_size() const; - void resize(size_t sz); - size_t capacity() const; - bool empty() const; - void reserve(size_t n); - - // Element access - gtsam::Point2 at(size_t n) const; - gtsam::Point2 front() const; - gtsam::Point2 back() const; - - // Modifiers - void assign(size_t n, const gtsam::Point2& u); - void push_back(const gtsam::Point2& x); - void pop_back(); -}; #include class StereoPoint2 { @@ -127,13 +95,6 @@ class Point3 { void serialize() const; }; -class Point3Pairs { - Point3Pairs(); - size_t size() const; - bool empty() const; - gtsam::Point3Pair at(size_t n) const; - void push_back(const gtsam::Point3Pair& point_pair); -}; #include class Rot2 { @@ -486,21 +447,6 @@ class Pose3 { void serialize() const; }; -class Pose3Pairs { - Pose3Pairs(); - size_t size() const; - bool empty() const; - gtsam::Pose3Pair at(size_t n) const; - void push_back(const gtsam::Pose3Pair& pose_pair); -}; - -class Pose3Vector { - Pose3Vector(); - size_t size() const; - bool empty() const; - gtsam::Pose3 at(size_t n) const; - void push_back(const gtsam::Pose3& pose); -}; #include class Unit3 { diff --git a/python/gtsam/preamble/geometry.h b/python/gtsam/preamble/geometry.h index 35fe2a577..ca03cdfe2 100644 --- a/python/gtsam/preamble/geometry.h +++ b/python/gtsam/preamble/geometry.h @@ -10,7 +10,6 @@ * Without this they will be automatically converted to a Python object, and all * mutations on Python side will not be reflected on C++. */ -#include // Support for binding boost::optional types in C++11. // https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html @@ -19,12 +18,7 @@ namespace pybind11 { namespace detail { struct type_caster> : optional_caster> {}; }} -PYBIND11_MAKE_OPAQUE( - std::vector>); -PYBIND11_MAKE_OPAQUE(gtsam::Point2Pairs); -PYBIND11_MAKE_OPAQUE(gtsam::Point3Pairs); -PYBIND11_MAKE_OPAQUE(gtsam::Pose3Pairs); -PYBIND11_MAKE_OPAQUE(std::vector); -PYBIND11_MAKE_OPAQUE( - gtsam::CameraSet>); PYBIND11_MAKE_OPAQUE(gtsam::CameraSet>); +PYBIND11_MAKE_OPAQUE(gtsam::CameraSet>); +PYBIND11_MAKE_OPAQUE(gtsam::CameraSet>); +PYBIND11_MAKE_OPAQUE(gtsam::CameraSet>); diff --git a/python/gtsam/specializations/geometry.h b/python/gtsam/specializations/geometry.h index a492ce8eb..f63e942a2 100644 --- a/python/gtsam/specializations/geometry.h +++ b/python/gtsam/specializations/geometry.h @@ -11,13 +11,6 @@ * and saves one copy operation. */ -py::bind_vector< - std::vector>>( - m_, "Point2Vector"); -py::bind_vector>(m_, "Point2Pairs"); -py::bind_vector>(m_, "Point3Pairs"); -py::bind_vector>(m_, "Pose3Pairs"); -py::bind_vector>(m_, "Pose3Vector"); py::bind_vector>>( m_, "CameraSetCal3_S2"); py::bind_vector>>( From f5da8522221b6d6e2e5f7de0eeec890f08074a5c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 31 Jan 2022 12:49:06 -0500 Subject: [PATCH 04/20] remove KeyVector --- gtsam/gtsam.i | 54 ---------------------------- python/gtsam/preamble/gtsam.h | 5 --- python/gtsam/specializations/gtsam.h | 8 ----- 3 files changed, 67 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index d4e959c3d..45bd61d0b 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -66,27 +66,6 @@ class KeySet { void serialize() const; }; -// Actually a vector -class KeyVector { - KeyVector(); - KeyVector(const gtsam::KeyVector& other); - - // Note: no print function - - // common STL methods - size_t size() const; - bool empty() const; - void clear(); - - // structure specific methods - size_t at(size_t i) const; - size_t front() const; - size_t back() const; - void push_back(size_t key) const; - - void serialize() const; -}; - // Actually a FastMap class KeyGroupMap { KeyGroupMap(); @@ -104,39 +83,6 @@ class KeyGroupMap { bool insert2(size_t key, int val); }; -// Actually a FastSet -class FactorIndexSet { - FactorIndexSet(); - FactorIndexSet(const gtsam::FactorIndexSet& set); - - // common STL methods - size_t size() const; - bool empty() const; - void clear(); - - // structure specific methods - void insert(size_t factorIndex); - bool erase(size_t factorIndex); // returns true if value was removed - bool count(size_t factorIndex) const; // returns true if value exists -}; - -// Actually a vector -class FactorIndices { - FactorIndices(); - FactorIndices(const gtsam::FactorIndices& other); - - // common STL methods - size_t size() const; - bool empty() const; - void clear(); - - // structure specific methods - size_t at(size_t i) const; - size_t front() const; - size_t back() const; - void push_back(size_t factorIndex) const; -}; - //************************************************************************* // Utilities //************************************************************************* diff --git a/python/gtsam/preamble/gtsam.h b/python/gtsam/preamble/gtsam.h index ec39c410a..d07a75f6f 100644 --- a/python/gtsam/preamble/gtsam.h +++ b/python/gtsam/preamble/gtsam.h @@ -10,8 +10,3 @@ * Without this they will be automatically converted to a Python object, and all * mutations on Python side will not be reflected on C++. */ -#ifdef GTSAM_ALLOCATOR_TBB -PYBIND11_MAKE_OPAQUE(std::vector>); -#else -PYBIND11_MAKE_OPAQUE(std::vector); -#endif diff --git a/python/gtsam/specializations/gtsam.h b/python/gtsam/specializations/gtsam.h index 490d71902..da8842eaf 100644 --- a/python/gtsam/specializations/gtsam.h +++ b/python/gtsam/specializations/gtsam.h @@ -10,11 +10,3 @@ * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python, * and saves one copy operation. */ - -#ifdef GTSAM_ALLOCATOR_TBB -py::bind_vector > >(m_, "KeyVector"); -py::implicitly_convertible > >(); -#else -py::bind_vector >(m_, "KeyVector"); -py::implicitly_convertible >(); -#endif From b93145cd89c420a47a95cd0deea29474548dad6a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 31 Jan 2022 13:33:09 -0500 Subject: [PATCH 05/20] remove unneeded code --- gtsam/geometry/Rot3.h | 3 --- gtsam/sfm/sfm.i | 16 ---------------- gtsam/slam/slam.i | 20 +------------------- python/gtsam/gtsam.tpl | 2 +- python/gtsam/preamble/base.h | 1 - python/gtsam/preamble/slam.h | 6 ------ python/gtsam/specializations/sfm.h | 4 ---- python/gtsam/specializations/slam.h | 8 -------- 8 files changed, 2 insertions(+), 58 deletions(-) diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 18bd88b52..d313883d9 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -564,9 +564,6 @@ class GTSAM_EXPORT Rot3 : public LieGroup { #endif }; - /// std::vector of Rot3s, mainly for wrapper - using Rot3Vector = std::vector >; - /** * [RQ] receives a 3 by 3 matrix and returns an upper triangular matrix R * and 3 rotation angles corresponding to the rotation matrix Q=Qz'*Qy'*Qx' diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index 705892e60..31057ec4b 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -29,13 +29,6 @@ class BinaryMeasurement { typedef gtsam::BinaryMeasurement BinaryMeasurementUnit3; typedef gtsam::BinaryMeasurement BinaryMeasurementRot3; -class BinaryMeasurementsUnit3 { - BinaryMeasurementsUnit3(); - 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 @@ -178,15 +171,6 @@ class ShonanAveraging3 { #include -class KeyPairDoubleMap { - KeyPairDoubleMap(); - KeyPairDoubleMap(const gtsam::KeyPairDoubleMap& other); - - size_t size() const; - bool empty() const; - void clear(); - size_t at(const pair& keypair) const; -}; class MFAS { MFAS(const gtsam::BinaryMeasurementsUnit3& relativeTranslations, diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index e044dd2c1..da06c0e26 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -268,24 +268,6 @@ void save2D(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& config, gtsam::noiseModel::Diagonal* model, string filename); -// std::vector::shared_ptr> -// Ignored by pybind -> will be List[BetweenFactorPose2] -class BetweenFactorPose2s { - BetweenFactorPose2s(); - size_t size() const; - gtsam::BetweenFactor* at(size_t i) const; - void push_back(const gtsam::BetweenFactor* factor); -}; -gtsam::BetweenFactorPose2s parse2DFactors(string filename); - -// std::vector::shared_ptr> -// Ignored by pybind -> will be List[BetweenFactorPose3] -class BetweenFactorPose3s { - BetweenFactorPose3s(); - size_t size() const; - gtsam::BetweenFactor* at(size_t i) const; - void push_back(const gtsam::BetweenFactor* factor); -}; gtsam::BetweenFactorPose3s parse3DFactors(string filename); pair load3D(string filename); @@ -323,7 +305,7 @@ virtual class KarcherMeanFactor : gtsam::NonlinearFactor { KarcherMeanFactor(const gtsam::KeyVector& keys); }; -gtsam::Rot3 FindKarcherMean(const gtsam::Rot3Vector& rotations); +gtsam::Rot3 FindKarcherMean(const std::vector& rotations); #include gtsam::noiseModel::Isotropic* ConvertNoiseModel(gtsam::noiseModel::Base* model, diff --git a/python/gtsam/gtsam.tpl b/python/gtsam/gtsam.tpl index bdeb2bccc..a974b246b 100644 --- a/python/gtsam/gtsam.tpl +++ b/python/gtsam/gtsam.tpl @@ -11,8 +11,8 @@ {include_boost} #include -#include #include +#include #include #include #include diff --git a/python/gtsam/preamble/base.h b/python/gtsam/preamble/base.h index 626b47ae4..a7effd1ca 100644 --- a/python/gtsam/preamble/base.h +++ b/python/gtsam/preamble/base.h @@ -12,5 +12,4 @@ */ PYBIND11_MAKE_OPAQUE(std::vector); - PYBIND11_MAKE_OPAQUE(std::vector); // JacobianVector diff --git a/python/gtsam/preamble/slam.h b/python/gtsam/preamble/slam.h index f7bf5863c..d07a75f6f 100644 --- a/python/gtsam/preamble/slam.h +++ b/python/gtsam/preamble/slam.h @@ -10,9 +10,3 @@ * Without this they will be automatically converted to a Python object, and all * mutations on Python side will not be reflected on C++. */ - -PYBIND11_MAKE_OPAQUE( - std::vector > >); -PYBIND11_MAKE_OPAQUE( - std::vector > >); -PYBIND11_MAKE_OPAQUE(gtsam::Rot3Vector); diff --git a/python/gtsam/specializations/sfm.h b/python/gtsam/specializations/sfm.h index 6de15217f..da8842eaf 100644 --- a/python/gtsam/specializations/sfm.h +++ b/python/gtsam/specializations/sfm.h @@ -10,7 +10,3 @@ * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python, * and saves one copy operation. */ - -py::bind_vector > >( - m_, "BinaryMeasurementsUnit3"); -py::bind_map(m_, "KeyPairDoubleMap"); diff --git a/python/gtsam/specializations/slam.h b/python/gtsam/specializations/slam.h index 6a439c370..da8842eaf 100644 --- a/python/gtsam/specializations/slam.h +++ b/python/gtsam/specializations/slam.h @@ -10,11 +10,3 @@ * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python, * and saves one copy operation. */ - -py::bind_vector< - std::vector>>>( - m_, "BetweenFactorPose3s"); -py::bind_vector< - std::vector>>>( - m_, "BetweenFactorPose2s"); -py::bind_vector(m_, "Rot3Vector"); From 042c236123a9a804676fc1bcbbd72ba82cdb6390 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 31 Jan 2022 14:08:13 -0500 Subject: [PATCH 06/20] update python tests --- gtsam/base/base.i | 14 +++++++- .../examples/TranslationAveragingExample.py | 11 +++--- python/gtsam/symbol_shorthand.py | 2 +- python/gtsam/tests/test_Cal3Fisheye.py | 13 ++++--- python/gtsam/tests/test_Cal3Unified.py | 13 ++++--- python/gtsam/tests/test_DecisionTreeFactor.py | 3 +- python/gtsam/tests/test_KarcherMeanFactor.py | 11 +++--- python/gtsam/tests/test_Pose2.py | 7 ++-- python/gtsam/tests/test_ShonanAveraging.py | 15 +++----- python/gtsam/tests/test_Sim3.py | 13 ++++--- .../gtsam/tests/test_TranslationRecovery.py | 6 ++-- python/gtsam/tests/test_Triangulation.py | 34 ++++++------------- 12 files changed, 63 insertions(+), 79 deletions(-) diff --git a/gtsam/base/base.i b/gtsam/base/base.i index f4184fa12..fbbd91553 100644 --- a/gtsam/base/base.i +++ b/gtsam/base/base.i @@ -41,10 +41,22 @@ class DSFMap { std::map sets(); }; +class IndexPairVector { + IndexPairVector(); + IndexPairVector(const gtsam::IndexPairVector& other); + + // common STL methods + size_t size() const; + bool empty() const; + void clear(); + + // structure specific methods + gtsam::IndexPair at(size_t i) const; + void push_back(gtsam::IndexPair key) const; +}; gtsam::IndexPairVector IndexPairSetAsArray(gtsam::IndexPairSet& set); - #include bool linear_independent(Matrix A, Matrix B, double tol); diff --git a/python/gtsam/examples/TranslationAveragingExample.py b/python/gtsam/examples/TranslationAveragingExample.py index 054b61126..a29b7e6a4 100644 --- a/python/gtsam/examples/TranslationAveragingExample.py +++ b/python/gtsam/examples/TranslationAveragingExample.py @@ -17,9 +17,8 @@ Date: September 2020 from collections import defaultdict from typing import List, Tuple -import numpy as np - import gtsam +import numpy as np from gtsam.examples import SFMdata # Hyperparameters for 1dsfm, values used from Kyle Wilson's code. @@ -59,7 +58,7 @@ def get_data() -> Tuple[gtsam.Values, List[gtsam.BinaryMeasurementUnit3]]: return wRc_values, i_iZj_list -def filter_outliers(w_iZj_list: gtsam.BinaryMeasurementsUnit3) -> gtsam.BinaryMeasurementsUnit3: +def filter_outliers(w_iZj_list: List[gtsam.BinaryMeasurementUnit3]) -> List[gtsam.BinaryMeasurementUnit3]: """Removes outliers from a list of Unit3 measurements that are the translation directions from camera i to camera j in the world frame.""" @@ -89,14 +88,14 @@ def filter_outliers(w_iZj_list: gtsam.BinaryMeasurementsUnit3) -> gtsam.BinaryMe avg_outlier_weights[keypair] += weight / len(outlier_weights) # Remove w_iZj that have weight greater than threshold, these are outliers. - w_iZj_inliers = gtsam.BinaryMeasurementsUnit3() + w_iZj_inliers = [] [w_iZj_inliers.append(w_iZj) for w_iZj in w_iZj_list if avg_outlier_weights[( w_iZj.key1(), w_iZj.key2())] < OUTLIER_WEIGHT_THRESHOLD] return w_iZj_inliers -def estimate_poses(i_iZj_list: gtsam.BinaryMeasurementsUnit3, +def estimate_poses(i_iZj_list: List[gtsam.BinaryMeasurementUnit3], wRc_values: gtsam.Values) -> gtsam.Values: """Estimate poses given rotations and normalized translation directions between cameras. @@ -112,7 +111,7 @@ def estimate_poses(i_iZj_list: gtsam.BinaryMeasurementsUnit3, """ # Convert the translation direction measurements to world frame using the rotations. - w_iZj_list = gtsam.BinaryMeasurementsUnit3() + w_iZj_list = [] for i_iZj in i_iZj_list: w_iZj = gtsam.Unit3(wRc_values.atRot3(i_iZj.key1()) .rotate(i_iZj.measured().point3())) diff --git a/python/gtsam/symbol_shorthand.py b/python/gtsam/symbol_shorthand.py index 748d36558..5f7866d18 100644 --- a/python/gtsam/symbol_shorthand.py +++ b/python/gtsam/symbol_shorthand.py @@ -1,4 +1,4 @@ # This trick is to allow direct import of sub-modules # without this, we can only do `from gtsam.gtsam.symbol_shorthand import X` # with this trick, we can do `from gtsam.symbol_shorthand import X` -from .gtsam.symbol_shorthand import * \ No newline at end of file +from .gtsam.symbol_shorthand import * diff --git a/python/gtsam/tests/test_Cal3Fisheye.py b/python/gtsam/tests/test_Cal3Fisheye.py index e54afc757..246ec19ee 100644 --- a/python/gtsam/tests/test_Cal3Fisheye.py +++ b/python/gtsam/tests/test_Cal3Fisheye.py @@ -11,11 +11,10 @@ Refactored: Roderick Koehle """ import unittest -import numpy as np - import gtsam -from gtsam.utils.test_case import GtsamTestCase +import numpy as np from gtsam.symbol_shorthand import K, L, P +from gtsam.utils.test_case import GtsamTestCase def ulp(ftype=np.float64): @@ -51,9 +50,9 @@ class TestCal3Fisheye(GtsamTestCase): camera1 = gtsam.PinholeCameraCal3Fisheye(pose1) camera2 = gtsam.PinholeCameraCal3Fisheye(pose2) cls.origin = np.array([0.0, 0.0, 0.0]) - cls.poses = gtsam.Pose3Vector([pose1, pose2]) - cls.cameras = gtsam.CameraSetCal3Fisheye([camera1, camera2]) - cls.measurements = gtsam.Point2Vector([k.project(cls.origin) for k in cls.cameras]) + cls.poses = [pose1, pose2] + cls.cameras = [camera1, camera2] + cls.measurements = [k.project(cls.origin) for k in cls.cameras] def test_Cal3Fisheye(self): K = gtsam.Cal3Fisheye() @@ -187,7 +186,7 @@ class TestCal3Fisheye(GtsamTestCase): def test_triangulation_rectify(self): """Estimate spatial point from image measurements using rectification""" - rectified = gtsam.Point2Vector([k.calibration().calibrate(pt) for k, pt in zip(self.cameras, self.measurements)]) + rectified = [k.calibration().calibrate(pt) for k, pt in zip(self.cameras, self.measurements)] shared_cal = gtsam.Cal3_S2() triangulated = gtsam.triangulatePoint3(self.poses, shared_cal, rectified, rank_tol=1e-9, optimize=False) self.gtsamAssertEquals(triangulated, self.origin) diff --git a/python/gtsam/tests/test_Cal3Unified.py b/python/gtsam/tests/test_Cal3Unified.py index bafbacfa4..da9f4a939 100644 --- a/python/gtsam/tests/test_Cal3Unified.py +++ b/python/gtsam/tests/test_Cal3Unified.py @@ -10,11 +10,10 @@ Author: Frank Dellaert & Duy Nguyen Ta (Python) """ import unittest -import numpy as np - import gtsam -from gtsam.utils.test_case import GtsamTestCase +import numpy as np from gtsam.symbol_shorthand import K, L, P +from gtsam.utils.test_case import GtsamTestCase class TestCal3Unified(GtsamTestCase): @@ -48,9 +47,9 @@ class TestCal3Unified(GtsamTestCase): camera1 = gtsam.PinholeCameraCal3Unified(pose1, cls.stereographic) camera2 = gtsam.PinholeCameraCal3Unified(pose2, cls.stereographic) cls.origin = np.array([0.0, 0.0, 0.0]) - cls.poses = gtsam.Pose3Vector([pose1, pose2]) - cls.cameras = gtsam.CameraSetCal3Unified([camera1, camera2]) - cls.measurements = gtsam.Point2Vector([k.project(cls.origin) for k in cls.cameras]) + cls.poses = [pose1, pose2] + cls.cameras = [camera1, camera2] + cls.measurements = [k.project(cls.origin) for k in cls.cameras] def test_Cal3Unified(self): K = gtsam.Cal3Unified() @@ -147,7 +146,7 @@ class TestCal3Unified(GtsamTestCase): def test_triangulation_rectify(self): """Estimate spatial point from image measurements using rectification""" - rectified = gtsam.Point2Vector([k.calibration().calibrate(pt) for k, pt in zip(self.cameras, self.measurements)]) + rectified = [k.calibration().calibrate(pt) for k, pt in zip(self.cameras, self.measurements)] shared_cal = gtsam.Cal3_S2() triangulated = gtsam.triangulatePoint3(self.poses, shared_cal, rectified, rank_tol=1e-9, optimize=False) self.gtsamAssertEquals(triangulated, self.origin) diff --git a/python/gtsam/tests/test_DecisionTreeFactor.py b/python/gtsam/tests/test_DecisionTreeFactor.py index 0499e7215..1b8598953 100644 --- a/python/gtsam/tests/test_DecisionTreeFactor.py +++ b/python/gtsam/tests/test_DecisionTreeFactor.py @@ -13,7 +13,8 @@ Author: Frank Dellaert import unittest -from gtsam import DecisionTreeFactor, DiscreteValues, DiscreteDistribution, Ordering +from gtsam import (DecisionTreeFactor, DiscreteDistribution, DiscreteValues, + Ordering) from gtsam.utils.test_case import GtsamTestCase diff --git a/python/gtsam/tests/test_KarcherMeanFactor.py b/python/gtsam/tests/test_KarcherMeanFactor.py index f4ec64283..9ec33ad8a 100644 --- a/python/gtsam/tests/test_KarcherMeanFactor.py +++ b/python/gtsam/tests/test_KarcherMeanFactor.py @@ -31,7 +31,7 @@ class TestKarcherMean(GtsamTestCase): def test_find(self): # Check that optimizing for Karcher mean (which minimizes Between distance) # gets correct result. - rotations = gtsam.Rot3Vector([R, R.inverse()]) + rotations = [R, R.inverse()] expected = Rot3() actual = gtsam.FindKarcherMean(rotations) self.gtsamAssertEquals(expected, actual) @@ -42,7 +42,7 @@ class TestKarcherMean(GtsamTestCase): a2Rb2 = Rot3() a3Rb3 = Rot3() - aRb_list = gtsam.Rot3Vector([a1Rb1, a2Rb2, a3Rb3]) + aRb_list = [a1Rb1, a2Rb2, a3Rb3] aRb_expected = Rot3() aRb = gtsam.FindKarcherMean(aRb_list) @@ -58,9 +58,7 @@ class TestKarcherMean(GtsamTestCase): graph = gtsam.NonlinearFactorGraph() R12 = R.compose(R.compose(R)) graph.add(gtsam.BetweenFactorRot3(1, 2, R12, MODEL)) - keys = gtsam.KeyVector() - keys.append(1) - keys.append(2) + keys = [1, 2] graph.add(gtsam.KarcherMeanFactorRot3(keys)) initial = gtsam.Values() @@ -69,8 +67,7 @@ class TestKarcherMean(GtsamTestCase): expected = Rot3() result = gtsam.GaussNewtonOptimizer(graph, initial).optimize() - actual = gtsam.FindKarcherMean( - gtsam.Rot3Vector([result.atRot3(1), result.atRot3(2)])) + actual = gtsam.FindKarcherMean([result.atRot3(1), result.atRot3(2)]) self.gtsamAssertEquals(expected, actual) self.gtsamAssertEquals( R12, result.atRot3(1).between(result.atRot3(2))) diff --git a/python/gtsam/tests/test_Pose2.py b/python/gtsam/tests/test_Pose2.py index e5ffbad7d..d70483482 100644 --- a/python/gtsam/tests/test_Pose2.py +++ b/python/gtsam/tests/test_Pose2.py @@ -10,10 +10,9 @@ Author: Frank Dellaert & Duy Nguyen Ta & John Lambert """ import unittest -import numpy as np - import gtsam -from gtsam import Point2, Point2Pairs, Pose2 +import numpy as np +from gtsam import Point2, Pose2 from gtsam.utils.test_case import GtsamTestCase @@ -55,7 +54,7 @@ class TestPose2(GtsamTestCase): ] # fmt: on - ab_pairs = Point2Pairs(list(zip(pts_a, pts_b))) + ab_pairs = list(zip(pts_a, pts_b)) bTa = gtsam.align(ab_pairs) aTb = bTa.inverse() assert aTb is not None diff --git a/python/gtsam/tests/test_ShonanAveraging.py b/python/gtsam/tests/test_ShonanAveraging.py index 19b9f8dc1..036ea401c 100644 --- a/python/gtsam/tests/test_ShonanAveraging.py +++ b/python/gtsam/tests/test_ShonanAveraging.py @@ -14,16 +14,9 @@ import unittest import gtsam import numpy as np -from gtsam import ( - BetweenFactorPose2, - LevenbergMarquardtParams, - Rot2, - Pose2, - ShonanAveraging2, - ShonanAveragingParameters2, - ShonanAveraging3, - ShonanAveragingParameters3, -) +from gtsam import (BetweenFactorPose2, LevenbergMarquardtParams, Pose2, Rot2, + ShonanAveraging2, ShonanAveraging3, + ShonanAveragingParameters2, ShonanAveragingParameters3) from gtsam.utils.test_case import GtsamTestCase DEFAULT_PARAMS = ShonanAveragingParameters3( @@ -183,7 +176,7 @@ class TestShonanAveraging(GtsamTestCase): shonan_params.setCertifyOptimality(True) noise_model = gtsam.noiseModel.Unit.Create(3) - between_factors = gtsam.BetweenFactorPose2s() + between_factors = [] for (i1, i2), i2Ri1 in i2Ri1_dict.items(): i2Ti1 = Pose2(i2Ri1, np.zeros(2)) between_factors.append( diff --git a/python/gtsam/tests/test_Sim3.py b/python/gtsam/tests/test_Sim3.py index c00a36435..c3fd9e909 100644 --- a/python/gtsam/tests/test_Sim3.py +++ b/python/gtsam/tests/test_Sim3.py @@ -12,10 +12,9 @@ Author: John Lambert import unittest from typing import List, Optional -import numpy as np - import gtsam -from gtsam import Point3, Pose3, Pose3Pairs, Rot3, Similarity3 +import numpy as np +from gtsam import Point3, Pose3, Rot3, Similarity3 from gtsam.utils.test_case import GtsamTestCase @@ -49,7 +48,7 @@ class TestSim3(GtsamTestCase): wToi_list = [wTo0, wTo1, wTo2] - we_pairs = Pose3Pairs(list(zip(wToi_list, eToi_list))) + we_pairs = list(zip(wToi_list, eToi_list)) # Recover the transformation wSe (i.e. world_S_egovehicle) wSe = Similarity3.Align(we_pairs) @@ -84,7 +83,7 @@ class TestSim3(GtsamTestCase): wToi_list = [wTo0, wTo1, wTo2] - we_pairs = Pose3Pairs(list(zip(wToi_list, eToi_list))) + we_pairs = list(zip(wToi_list, eToi_list)) # Recover the transformation wSe (i.e. world_S_egovehicle) wSe = Similarity3.Align(we_pairs) @@ -122,7 +121,7 @@ class TestSim3(GtsamTestCase): bTi_list = [bTi0, bTi1, bTi2, bTi3] - ab_pairs = Pose3Pairs(list(zip(aTi_list, bTi_list))) + ab_pairs = list(zip(aTi_list, bTi_list)) # Recover the transformation wSe (i.e. world_S_egovehicle) aSb = Similarity3.Align(ab_pairs) @@ -689,7 +688,7 @@ def align_poses_sim3(aTi_list: List[Pose3], bTi_list: List[Pose3]) -> Similarity assert len(aTi_list) == len(bTi_list) assert n_to_align >= 2, "SIM(3) alignment uses at least 2 frames" - ab_pairs = Pose3Pairs(list(zip(aTi_list, bTi_list))) + ab_pairs = list(zip(aTi_list, bTi_list)) aSb = Similarity3.Align(ab_pairs) diff --git a/python/gtsam/tests/test_TranslationRecovery.py b/python/gtsam/tests/test_TranslationRecovery.py index 0fb0518b6..962b3cc9a 100644 --- a/python/gtsam/tests/test_TranslationRecovery.py +++ b/python/gtsam/tests/test_TranslationRecovery.py @@ -1,9 +1,9 @@ from __future__ import print_function -import numpy as np import unittest import gtsam +import numpy as np """ Returns example pose values of 3 points A, B and C in the world frame """ def ExampleValues(): @@ -19,7 +19,7 @@ def ExampleValues(): """ Returns binary measurements for the points in the given edges.""" def SimulateMeasurements(gt_poses, graph_edges): - measurements = gtsam.BinaryMeasurementsUnit3() + measurements = [] for edge in graph_edges: Ta = gt_poses.atPose3(edge[0]).translation() Tb = gt_poses.atPose3(edge[1]).translation() @@ -34,7 +34,7 @@ class TestTranslationRecovery(unittest.TestCase): def test_constructor(self): """Construct from binary measurements.""" - algorithm = gtsam.TranslationRecovery(gtsam.BinaryMeasurementsUnit3()) + algorithm = gtsam.TranslationRecovery([]) self.assertIsInstance(algorithm, gtsam.TranslationRecovery) def test_run(self): diff --git a/python/gtsam/tests/test_Triangulation.py b/python/gtsam/tests/test_Triangulation.py index 0a258a0af..0de0f6d95 100644 --- a/python/gtsam/tests/test_Triangulation.py +++ b/python/gtsam/tests/test_Triangulation.py @@ -11,23 +11,11 @@ Authors: Frank Dellaert & Fan Jiang (Python) & Sushmita Warrier & John Lambert import unittest from typing import Iterable, List, Optional, Tuple, Union -import numpy as np - import gtsam -from gtsam import ( - Cal3_S2, - Cal3Bundler, - CameraSetCal3_S2, - CameraSetCal3Bundler, - PinholeCameraCal3_S2, - PinholeCameraCal3Bundler, - Point2, - Point2Vector, - Point3, - Pose3, - Pose3Vector, - Rot3, -) +import numpy as np +from gtsam import (Cal3_S2, Cal3Bundler, CameraSetCal3_S2, + CameraSetCal3Bundler, PinholeCameraCal3_S2, + PinholeCameraCal3Bundler, Point2, Point3, Pose3, Rot3) from gtsam.utils.test_case import GtsamTestCase UPRIGHT = Rot3.Ypr(-np.pi / 2, 0.0, -np.pi / 2) @@ -44,9 +32,7 @@ class TestTriangulationExample(GtsamTestCase): # create second camera 1 meter to the right of first camera pose2 = pose1.compose(Pose3(Rot3(), Point3(1, 0, 0))) # twoPoses - self.poses = Pose3Vector() - self.poses.append(pose1) - self.poses.append(pose2) + self.poses = [pose1, pose2] # landmark ~5 meters infront of camera self.landmark = Point3(5, 0.5, 1.2) @@ -58,7 +44,7 @@ class TestTriangulationExample(GtsamTestCase): cal_params: Iterable[Iterable[Union[int, float]]], camera_set: Optional[Union[CameraSetCal3Bundler, CameraSetCal3_S2]] = None, - ) -> Tuple[Point2Vector, Union[CameraSetCal3Bundler, CameraSetCal3_S2, + ) -> Tuple[List[Point2], Union[CameraSetCal3Bundler, CameraSetCal3_S2, List[Cal3Bundler], List[Cal3_S2]]]: """ Generate vector of measurements for given calibration and camera model. @@ -76,7 +62,7 @@ class TestTriangulationExample(GtsamTestCase): cameras = camera_set() else: cameras = [] - measurements = Point2Vector() + measurements = [] for k, pose in zip(cal_params, self.poses): K = calibration(*k) @@ -105,7 +91,7 @@ class TestTriangulationExample(GtsamTestCase): self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9) # Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) - measurements_noisy = Point2Vector() + measurements_noisy = [] measurements_noisy.append(measurements[0] - np.array([0.1, 0.5])) measurements_noisy.append(measurements[1] - np.array([-0.2, 0.3])) @@ -172,8 +158,8 @@ class TestTriangulationExample(GtsamTestCase): z2: Point2 = camera2.project(landmark) z3: Point2 = camera3.project(landmark) - poses = gtsam.Pose3Vector([pose1, pose2, pose3]) - measurements = Point2Vector([z1, z2, z3]) + poses = [pose1, pose2, pose3] + measurements = [z1, z2, z3] # noise free, so should give exactly the landmark actual = gtsam.triangulatePoint3(poses, From fe9c5718bbea01f523eb4afb344d5b82f948a484 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 31 Jan 2022 14:09:57 -0500 Subject: [PATCH 07/20] update CMake ignore list --- python/CMakeLists.txt | 27 ++------------------------- 1 file changed, 2 insertions(+), 25 deletions(-) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 85ddc7b6d..e666dced3 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -34,22 +34,7 @@ set(ignore gtsam::Point2 gtsam::Point3 gtsam::ISAM2ThresholdMapValue - gtsam::FactorIndices - gtsam::FactorIndexSet - gtsam::IndexPairSetMap - gtsam::IndexPairVector - gtsam::BetweenFactorPose2s - gtsam::BetweenFactorPose3s - gtsam::Point2Vector - gtsam::Point2Pairs - gtsam::Point3Pairs - gtsam::Pose3Pairs - gtsam::Pose3Vector - gtsam::Rot3Vector - gtsam::KeyVector - gtsam::BinaryMeasurementsUnit3 - gtsam::DiscreteKey - gtsam::KeyPairDoubleMap) + gtsam::DiscreteKey) set(interface_headers ${PROJECT_SOURCE_DIR}/gtsam/gtsam.i @@ -117,19 +102,11 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) gtsam::Point2 gtsam::Point3 gtsam::ISAM2ThresholdMapValue - gtsam::FactorIndices - gtsam::FactorIndexSet - gtsam::BetweenFactorPose3s - gtsam::Point2Vector - gtsam::Pose3Vector - gtsam::KeyVector gtsam::FixedLagSmootherKeyTimestampMapValue - gtsam::BinaryMeasurementsUnit3 gtsam::CameraSetCal3_S2 gtsam::CameraSetCal3Bundler gtsam::CameraSetCal3Unified - gtsam::CameraSetCal3Fisheye - gtsam::KeyPairDoubleMap) + gtsam::CameraSetCal3Fisheye) pybind_wrap(${GTSAM_PYTHON_UNSTABLE_TARGET} # target ${PROJECT_SOURCE_DIR}/gtsam_unstable/gtsam_unstable.i # interface_header From c68c986321d2b88fd17900da87c6302e8c236a5c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 31 Jan 2022 17:54:50 -0500 Subject: [PATCH 08/20] Fix IndexPairVector wrapping --- gtsam/base/base.i | 13 ------------- python/gtsam/preamble/base.h | 1 - 2 files changed, 14 deletions(-) diff --git a/gtsam/base/base.i b/gtsam/base/base.i index fbbd91553..212578736 100644 --- a/gtsam/base/base.i +++ b/gtsam/base/base.i @@ -41,19 +41,6 @@ class DSFMap { std::map sets(); }; -class IndexPairVector { - IndexPairVector(); - IndexPairVector(const gtsam::IndexPairVector& other); - - // common STL methods - size_t size() const; - bool empty() const; - void clear(); - - // structure specific methods - gtsam::IndexPair at(size_t i) const; - void push_back(gtsam::IndexPair key) const; -}; gtsam::IndexPairVector IndexPairSetAsArray(gtsam::IndexPairSet& set); diff --git a/python/gtsam/preamble/base.h b/python/gtsam/preamble/base.h index a7effd1ca..affb03ffa 100644 --- a/python/gtsam/preamble/base.h +++ b/python/gtsam/preamble/base.h @@ -11,5 +11,4 @@ * mutations on Python side will not be reflected on C++. */ -PYBIND11_MAKE_OPAQUE(std::vector); PYBIND11_MAKE_OPAQUE(std::vector); // JacobianVector From ef912eefd3689edf05133c45e3f74483970456e1 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 31 Jan 2022 18:53:39 -0500 Subject: [PATCH 09/20] minor cleanup --- python/gtsam/preamble/base.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/python/gtsam/preamble/base.h b/python/gtsam/preamble/base.h index affb03ffa..d07a75f6f 100644 --- a/python/gtsam/preamble/base.h +++ b/python/gtsam/preamble/base.h @@ -10,5 +10,3 @@ * Without this they will be automatically converted to a Python object, and all * mutations on Python side will not be reflected on C++. */ - -PYBIND11_MAKE_OPAQUE(std::vector); // JacobianVector From fbe9a21070c90454bfdf9e3c5dc43d6387e64ffc Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 31 Jan 2022 18:55:40 -0500 Subject: [PATCH 10/20] attempt to get custom factor tests passing --- gtsam/nonlinear/CustomFactor.cpp | 16 +++++++++---- gtsam/nonlinear/CustomFactor.h | 5 ++-- python/gtsam/tests/test_custom_factor.py | 29 ++++++++++++------------ 3 files changed, 29 insertions(+), 21 deletions(-) diff --git a/gtsam/nonlinear/CustomFactor.cpp b/gtsam/nonlinear/CustomFactor.cpp index e33caed6f..b8368c497 100644 --- a/gtsam/nonlinear/CustomFactor.cpp +++ b/gtsam/nonlinear/CustomFactor.cpp @@ -22,13 +22,14 @@ namespace gtsam { /* * Calculates the unwhitened error by invoking the callback functor (i.e. from Python). */ -Vector CustomFactor::unwhitenedError(const Values& x, boost::optional&> H) const { +Vector CustomFactor::unwhitenedError( + const Values &x, boost::optional&> H) const { if(this->active(x)) { if(H) { /* * In this case, we pass the raw pointer to the `std::vector` object directly to pybind. - * As the type `std::vector` has been marked as opaque in `preamble.h`, any changes in + * As the type `std::vector` has been marked as opaque in `preamble/base.h`, any changes in * Python will be immediately reflected on the C++ side. * * Example: @@ -43,13 +44,20 @@ Vector CustomFactor::unwhitenedError(const Values& x, boost::optionalerror_function_(*this, x, H.get_ptr()); + std::pair errorAndJacobian = + this->error_function_(*this, x, H.get_ptr()); + + Vector error = errorAndJacobian.first; + (*H) = errorAndJacobian.second; + + return error; } else { /* * In this case, we pass the a `nullptr` to pybind, and it will translate to `None` in Python. * Users can check for `None` in their callback to determine if the Jacobian is requested. */ - return this->error_function_(*this, x, nullptr); + auto errorAndJacobian = this->error_function_(*this, x, nullptr); + return errorAndJacobian.first; } } else { return Vector::Zero(this->dim()); diff --git a/gtsam/nonlinear/CustomFactor.h b/gtsam/nonlinear/CustomFactor.h index 615b5418e..6261636b5 100644 --- a/gtsam/nonlinear/CustomFactor.h +++ b/gtsam/nonlinear/CustomFactor.h @@ -35,7 +35,8 @@ class CustomFactor; * This is safe because this is passing a const pointer, and pybind11 will maintain the `std::vector` memory layout. * Thus the pointer will never be invalidated. */ -using CustomErrorFunction = std::function; +using CustomErrorFunction = std::function( + const CustomFactor &, const Values &, JacobianVector *)>; /** * @brief Custom factor that takes a std::function as the error @@ -77,7 +78,7 @@ public: * Calls the errorFunction closure, which is a std::function object * One can check if a derivative is needed in the errorFunction by checking the length of Jacobian array */ - Vector unwhitenedError(const Values &x, boost::optional &> H = boost::none) const override; + Vector unwhitenedError(const Values &x, boost::optional&> H = boost::none) const override; /** print */ void print(const std::string &s, diff --git a/python/gtsam/tests/test_custom_factor.py b/python/gtsam/tests/test_custom_factor.py index 4f0f33361..03e6917f0 100644 --- a/python/gtsam/tests/test_custom_factor.py +++ b/python/gtsam/tests/test_custom_factor.py @@ -8,13 +8,12 @@ See LICENSE for the license information CustomFactor unit tests. Author: Fan Jiang """ -from typing import List import unittest -from gtsam import Values, Pose2, CustomFactor - -import numpy as np +from typing import List import gtsam +import numpy as np +from gtsam import CustomFactor, JacobianFactor, Pose2, Values from gtsam.utils.test_case import GtsamTestCase @@ -24,17 +23,17 @@ class TestCustomFactor(GtsamTestCase): def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]): """Minimal error function stub""" - return np.array([1, 0, 0]) + return np.array([1, 0, 0]), H noise_model = gtsam.noiseModel.Unit.Create(3) - cf = CustomFactor(noise_model, gtsam.KeyVector([0]), error_func) + cf = CustomFactor(noise_model, [0], error_func) def test_new_keylist(self): """Test the creation of a new CustomFactor""" def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]): """Minimal error function stub""" - return np.array([1, 0, 0]) + return np.array([1, 0, 0]), H noise_model = gtsam.noiseModel.Unit.Create(3) cf = CustomFactor(noise_model, [0], error_func) @@ -47,7 +46,7 @@ class TestCustomFactor(GtsamTestCase): """Minimal error function with no Jacobian""" key0 = this.keys()[0] error = -v.atPose2(key0).localCoordinates(expected_pose) - return error + return error, H noise_model = gtsam.noiseModel.Unit.Create(3) cf = CustomFactor(noise_model, [0], error_func) @@ -81,10 +80,10 @@ class TestCustomFactor(GtsamTestCase): result = gT1.between(gT2) H[0] = -result.inverse().AdjointMap() H[1] = np.eye(3) - return error + return error, H noise_model = gtsam.noiseModel.Unit.Create(3) - cf = CustomFactor(noise_model, gtsam.KeyVector([0, 1]), error_func) + cf = CustomFactor(noise_model, [0, 1], error_func) v = Values() v.insert(0, gT1) v.insert(1, gT2) @@ -104,9 +103,9 @@ class TestCustomFactor(GtsamTestCase): gT1 = Pose2(1, 2, np.pi / 2) gT2 = Pose2(-1, 4, np.pi) - def error_func(this: CustomFactor, v: gtsam.Values, _: List[np.ndarray]): + def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]): """Minimal error function stub""" - return np.array([1, 0, 0]) + return np.array([1, 0, 0]), H noise_model = gtsam.noiseModel.Unit.Create(3) from gtsam.symbol_shorthand import X @@ -144,10 +143,10 @@ class TestCustomFactor(GtsamTestCase): result = gT1.between(gT2) H[0] = -result.inverse().AdjointMap() H[1] = np.eye(3) - return error + return error, H noise_model = gtsam.noiseModel.Unit.Create(3) - cf = CustomFactor(noise_model, gtsam.KeyVector([0, 1]), error_func) + cf = CustomFactor(noise_model, [0, 1], error_func) v = Values() v.insert(0, gT1) v.insert(1, gT2) @@ -182,7 +181,7 @@ class TestCustomFactor(GtsamTestCase): result = gT1.between(gT2) H[0] = -result.inverse().AdjointMap() H[1] = np.eye(3) - return error + return error, H noise_model = gtsam.noiseModel.Unit.Create(3) cf = CustomFactor(noise_model, [0, 1], error_func) From b60c5e3ab54f303e35e99535d0744d652698efa2 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 17 Jul 2022 16:05:59 -0400 Subject: [PATCH 11/20] undo CustomFactor changes --- gtsam/nonlinear/CustomFactor.cpp | 10 ++-------- gtsam/nonlinear/CustomFactor.h | 5 ++--- python/gtsam/tests/test_custom_factor.py | 14 +++++++------- 3 files changed, 11 insertions(+), 18 deletions(-) diff --git a/gtsam/nonlinear/CustomFactor.cpp b/gtsam/nonlinear/CustomFactor.cpp index b8368c497..4f86bc757 100644 --- a/gtsam/nonlinear/CustomFactor.cpp +++ b/gtsam/nonlinear/CustomFactor.cpp @@ -44,20 +44,14 @@ Vector CustomFactor::unwhitenedError( * return error * ``` */ - std::pair errorAndJacobian = - this->error_function_(*this, x, H.get_ptr()); + return this->error_function_(*this, x, H.get_ptr()); - Vector error = errorAndJacobian.first; - (*H) = errorAndJacobian.second; - - return error; } else { /* * In this case, we pass the a `nullptr` to pybind, and it will translate to `None` in Python. * Users can check for `None` in their callback to determine if the Jacobian is requested. */ - auto errorAndJacobian = this->error_function_(*this, x, nullptr); - return errorAndJacobian.first; + return this->error_function_(*this, x, nullptr); } } else { return Vector::Zero(this->dim()); diff --git a/gtsam/nonlinear/CustomFactor.h b/gtsam/nonlinear/CustomFactor.h index 6261636b5..615b5418e 100644 --- a/gtsam/nonlinear/CustomFactor.h +++ b/gtsam/nonlinear/CustomFactor.h @@ -35,8 +35,7 @@ class CustomFactor; * This is safe because this is passing a const pointer, and pybind11 will maintain the `std::vector` memory layout. * Thus the pointer will never be invalidated. */ -using CustomErrorFunction = std::function( - const CustomFactor &, const Values &, JacobianVector *)>; +using CustomErrorFunction = std::function; /** * @brief Custom factor that takes a std::function as the error @@ -78,7 +77,7 @@ public: * Calls the errorFunction closure, which is a std::function object * One can check if a derivative is needed in the errorFunction by checking the length of Jacobian array */ - Vector unwhitenedError(const Values &x, boost::optional&> H = boost::none) const override; + Vector unwhitenedError(const Values &x, boost::optional &> H = boost::none) const override; /** print */ void print(const std::string &s, diff --git a/python/gtsam/tests/test_custom_factor.py b/python/gtsam/tests/test_custom_factor.py index 03e6917f0..a3bb00384 100644 --- a/python/gtsam/tests/test_custom_factor.py +++ b/python/gtsam/tests/test_custom_factor.py @@ -33,7 +33,7 @@ class TestCustomFactor(GtsamTestCase): def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]): """Minimal error function stub""" - return np.array([1, 0, 0]), H + return np.array([1, 0, 0]) noise_model = gtsam.noiseModel.Unit.Create(3) cf = CustomFactor(noise_model, [0], error_func) @@ -46,7 +46,7 @@ class TestCustomFactor(GtsamTestCase): """Minimal error function with no Jacobian""" key0 = this.keys()[0] error = -v.atPose2(key0).localCoordinates(expected_pose) - return error, H + return error noise_model = gtsam.noiseModel.Unit.Create(3) cf = CustomFactor(noise_model, [0], error_func) @@ -80,7 +80,7 @@ class TestCustomFactor(GtsamTestCase): result = gT1.between(gT2) H[0] = -result.inverse().AdjointMap() H[1] = np.eye(3) - return error, H + return error noise_model = gtsam.noiseModel.Unit.Create(3) cf = CustomFactor(noise_model, [0, 1], error_func) @@ -103,9 +103,9 @@ class TestCustomFactor(GtsamTestCase): gT1 = Pose2(1, 2, np.pi / 2) gT2 = Pose2(-1, 4, np.pi) - def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]): + def error_func(this: CustomFactor, v: gtsam.Values, _: List[np.ndarray]): """Minimal error function stub""" - return np.array([1, 0, 0]), H + return np.array([1, 0, 0]) noise_model = gtsam.noiseModel.Unit.Create(3) from gtsam.symbol_shorthand import X @@ -143,7 +143,7 @@ class TestCustomFactor(GtsamTestCase): result = gT1.between(gT2) H[0] = -result.inverse().AdjointMap() H[1] = np.eye(3) - return error, H + return error noise_model = gtsam.noiseModel.Unit.Create(3) cf = CustomFactor(noise_model, [0, 1], error_func) @@ -181,7 +181,7 @@ class TestCustomFactor(GtsamTestCase): result = gT1.between(gT2) H[0] = -result.inverse().AdjointMap() H[1] = np.eye(3) - return error, H + return error noise_model = gtsam.noiseModel.Unit.Create(3) cf = CustomFactor(noise_model, [0, 1], error_func) From d6415deac4329c37c4995270feac0cd3085753f8 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 19 Jul 2022 12:13:20 -0400 Subject: [PATCH 12/20] minor updates to sfm.h preamble --- python/gtsam/preamble/sfm.h | 11 ++--------- 1 file changed, 2 insertions(+), 9 deletions(-) diff --git a/python/gtsam/preamble/sfm.h b/python/gtsam/preamble/sfm.h index bce2a750d..5ba48f1c8 100644 --- a/python/gtsam/preamble/sfm.h +++ b/python/gtsam/preamble/sfm.h @@ -11,12 +11,5 @@ * mutations on Python side will not be reflected on C++. */ -// Including can cause some serious hard-to-debug bugs!!! -// #include -#include - -PYBIND11_MAKE_OPAQUE( - std::vector); - -PYBIND11_MAKE_OPAQUE( - std::vector); +PYBIND11_MAKE_OPAQUE(std::vector); +PYBIND11_MAKE_OPAQUE(std::vector); From eb6e56a4f889426b4d07ab85052fa42498dd97d8 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 25 Jul 2022 17:20:09 -0400 Subject: [PATCH 13/20] Mark JacobianVector as opaque --- python/gtsam/preamble/base.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/python/gtsam/preamble/base.h b/python/gtsam/preamble/base.h index d07a75f6f..b1ed2ba14 100644 --- a/python/gtsam/preamble/base.h +++ b/python/gtsam/preamble/base.h @@ -10,3 +10,5 @@ * Without this they will be automatically converted to a Python object, and all * mutations on Python side will not be reflected on C++. */ + +PYBIND11_MAKE_OPAQUE(std::vector); // JacobianVector From 5b588f2ea70c159f1891d28df3d2eb3c1c219200 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 15 Jun 2023 16:30:10 -0400 Subject: [PATCH 14/20] update sfm module --- gtsam/sfm/sfm.i | 5 ++-- python/gtsam/preamble/sfm.h | 6 ----- python/gtsam/tests/test_DsfTrackGenerator.py | 28 ++++++++++---------- 3 files changed, 16 insertions(+), 23 deletions(-) diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index 402d29039..059aa21ed 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -6,15 +6,14 @@ namespace gtsam { #include class SfmTrack2d { - std::vector> measurements; + std::vector measurements; SfmTrack2d(); SfmTrack2d(const std::vector& measurements); size_t numberMeasurements() const; - pair measurement(size_t idx) const; + gtsam::SfmMeasurement measurement(size_t idx) const; pair siftIndex(size_t idx) const; void addMeasurement(size_t idx, const gtsam::Point2& m); - gtsam::SfmMeasurement measurement(size_t idx) const; bool hasUniqueCameras() const; Eigen::MatrixX2d measurementMatrix() const; Eigen::VectorXi indexVector() const; diff --git a/python/gtsam/preamble/sfm.h b/python/gtsam/preamble/sfm.h index 814c1df31..bd71c0817 100644 --- a/python/gtsam/preamble/sfm.h +++ b/python/gtsam/preamble/sfm.h @@ -11,9 +11,3 @@ * mutations on Python side will not be reflected on C++. */ -PYBIND11_MAKE_OPAQUE(std::vector); -PYBIND11_MAKE_OPAQUE(std::vector); -PYBIND11_MAKE_OPAQUE(std::vector); -PYBIND11_MAKE_OPAQUE( - std::vector); -PYBIND11_MAKE_OPAQUE(gtsam::gtsfm::MatchIndicesMap); diff --git a/python/gtsam/tests/test_DsfTrackGenerator.py b/python/gtsam/tests/test_DsfTrackGenerator.py index 284c3f5cc..282b384ec 100644 --- a/python/gtsam/tests/test_DsfTrackGenerator.py +++ b/python/gtsam/tests/test_DsfTrackGenerator.py @@ -5,13 +5,13 @@ Authors: John Lambert import unittest -import gtsam import numpy as np -from gtsam import (IndexPair, KeypointsVector, MatchIndicesMap, Point2, - SfmMeasurementVector, SfmTrack2d) from gtsam.gtsfm import Keypoints from gtsam.utils.test_case import GtsamTestCase +import gtsam +from gtsam import IndexPair, Point2, SfmTrack2d + class TestDsfTrackGenerator(GtsamTestCase): """Tests for DsfTrackGenerator.""" @@ -23,14 +23,14 @@ class TestDsfTrackGenerator(GtsamTestCase): kps_i1 = Keypoints(np.array([[50.0, 60], [70, 80], [90, 100]])) kps_i2 = Keypoints(np.array([[110.0, 120], [130, 140]])) - keypoints_list = KeypointsVector() + keypoints_list = [] keypoints_list.append(kps_i0) keypoints_list.append(kps_i1) keypoints_list.append(kps_i2) # For each image pair (i1,i2), we provide a (K,2) matrix # of corresponding image indices (k1,k2). - matches_dict = MatchIndicesMap() + matches_dict = {} matches_dict[IndexPair(0, 1)] = np.array([[0, 0], [1, 1]]) matches_dict[IndexPair(1, 2)] = np.array([[2, 0], [1, 1]]) @@ -81,16 +81,16 @@ class TestDsfTrackGenerator(GtsamTestCase): np.testing.assert_allclose(track2.indexVector(), [1, 2]) -class TestSfmTrack2d(GtsamTestCase): - """Tests for SfmTrack2d.""" +# class TestSfmTrack2d(GtsamTestCase): +# """Tests for SfmTrack2d.""" - def test_sfm_track_2d_constructor(self) -> None: - """Test construction of 2D SfM track.""" - measurements = SfmMeasurementVector() - measurements.append((0, Point2(10, 20))) - track = SfmTrack2d(measurements=measurements) - track.measurement(0) - assert track.numberMeasurements() == 1 +# def test_sfm_track_2d_constructor(self) -> None: +# """Test construction of 2D SfM track.""" +# measurements = [] +# measurements.append((0, Point2(10, 20))) +# track = SfmTrack2d(measurements=measurements) +# track.measurement(0) +# assert track.numberMeasurements() == 1 if __name__ == "__main__": From 31adb3ed453b4fb107693b0f63166d5de4a9474e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 15 Jun 2023 16:34:37 -0400 Subject: [PATCH 15/20] fix remaining python tests --- python/CMakeLists.txt | 1 + .../gtsam/examples/FixedLagSmootherExample.py | 7 +++-- python/gtsam/preamble/base.h | 2 -- python/gtsam/preamble/custom.h | 3 ++ python/gtsam/preamble/geometry.h | 2 -- python/gtsam/preamble/hybrid.h | 8 ----- python/gtsam/specializations/base.h | 1 - python/gtsam/specializations/custom.h | 5 +++- python/gtsam/specializations/inference.h | 2 -- .../tests/test_FixedLagSmootherExample.py | 9 +++--- python/gtsam/tests/test_HybridBayesNet.py | 19 +++++++----- python/gtsam/tests/test_custom_factor.py | 30 ++++++++++++------- python/gtsam/utils/plot.py | 3 +- 13 files changed, 49 insertions(+), 43 deletions(-) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 2557da237..684cf31a1 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -52,6 +52,7 @@ set(ignore gtsam::IndexPairVector gtsam::BetweenFactorPose2s gtsam::BetweenFactorPose3s + gtsam::FixedLagSmootherKeyTimestampMap gtsam::FixedLagSmootherKeyTimestampMapValue gtsam::Point2Vector gtsam::Point2Pairs diff --git a/python/gtsam/examples/FixedLagSmootherExample.py b/python/gtsam/examples/FixedLagSmootherExample.py index 99af0edcf..c56ebbe07 100644 --- a/python/gtsam/examples/FixedLagSmootherExample.py +++ b/python/gtsam/examples/FixedLagSmootherExample.py @@ -16,6 +16,7 @@ import numpy as np import gtsam import gtsam_unstable + def BatchFixedLagSmootherExample(): """ Runs a batch fixed smoother on an agent with two odometry @@ -31,7 +32,7 @@ def BatchFixedLagSmootherExample(): # that will be sent to the smoothers new_factors = gtsam.NonlinearFactorGraph() new_values = gtsam.Values() - new_timestamps = gtsam.FixedLagSmootherKeyTimestampMap() + new_timestamps = {} # Create a prior on the first pose, placing it at the origin prior_mean = gtsam.Pose2(0, 0, 0) @@ -39,7 +40,7 @@ def BatchFixedLagSmootherExample(): X1 = 0 new_factors.push_back(gtsam.PriorFactorPose2(X1, prior_mean, prior_noise)) new_values.insert(X1, prior_mean) - new_timestamps.insert((X1, 0.0)) + new_timestamps[X1] = 0.0 delta_time = 0.25 time = 0.25 @@ -49,7 +50,7 @@ def BatchFixedLagSmootherExample(): current_key = int(1000 * time) # assign current key to the current timestamp - new_timestamps.insert((current_key, time)) + new_timestamps[current_key] = time # Add a guess for this pose to the new values # Assume that the robot moves at 2 m/s. Position is time[s] * 2[m/s] diff --git a/python/gtsam/preamble/base.h b/python/gtsam/preamble/base.h index b1ed2ba14..d07a75f6f 100644 --- a/python/gtsam/preamble/base.h +++ b/python/gtsam/preamble/base.h @@ -10,5 +10,3 @@ * Without this they will be automatically converted to a Python object, and all * mutations on Python side will not be reflected on C++. */ - -PYBIND11_MAKE_OPAQUE(std::vector); // JacobianVector diff --git a/python/gtsam/preamble/custom.h b/python/gtsam/preamble/custom.h index d07a75f6f..d7bc6360f 100644 --- a/python/gtsam/preamble/custom.h +++ b/python/gtsam/preamble/custom.h @@ -10,3 +10,6 @@ * Without this they will be automatically converted to a Python object, and all * mutations on Python side will not be reflected on C++. */ + +// Added so that CustomFactor can pass the JacobianVector to the C++ side +PYBIND11_MAKE_OPAQUE(std::vector); // JacobianVector diff --git a/python/gtsam/preamble/geometry.h b/python/gtsam/preamble/geometry.h index 5c9df8ef8..40b841abc 100644 --- a/python/gtsam/preamble/geometry.h +++ b/python/gtsam/preamble/geometry.h @@ -11,8 +11,6 @@ * mutations on Python side will not be reflected on C++. */ -#include - PYBIND11_MAKE_OPAQUE(gtsam::CameraSet>); PYBIND11_MAKE_OPAQUE(gtsam::CameraSet>); PYBIND11_MAKE_OPAQUE(gtsam::CameraSet>); diff --git a/python/gtsam/preamble/hybrid.h b/python/gtsam/preamble/hybrid.h index 90a062d66..d07a75f6f 100644 --- a/python/gtsam/preamble/hybrid.h +++ b/python/gtsam/preamble/hybrid.h @@ -10,11 +10,3 @@ * Without this they will be automatically converted to a Python object, and all * mutations on Python side will not be reflected on C++. */ -#include - -// NOTE: Needed since we are including pybind11/stl.h. -#ifdef GTSAM_ALLOCATOR_TBB -PYBIND11_MAKE_OPAQUE(std::vector>); -#else -PYBIND11_MAKE_OPAQUE(std::vector); -#endif diff --git a/python/gtsam/specializations/base.h b/python/gtsam/specializations/base.h index 995753da1..22fe3beff 100644 --- a/python/gtsam/specializations/base.h +++ b/python/gtsam/specializations/base.h @@ -11,4 +11,3 @@ * and saves one copy operation. */ -py::bind_vector >(m_, "JacobianVector"); diff --git a/python/gtsam/specializations/custom.h b/python/gtsam/specializations/custom.h index d46ccdc66..cffb5115f 100644 --- a/python/gtsam/specializations/custom.h +++ b/python/gtsam/specializations/custom.h @@ -9,4 +9,7 @@ * interface, but without the `` copying mechanism. Combined * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python, * and saves one copy operation. - */ \ No newline at end of file + */ + +// Added so that CustomFactor can pass the JacobianVector to the C++ side +py::bind_vector >(m_, "JacobianVector"); diff --git a/python/gtsam/specializations/inference.h b/python/gtsam/specializations/inference.h index 9e23444ea..da8842eaf 100644 --- a/python/gtsam/specializations/inference.h +++ b/python/gtsam/specializations/inference.h @@ -10,5 +10,3 @@ * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python, * and saves one copy operation. */ - -py::bind_map>(m_, "__MapCharDouble"); diff --git a/python/gtsam/tests/test_FixedLagSmootherExample.py b/python/gtsam/tests/test_FixedLagSmootherExample.py index ddd2d49e4..412abe987 100644 --- a/python/gtsam/tests/test_FixedLagSmootherExample.py +++ b/python/gtsam/tests/test_FixedLagSmootherExample.py @@ -11,10 +11,11 @@ Author: Frank Dellaert & Duy Nguyen Ta (Python) import unittest import numpy as np +from gtsam.utils.test_case import GtsamTestCase import gtsam import gtsam_unstable -from gtsam.utils.test_case import GtsamTestCase + class TestFixedLagSmootherExample(GtsamTestCase): ''' @@ -36,7 +37,7 @@ class TestFixedLagSmootherExample(GtsamTestCase): # that will be sent to the smoothers new_factors = gtsam.NonlinearFactorGraph() new_values = gtsam.Values() - new_timestamps = gtsam.FixedLagSmootherKeyTimestampMap() + new_timestamps = {} # Create a prior on the first pose, placing it at the origin prior_mean = gtsam.Pose2(0, 0, 0) @@ -47,7 +48,7 @@ class TestFixedLagSmootherExample(GtsamTestCase): gtsam.PriorFactorPose2( X1, prior_mean, prior_noise)) new_values.insert(X1, prior_mean) - new_timestamps.insert((X1, 0.0)) + new_timestamps[X1] = 0.0 delta_time = 0.25 time = 0.25 @@ -77,7 +78,7 @@ class TestFixedLagSmootherExample(GtsamTestCase): current_key = int(1000 * time) # assign current key to the current timestamp - new_timestamps.insert((current_key, time)) + new_timestamps[current_key] = time # Add a guess for this pose to the new values # Assume that the robot moves at 2 m/s. Position is time[s] * diff --git a/python/gtsam/tests/test_HybridBayesNet.py b/python/gtsam/tests/test_HybridBayesNet.py index 01e1c5a5d..bc685dec6 100644 --- a/python/gtsam/tests/test_HybridBayesNet.py +++ b/python/gtsam/tests/test_HybridBayesNet.py @@ -17,8 +17,9 @@ import numpy as np from gtsam.symbol_shorthand import A, X from gtsam.utils.test_case import GtsamTestCase -from gtsam import (DiscreteConditional, DiscreteKeys, DiscreteValues, GaussianConditional, - GaussianMixture, HybridBayesNet, HybridValues, noiseModel, VectorValues) +from gtsam import (DiscreteConditional, DiscreteKeys, DiscreteValues, + GaussianConditional, GaussianMixture, HybridBayesNet, + HybridValues, VectorValues, noiseModel) class TestHybridBayesNet(GtsamTestCase): @@ -31,8 +32,8 @@ class TestHybridBayesNet(GtsamTestCase): # Create the continuous conditional I_1x1 = np.eye(1) - conditional = GaussianConditional.FromMeanAndStddev(X(0), 2 * I_1x1, X(1), [-4], - 5.0) + conditional = GaussianConditional.FromMeanAndStddev( + X(0), 2 * I_1x1, X(1), [-4], 5.0) # Create the noise models model0 = noiseModel.Diagonal.Sigmas([2.0]) @@ -47,8 +48,9 @@ class TestHybridBayesNet(GtsamTestCase): # Create hybrid Bayes net. bayesNet = HybridBayesNet() bayesNet.push_back(conditional) - bayesNet.push_back(GaussianMixture( - [X(1)], [], discrete_keys, [conditional0, conditional1])) + bayesNet.push_back( + GaussianMixture([X(1)], [], discrete_keys, + [conditional0, conditional1])) bayesNet.push_back(DiscreteConditional(Asia, "99/1")) # Create values at which to evaluate. @@ -76,7 +78,7 @@ class TestHybridBayesNet(GtsamTestCase): self.check_invariance(bayesNet.at(0).asGaussian(), continuous) self.check_invariance(bayesNet.at(0).asGaussian(), values) self.check_invariance(bayesNet.at(0), values) - + self.check_invariance(bayesNet.at(1), values) self.check_invariance(bayesNet.at(2).asDiscrete(), discrete) @@ -89,7 +91,8 @@ class TestHybridBayesNet(GtsamTestCase): self.assertTrue(probability >= 0.0) logProb = conditional.logProbability(values) self.assertAlmostEqual(probability, np.exp(logProb)) - expected = conditional.logNormalizationConstant() - conditional.error(values) + expected = conditional.logNormalizationConstant() - \ + conditional.error(values) self.assertAlmostEqual(logProb, expected) diff --git a/python/gtsam/tests/test_custom_factor.py b/python/gtsam/tests/test_custom_factor.py index a3bb00384..d19706c49 100644 --- a/python/gtsam/tests/test_custom_factor.py +++ b/python/gtsam/tests/test_custom_factor.py @@ -11,17 +11,20 @@ Author: Fan Jiang import unittest from typing import List -import gtsam import numpy as np -from gtsam import CustomFactor, JacobianFactor, Pose2, Values from gtsam.utils.test_case import GtsamTestCase +import gtsam +from gtsam import CustomFactor, Pose2, Values + class TestCustomFactor(GtsamTestCase): + def test_new(self): """Test the creation of a new CustomFactor""" - def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]): + def error_func(this: CustomFactor, v: gtsam.Values, + H: List[np.ndarray]): """Minimal error function stub""" return np.array([1, 0, 0]), H @@ -31,7 +34,8 @@ class TestCustomFactor(GtsamTestCase): def test_new_keylist(self): """Test the creation of a new CustomFactor""" - def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]): + def error_func(this: CustomFactor, v: gtsam.Values, + H: List[np.ndarray]): """Minimal error function stub""" return np.array([1, 0, 0]) @@ -42,7 +46,8 @@ class TestCustomFactor(GtsamTestCase): """Test if calling the factor works (only error)""" expected_pose = Pose2(1, 1, 0) - def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]) -> np.ndarray: + def error_func(this: CustomFactor, v: gtsam.Values, + H: List[np.ndarray]) -> np.ndarray: """Minimal error function with no Jacobian""" key0 = this.keys()[0] error = -v.atPose2(key0).localCoordinates(expected_pose) @@ -64,7 +69,8 @@ class TestCustomFactor(GtsamTestCase): expected = Pose2(2, 2, np.pi / 2) - def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]): + def error_func(this: CustomFactor, v: gtsam.Values, + H: List[np.ndarray]): """ the custom error function. One can freely use variables captured from the outside scope. Or the variables can be acquired by indexing `v`. @@ -103,7 +109,8 @@ class TestCustomFactor(GtsamTestCase): gT1 = Pose2(1, 2, np.pi / 2) gT2 = Pose2(-1, 4, np.pi) - def error_func(this: CustomFactor, v: gtsam.Values, _: List[np.ndarray]): + def error_func(this: CustomFactor, v: gtsam.Values, + _: List[np.ndarray]): """Minimal error function stub""" return np.array([1, 0, 0]) @@ -124,7 +131,8 @@ class TestCustomFactor(GtsamTestCase): expected = Pose2(2, 2, np.pi / 2) - def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]): + def error_func(this: CustomFactor, v: gtsam.Values, + H: List[np.ndarray]): """ Error function that mimics a BetweenFactor :param this: reference to the current CustomFactor being evaluated @@ -137,7 +145,8 @@ class TestCustomFactor(GtsamTestCase): gT1, gT2 = v.atPose2(key0), v.atPose2(key1) error = expected.localCoordinates(gT1.between(gT2)) - self.assertTrue(H is None) # Should be true if we only request the error + self.assertTrue( + H is None) # Should be true if we only request the error if H is not None: result = gT1.between(gT2) @@ -164,7 +173,8 @@ class TestCustomFactor(GtsamTestCase): expected = Pose2(2, 2, np.pi / 2) - def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]): + def error_func(this: CustomFactor, v: gtsam.Values, + H: List[np.ndarray]): """ Error function that mimics a BetweenFactor :param this: reference to the current CustomFactor being evaluated diff --git a/python/gtsam/utils/plot.py b/python/gtsam/utils/plot.py index 880c436e8..fe067e787 100644 --- a/python/gtsam/utils/plot.py +++ b/python/gtsam/utils/plot.py @@ -12,7 +12,6 @@ from mpl_toolkits.mplot3d import Axes3D # pylint: disable=unused-import import gtsam from gtsam import Marginals, Point2, Point3, Pose2, Pose3, Values - # For translation between a scaling of the uncertainty ellipse and the # percentage of inliers see discussion in # [PR 1067](https://github.com/borglab/gtsam/pull/1067) @@ -557,7 +556,7 @@ def plot_incremental_trajectory(fignum: int, axes = fig.axes[0] poses = gtsam.utilities.allPose3s(values) - keys = gtsam.KeyVector(poses.keys()) + keys = poses.keys() for key in keys[start:]: if values.exists(key): From 87d56aff9c49e6157648af528c81debe757bd5b1 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 15 Jun 2023 17:51:34 -0400 Subject: [PATCH 16/20] re-add classes for Matlab wrapper --- gtsam/geometry/Rot3.h | 3 ++ gtsam/geometry/geometry.i | 61 +++++++++++++++++++++++++++++++- gtsam/gtsam.i | 35 ++++++++++++++++++ gtsam/nonlinear/CustomFactor.cpp | 2 +- gtsam/sfm/sfm.i | 34 ++++++++++++++++++ gtsam/slam/slam.i | 21 ++++++++++- 6 files changed, 153 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 1a5a1ed96..2b9c5a45a 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -560,6 +560,9 @@ class GTSAM_EXPORT Rot3 : public LieGroup { #endif }; + /// std::vector of Rot3s, used in Matlab wrapper + using Rot3Vector = std::vector>; + /** * [RQ] receives a 3 by 3 matrix and returns an upper triangular matrix R * and 3 rotation angles corresponding to the rotation matrix Q=Qz'*Qy'*Qx' diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index c28d8dc56..4d265fca2 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -28,7 +28,41 @@ class Point2 { // enabling serialization functionality void serialize() const; }; - + +// Used in Matlab wrapper +class Point2Pairs { + Point2Pairs(); + size_t size() const; + bool empty() const; + gtsam::Point2Pair at(size_t n) const; + void push_back(const gtsam::Point2Pair& point_pair); +}; + +// std::vector +// Used in Matlab wrapper +class Point2Vector { + // Constructors + Point2Vector(); + Point2Vector(const gtsam::Point2Vector& v); + + // Capacity + size_t size() const; + size_t max_size() const; + void resize(size_t sz); + size_t capacity() const; + bool empty() const; + void reserve(size_t n); + + // Element access + gtsam::Point2 at(size_t n) const; + gtsam::Point2 front() const; + gtsam::Point2 back() const; + + // Modifiers + void assign(size_t n, const gtsam::Point2& u); + void push_back(const gtsam::Point2& x); + void pop_back(); +}; #include class StereoPoint2 { @@ -99,6 +133,14 @@ class Point3 { gtsam::Point3 normalize(const gtsam::Point3 &p, Eigen::Ref H) const; }; +// Used in Matlab wrapper +class Point3Pairs { + Point3Pairs(); + size_t size() const; + bool empty() const; + gtsam::Point3Pair at(size_t n) const; + void push_back(const gtsam::Point3Pair& point_pair); +}; #include class Rot2 { @@ -503,6 +545,23 @@ class Pose3 { void serialize() const; }; +// Used in Matlab wrapper +class Pose3Pairs { + Pose3Pairs(); + size_t size() const; + bool empty() const; + gtsam::Pose3Pair at(size_t n) const; + void push_back(const gtsam::Pose3Pair& pose_pair); +}; + +// Used in Matlab wrapper +class Pose3Vector { + Pose3Vector(); + size_t size() const; + bool empty() const; + gtsam::Pose3 at(size_t n) const; + void push_back(const gtsam::Pose3& pose); +}; #include class Unit3 { diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 3e18168e3..834d5a147 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -104,6 +104,41 @@ class KeyGroupMap { bool insert2(size_t key, int val); }; +// Actually a FastSet +// Used in Matlab wrapper +class FactorIndexSet { + FactorIndexSet(); + FactorIndexSet(const gtsam::FactorIndexSet& set); + + // common STL methods + size_t size() const; + bool empty() const; + void clear(); + + // structure specific methods + void insert(size_t factorIndex); + bool erase(size_t factorIndex); // returns true if value was removed + bool count(size_t factorIndex) const; // returns true if value exists +}; + +// Actually a vector +// Used in Matlab wrapper +class FactorIndices { + FactorIndices(); + FactorIndices(const gtsam::FactorIndices& other); + + // common STL methods + size_t size() const; + bool empty() const; + void clear(); + + // structure specific methods + size_t at(size_t i) const; + size_t front() const; + size_t back() const; + void push_back(size_t factorIndex) const; +}; + //************************************************************************* // Utilities //************************************************************************* diff --git a/gtsam/nonlinear/CustomFactor.cpp b/gtsam/nonlinear/CustomFactor.cpp index 52f49574c..a6415df20 100644 --- a/gtsam/nonlinear/CustomFactor.cpp +++ b/gtsam/nonlinear/CustomFactor.cpp @@ -28,7 +28,7 @@ Vector CustomFactor::unwhitenedError(const Values& x, OptionalMatrixVecType H) c if(H) { /* * In this case, we pass the raw pointer to the `std::vector` object directly to pybind. - * As the type `std::vector` has been marked as opaque in `preamble/base.h`, any changes in + * As the type `std::vector` has been marked as opaque in `preamble/custom.h`, any changes in * Python will be immediately reflected on the C++ side. * * Example: diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index 059aa21ed..2198692e8 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -99,6 +99,30 @@ typedef gtsam::BinaryMeasurement BinaryMeasurementUnit3; typedef gtsam::BinaryMeasurement BinaryMeasurementRot3; typedef gtsam::BinaryMeasurement BinaryMeasurementPoint3; +// Used in Matlab wrapper +class BinaryMeasurementsUnit3 { + BinaryMeasurementsUnit3(); + size_t size() const; + gtsam::BinaryMeasurement at(size_t idx) const; + void push_back(const gtsam::BinaryMeasurement& measurement); +}; + +// Used in Matlab wrapper +class BinaryMeasurementsPoint3 { + BinaryMeasurementsPoint3(); + size_t size() const; + gtsam::BinaryMeasurement at(size_t idx) const; + void push_back(const gtsam::BinaryMeasurement& measurement); +}; + +// Used in Matlab wrapper +class BinaryMeasurementsRot3 { + BinaryMeasurementsRot3(); + size_t size() const; + gtsam::BinaryMeasurement at(size_t idx) const; + void push_back(const gtsam::BinaryMeasurement& measurement); +}; + #include #include @@ -246,6 +270,16 @@ class ShonanAveraging3 { #include +// Used in Matlab wrapper +class KeyPairDoubleMap { + KeyPairDoubleMap(); + KeyPairDoubleMap(const gtsam::KeyPairDoubleMap& other); + + size_t size() const; + bool empty() const; + void clear(); + size_t at(const pair& keypair) const; +}; class MFAS { MFAS(const gtsam::BinaryMeasurementsUnit3& relativeTranslations, diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index dfdd63aa3..d35a87eee 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -250,6 +250,25 @@ void save2D(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& config, gtsam::noiseModel::Diagonal* model, string filename); +// std::vector::shared_ptr> +// Used in Matlab wrapper +class BetweenFactorPose2s { + BetweenFactorPose2s(); + size_t size() const; + gtsam::BetweenFactor* at(size_t i) const; + void push_back(const gtsam::BetweenFactor* factor); +}; +gtsam::BetweenFactorPose2s parse2DFactors(string filename); + +// std::vector::shared_ptr> +// Used in Matlab wrapper +class BetweenFactorPose3s { + BetweenFactorPose3s(); + size_t size() const; + gtsam::BetweenFactor* at(size_t i) const; + void push_back(const gtsam::BetweenFactor* factor); +}; + gtsam::BetweenFactorPose3s parse3DFactors(string filename); pair load3D(string filename); @@ -288,7 +307,7 @@ virtual class KarcherMeanFactor : gtsam::NonlinearFactor { KarcherMeanFactor(const gtsam::KeyVector& keys); }; -gtsam::Rot3 FindKarcherMean(const std::vector& rotations); +gtsam::Rot3 FindKarcherMean(const gtsam::Rot3Vector& rotations); #include gtsam::noiseModel::Isotropic* ConvertNoiseModel(gtsam::noiseModel::Base* model, From e70f8af448a3aaf214c5eb66f72fbf045db704ed Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 15 Jun 2023 17:55:33 -0400 Subject: [PATCH 17/20] leftover classes to re-add --- gtsam/base/base.i | 40 ++++++++++++++++++++++++++++++++++++++++ python/CMakeLists.txt | 1 + 2 files changed, 41 insertions(+) diff --git a/gtsam/base/base.i b/gtsam/base/base.i index d8cf9d8ea..31d09cb3d 100644 --- a/gtsam/base/base.i +++ b/gtsam/base/base.i @@ -41,9 +41,49 @@ class DSFMap { std::map sets(); }; +// Used in Matlab wrapper +class IndexPairSet { + IndexPairSet(); + // common STL methods + size_t size() const; + bool empty() const; + void clear(); + + // structure specific methods + void insert(gtsam::IndexPair key); + bool erase(gtsam::IndexPair key); // returns true if value was removed + bool count(gtsam::IndexPair key) const; // returns true if value exists +}; + +// Used in Matlab wrapper +class IndexPairVector { + IndexPairVector(); + IndexPairVector(const gtsam::IndexPairVector& other); + + // common STL methods + size_t size() const; + bool empty() const; + void clear(); + + // structure specific methods + gtsam::IndexPair at(size_t i) const; + void push_back(gtsam::IndexPair key) const; +}; gtsam::IndexPairVector IndexPairSetAsArray(gtsam::IndexPairSet& set); +// Used in Matlab wrapper +class IndexPairSetMap { + IndexPairSetMap(); + // common STL methods + size_t size() const; + bool empty() const; + void clear(); + + // structure specific methods + gtsam::IndexPairSet at(gtsam::IndexPair& key); +}; + #include #include bool linear_independent(Matrix A, Matrix B, double tol); diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 684cf31a1..2b2abf507 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -48,6 +48,7 @@ set(ignore gtsam::ISAM2ThresholdMapValue gtsam::FactorIndices gtsam::FactorIndexSet + gtsam::IndexPairSet gtsam::IndexPairSetMap gtsam::IndexPairVector gtsam::BetweenFactorPose2s From a763944249fbb9b2b5c3aadb53a38b2441d5868e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 15 Jun 2023 18:55:05 -0400 Subject: [PATCH 18/20] handle some TODOs --- gtsam/nonlinear/GncParams.h | 3 +-- gtsam/nonlinear/nonlinear.i | 8 ++++---- gtsam/sfm/sfm.i | 5 ++--- 3 files changed, 7 insertions(+), 9 deletions(-) diff --git a/gtsam/nonlinear/GncParams.h b/gtsam/nonlinear/GncParams.h index 10ac80663..d05e72ee2 100644 --- a/gtsam/nonlinear/GncParams.h +++ b/gtsam/nonlinear/GncParams.h @@ -73,8 +73,7 @@ class GncParams { double weightsTol = 1e-4; ///< If the weights are within weightsTol from being binary, stop iterating (only for TLS) Verbosity verbosity = SILENT; ///< Verbosity level - //TODO(Varun) replace IndexVector with vector once pybind11/stl.h is globally enabled. - /// Use IndexVector for inliers and outliers since it is fast + wrapping + /// Use IndexVector for inliers and outliers since it is fast using IndexVector = FastVector; ///< Slots in the factor graph corresponding to measurements that we know are inliers IndexVector knownInliers = IndexVector(); diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 19f4ae588..b47fd8a4c 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -305,8 +305,8 @@ virtual class GncParams { double relativeCostTol; double weightsTol; gtsam::This::Verbosity verbosity; - gtsam::KeyVector knownInliers; - gtsam::KeyVector knownOutliers; + gtsam::This::IndexVector knownInliers; + gtsam::This::IndexVector knownOutliers; void setLossType(const gtsam::GncLossType type); void setMaxIterations(const size_t maxIter); @@ -314,8 +314,8 @@ virtual class GncParams { void setRelativeCostTol(double value); void setWeightsTol(double value); void setVerbosityGNC(const gtsam::This::Verbosity value); - void setKnownInliers(const gtsam::KeyVector& knownIn); - void setKnownOutliers(const gtsam::KeyVector& knownOut); + void setKnownInliers(const gtsam::This::IndexVector& knownIn); + void setKnownOutliers(const gtsam::This::IndexVector& knownOut); void print(const string& str = "GncParams: ") const; enum Verbosity { diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index 2198692e8..a9deaa895 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -225,10 +225,9 @@ class ShonanAveraging3 { ShonanAveraging3(string g2oFile, const gtsam::ShonanAveragingParameters3& parameters); - // TODO(frank): deprecate once we land pybind wrapper - ShonanAveraging3(const gtsam::BetweenFactorPose3s& factors); ShonanAveraging3(const gtsam::BetweenFactorPose3s& factors, - const gtsam::ShonanAveragingParameters3& parameters); + const gtsam::ShonanAveragingParameters3& parameters = + gtsam::ShonanAveragingParameters3()); // Query properties size_t nrUnknowns() const; From 2340f1aa09a9db047819329b0457ca616e842bbb Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 16 Jun 2023 05:53:50 -0400 Subject: [PATCH 19/20] uncomment test --- python/gtsam/tests/test_DsfTrackGenerator.py | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/python/gtsam/tests/test_DsfTrackGenerator.py b/python/gtsam/tests/test_DsfTrackGenerator.py index 282b384ec..be6aa0796 100644 --- a/python/gtsam/tests/test_DsfTrackGenerator.py +++ b/python/gtsam/tests/test_DsfTrackGenerator.py @@ -81,16 +81,16 @@ class TestDsfTrackGenerator(GtsamTestCase): np.testing.assert_allclose(track2.indexVector(), [1, 2]) -# class TestSfmTrack2d(GtsamTestCase): -# """Tests for SfmTrack2d.""" +class TestSfmTrack2d(GtsamTestCase): + """Tests for SfmTrack2d.""" -# def test_sfm_track_2d_constructor(self) -> None: -# """Test construction of 2D SfM track.""" -# measurements = [] -# measurements.append((0, Point2(10, 20))) -# track = SfmTrack2d(measurements=measurements) -# track.measurement(0) -# assert track.numberMeasurements() == 1 + def test_sfm_track_2d_constructor(self) -> None: + """Test construction of 2D SfM track.""" + measurements = [] + measurements.append((0, Point2(10, 20))) + track = SfmTrack2d(measurements=measurements) + track.measurement(0) + assert track.numberMeasurements() == 1 if __name__ == "__main__": From 80a052d048211f9f369ca9602db340508ec58b78 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 16 Jun 2023 10:38:31 -0400 Subject: [PATCH 20/20] fix templating TODOs --- gtsam/geometry/geometry.i | 4 ++-- gtsam/inference/inference.i | 10 +++------ gtsam/sfm/sfm.i | 43 +++++++++---------------------------- timing/timeTest.cpp | 1 - 4 files changed, 15 insertions(+), 43 deletions(-) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 60d6a2bed..46172b774 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -1181,8 +1181,8 @@ class TriangulationParameters { const gtsam::SharedNoiseModel& noiseModel = nullptr); }; -// Templates appear not yet supported for free functions - issue raised at -// borglab/wrap#14 to add support +// Can be templated but overloaded for convenience. +// We can now call `triangulatePoint3` with any template type. // Cal3_S2 versions gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, diff --git a/gtsam/inference/inference.i b/gtsam/inference/inference.i index d4c90e356..b5413c747 100644 --- a/gtsam/inference/inference.i +++ b/gtsam/inference/inference.i @@ -176,13 +176,9 @@ class DotWriter { class VariableIndex { // Standard Constructors and Named Constructors VariableIndex(); - // TODO: Templetize constructor when wrap supports it - // template - // VariableIndex(const T& factorGraph, size_t nVariables); - // VariableIndex(const T& factorGraph); - VariableIndex(const gtsam::SymbolicFactorGraph& sfg); - VariableIndex(const gtsam::GaussianFactorGraph& gfg); - VariableIndex(const gtsam::NonlinearFactorGraph& fg); + template + VariableIndex(const T& factorGraph); VariableIndex(const gtsam::VariableIndex& other); // Testable diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index a9deaa895..930a0dd46 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -126,39 +126,16 @@ class BinaryMeasurementsRot3 { #include #include -// TODO(frank): copy/pasta below until we have integer template parameters in -// wrap! - -class ShonanAveragingParameters2 { - ShonanAveragingParameters2(const gtsam::LevenbergMarquardtParams& lm); - ShonanAveragingParameters2(const gtsam::LevenbergMarquardtParams& lm, +template +class ShonanAveragingParameters { + ShonanAveragingParameters(const gtsam::LevenbergMarquardtParams& lm); + ShonanAveragingParameters(const gtsam::LevenbergMarquardtParams& lm, string method); gtsam::LevenbergMarquardtParams getLMParams() const; void setOptimalityThreshold(double value); double getOptimalityThreshold() const; - void setAnchor(size_t index, const gtsam::Rot2& value); - pair getAnchor(); - void setAnchorWeight(double value); - double getAnchorWeight() const; - void setKarcherWeight(double value); - double getKarcherWeight() const; - void setGaugesWeight(double value); - double getGaugesWeight() const; - void setUseHuber(bool value); - bool getUseHuber() const; - void setCertifyOptimality(bool value); - bool getCertifyOptimality() const; -}; - -class ShonanAveragingParameters3 { - ShonanAveragingParameters3(const gtsam::LevenbergMarquardtParams& lm); - ShonanAveragingParameters3(const gtsam::LevenbergMarquardtParams& lm, - string method); - gtsam::LevenbergMarquardtParams getLMParams() const; - void setOptimalityThreshold(double value); - double getOptimalityThreshold() const; - void setAnchor(size_t index, const gtsam::Rot3& value); - pair getAnchor(); + void setAnchor(size_t index, const gtsam::This::Rot& value); + pair getAnchor(); void setAnchorWeight(double value); double getAnchorWeight() const; void setKarcherWeight(double value); @@ -171,6 +148,7 @@ class ShonanAveragingParameters3 { bool getCertifyOptimality() const; }; +// NOTE(Varun): Not templated because each class has specializations defined. class ShonanAveraging2 { ShonanAveraging2(string g2oFile); ShonanAveraging2(string g2oFile, @@ -217,10 +195,9 @@ class ShonanAveraging2 { }; class ShonanAveraging3 { - ShonanAveraging3( - const std::vector>& measurements, - const gtsam::ShonanAveragingParameters3& parameters = - gtsam::ShonanAveragingParameters3()); + ShonanAveraging3(const gtsam::This::Measurements& measurements, + const gtsam::ShonanAveragingParameters3& parameters = + gtsam::ShonanAveragingParameters3()); ShonanAveraging3(string g2oFile); ShonanAveraging3(string g2oFile, const gtsam::ShonanAveragingParameters3& parameters); diff --git a/timing/timeTest.cpp b/timing/timeTest.cpp index 9da3cfb2e..c450b9cf0 100644 --- a/timing/timeTest.cpp +++ b/timing/timeTest.cpp @@ -21,7 +21,6 @@ using namespace gtsam; int main(int argc, char *argv[]) { - // FIXME: ticPush_ does not exist { gttic_(top1); gttic_(sub1);