From 02e94730a634a56d411ccbaf75bbbcf267bdb9cf Mon Sep 17 00:00:00 2001 From: Sushmita Date: Fri, 27 Nov 2020 00:14:52 -0500 Subject: [PATCH 01/17] vector of cameras and triangulation function wrapped --- gtsam/geometry/triangulation.h | 5 ++ gtsam/gtsam.i | 34 +++++++++- python/CMakeLists.txt | 2 + python/gtsam/preamble.h | 2 + python/gtsam/specializations.h | 2 + python/gtsam/tests/test_Triangulation.py | 83 ++++++++++++++++++++---- 6 files changed, 116 insertions(+), 12 deletions(-) diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 6f6c645b8..01daab361 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -24,6 +24,8 @@ #include #include #include +#include +#include namespace gtsam { @@ -494,5 +496,8 @@ TriangulationResult triangulateSafe(const CameraSet& cameras, } } +typedef CameraSet> CameraSetCal3Bundler; +typedef CameraSet> CameraSetCal3_S2; + } // \namespace gtsam diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 880b1d4c7..47a09648a 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -1108,6 +1108,32 @@ typedef gtsam::PinholeCamera PinholeCameraCal3_S2; //typedef gtsam::PinholeCamera PinholeCameraCal3Unified; typedef gtsam::PinholeCamera PinholeCameraCal3Bundler; +class CameraSetCal3Bundler { + CameraSetCal3Bundler(); + + // common STL methods + size_t size() const; + bool empty() const; + void clear(); + + // structure specific methods + gtsam::PinholeCameraCal3Bundler at(size_t i) const; + void push_back(gtsam::PinholeCameraCal3Bundler& cam) const; +}; + +class CameraSetCal3_S2 { + CameraSetCal3_S2(); + + // common STL methods + size_t size() const; + bool empty() const; + void clear(); + + // structure specific methods + gtsam::PinholeCameraCal3_S2 at(size_t i) const; + void push_back(gtsam::PinholeCameraCal3_S2& cam) const; +}; + #include class StereoCamera { // Standard Constructors and Named Constructors @@ -1149,7 +1175,13 @@ gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, gtsam::Cal3Bundler* sharedCal, const gtsam::Point2Vector& measurements, double rank_tol, bool optimize); - +gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3_S2& cameras, + const gtsam::Point2Vector& measurements, double rank_tol, + bool optimize); +gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Bundler& cameras, + const gtsam::Point2Vector& measurements, double rank_tol, + bool optimize); + //************************************************************************* // Symbolic //************************************************************************* diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 00b537340..a318a483b 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -38,6 +38,8 @@ set(ignore gtsam::Pose3Vector gtsam::KeyVector gtsam::BinaryMeasurementsUnit3 + gtsam::CameraSetCal3Bundler + gtsam::CameraSetCal3_S2 gtsam::KeyPairDoubleMap) pybind_wrap(gtsam_py # target diff --git a/python/gtsam/preamble.h b/python/gtsam/preamble.h index 6166f615e..b56766c72 100644 --- a/python/gtsam/preamble.h +++ b/python/gtsam/preamble.h @@ -10,3 +10,5 @@ PYBIND11_MAKE_OPAQUE(std::vector); PYBIND11_MAKE_OPAQUE(std::vector > >); PYBIND11_MAKE_OPAQUE(std::vector > >); PYBIND11_MAKE_OPAQUE(std::vector); +PYBIND11_MAKE_OPAQUE(gtsam::CameraSet >); +PYBIND11_MAKE_OPAQUE(gtsam::CameraSet >); diff --git a/python/gtsam/specializations.h b/python/gtsam/specializations.h index cacad874c..431697aac 100644 --- a/python/gtsam/specializations.h +++ b/python/gtsam/specializations.h @@ -13,3 +13,5 @@ py::bind_vector > >(m_, "Bina py::bind_map(m_, "IndexPairSetMap"); py::bind_vector(m_, "IndexPairVector"); py::bind_map(m_, "KeyPairDoubleMap"); +py::bind_vector > >(m_, "CameraSetCal3_S2"); +py::bind_vector > >(m_, "CameraSetCal3Bundler"); diff --git a/python/gtsam/tests/test_Triangulation.py b/python/gtsam/tests/test_Triangulation.py index b43ad9b57..c358152ae 100644 --- a/python/gtsam/tests/test_Triangulation.py +++ b/python/gtsam/tests/test_Triangulation.py @@ -15,21 +15,24 @@ import numpy as np import gtsam as g from gtsam.utils.test_case import GtsamTestCase from gtsam import Cal3_S2, Cal3Bundler, Rot3, Pose3, \ - PinholeCameraCal3_S2, Point3, Point2Vector, Pose3Vector, triangulatePoint3 + PinholeCameraCal3_S2, Point3, Point2Vector, Pose3Vector, triangulatePoint3, CameraSetCal3_S2, CameraSetCal3Bundler, PinholeCameraCal3Bundler class TestVisualISAMExample(GtsamTestCase): - def test_TriangulationExample(self): - # Some common constants - sharedCal = Cal3_S2(1500, 1200, 0, 640, 480) - + def setUp(self): + # Set up two camera poses # Looking along X-axis, 1 meter above ground plane (x-y) upright = Rot3.Ypr(-np.pi / 2, 0., -np.pi / 2) - pose1 = Pose3(upright, Point3(0, 0, 1)) - camera1 = PinholeCameraCal3_S2(pose1, sharedCal) + self.pose1 = Pose3(upright, Point3(0, 0, 1)) # create second camera 1 meter to the right of first camera - pose2 = pose1.compose(Pose3(Rot3(), Point3(1, 0, 0))) - camera2 = PinholeCameraCal3_S2(pose2, sharedCal) + self.pose2 = self.pose1.compose(Pose3(Rot3(), Point3(1, 0, 0))) + + + def test_TriangulationExample(self): + # Some common constants + sharedCal = Cal3_S2(1500, 1200, 0, 640, 480) + camera1 = PinholeCameraCal3_S2(self.pose1, sharedCal) + camera2 = PinholeCameraCal3_S2(self.pose2, sharedCal) # landmark ~5 meters infront of camera landmark = Point3(5, 0.5, 1.2) @@ -42,8 +45,8 @@ class TestVisualISAMExample(GtsamTestCase): poses = Pose3Vector() measurements = Point2Vector() - poses.append(pose1) - poses.append(pose2) + poses.append(self.pose1) + poses.append(self.pose2) measurements.append(z1) measurements.append(z2) @@ -76,5 +79,63 @@ class TestVisualISAMExample(GtsamTestCase): # triangulated_landmark = triangulatePoint3(poses,bundlerCal, measurements, rank_tol, optimize) # self.gtsamAssertEquals(landmark, triangulated_landmark,1e-9) + def test_distinct_Ks(self): + K1 = Cal3_S2(1500, 1200, 0, 640, 480) + camera1 = PinholeCameraCal3_S2(self.pose1, K1) + + K2 = Cal3_S2(1600, 1300, 0, 650, 440) + camera2 = PinholeCameraCal3_S2(self.pose2, K2) + + # landmark ~5 meters infront of camera + landmark = Point3(5, 0.5, 1.2) + + # 1. Project two landmarks into two cameras and triangulate + z1 = camera1.project(landmark) + z2 = camera2.project(landmark) + # two cameras + measurements = Point2Vector() + cameras = CameraSetCal3_S2() + + measurements.append(z1) + measurements.append(z2) + cameras.append(camera1) + cameras.append(camera2) + + optimize = True + rank_tol = 1e-9 + + triangulated_landmark = triangulatePoint3(cameras, measurements, rank_tol, optimize) + self.gtsamAssertEquals(landmark, triangulated_landmark, 1e-2) + + def test_distinct_Ks_Bundler(self): + K1 = Cal3Bundler(1500, 0, 0, 640, 480) + camera1 = PinholeCameraCal3Bundler(self.pose1, K1) + + K2 = Cal3Bundler(1500, 0, 0, 640, 480) + camera2 = PinholeCameraCal3Bundler(self.pose2, K2) + + # landmark ~5 meters infront of camera + landmark = Point3(5, 0.5, 1.2) + + # 1. Project two landmarks into two cameras and triangulate + z1 = camera1.project(landmark) + z2 = camera2.project(landmark) + # two cameras + measurements = Point2Vector() + cameras = CameraSetCal3Bundler() + + measurements.append(z1) + measurements.append(z2) + cameras.append(camera1) + cameras.append(camera2) + + optimize = True + rank_tol = 1e-9 + + triangulated_landmark = triangulatePoint3(cameras, measurements, rank_tol, optimize) + self.gtsamAssertEquals(landmark, triangulated_landmark, 1e-2) + + + if __name__ == "__main__": unittest.main() From cc54b18fe508139e11e51517a5c7c29090161762 Mon Sep 17 00:00:00 2001 From: Sushmita Date: Sat, 28 Nov 2020 15:49:08 -0500 Subject: [PATCH 02/17] docs fixed and error threshold reduced --- gtsam/geometry/triangulation.h | 7 ++--- python/gtsam/tests/test_Triangulation.py | 34 +++++++++++++----------- 2 files changed, 22 insertions(+), 19 deletions(-) diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 01daab361..23ea7e50b 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -18,14 +18,14 @@ #pragma once -#include +#include +#include #include +#include #include #include #include #include -#include -#include namespace gtsam { @@ -496,6 +496,7 @@ TriangulationResult triangulateSafe(const CameraSet& cameras, } } +// Vector of Cameras - used by the Python/MATLAB wrapper typedef CameraSet> CameraSetCal3Bundler; typedef CameraSet> CameraSetCal3_S2; diff --git a/python/gtsam/tests/test_Triangulation.py b/python/gtsam/tests/test_Triangulation.py index c358152ae..96a6a5c4a 100644 --- a/python/gtsam/tests/test_Triangulation.py +++ b/python/gtsam/tests/test_Triangulation.py @@ -15,7 +15,9 @@ import numpy as np import gtsam as g from gtsam.utils.test_case import GtsamTestCase from gtsam import Cal3_S2, Cal3Bundler, Rot3, Pose3, \ - PinholeCameraCal3_S2, Point3, Point2Vector, Pose3Vector, triangulatePoint3, CameraSetCal3_S2, CameraSetCal3Bundler, PinholeCameraCal3Bundler + PinholeCameraCal3_S2, PinholeCameraCal3Bundler, Point3, \ + Point2Vector, Pose3Vector, triangulatePoint3, \ + CameraSetCal3_S2, CameraSetCal3Bundler class TestVisualISAMExample(GtsamTestCase): def setUp(self): @@ -80,62 +82,62 @@ class TestVisualISAMExample(GtsamTestCase): # self.gtsamAssertEquals(landmark, triangulated_landmark,1e-9) def test_distinct_Ks(self): + # two cameras K1 = Cal3_S2(1500, 1200, 0, 640, 480) camera1 = PinholeCameraCal3_S2(self.pose1, K1) K2 = Cal3_S2(1600, 1300, 0, 650, 440) camera2 = PinholeCameraCal3_S2(self.pose2, K2) + cameras = CameraSetCal3_S2() + cameras.append(camera1) + cameras.append(camera2) + # landmark ~5 meters infront of camera landmark = Point3(5, 0.5, 1.2) # 1. Project two landmarks into two cameras and triangulate z1 = camera1.project(landmark) z2 = camera2.project(landmark) - # two cameras + measurements = Point2Vector() - cameras = CameraSetCal3_S2() - measurements.append(z1) measurements.append(z2) - cameras.append(camera1) - cameras.append(camera2) optimize = True rank_tol = 1e-9 triangulated_landmark = triangulatePoint3(cameras, measurements, rank_tol, optimize) - self.gtsamAssertEquals(landmark, triangulated_landmark, 1e-2) + self.gtsamAssertEquals(landmark, triangulated_landmark, 1e-9) def test_distinct_Ks_Bundler(self): + # two cameras K1 = Cal3Bundler(1500, 0, 0, 640, 480) camera1 = PinholeCameraCal3Bundler(self.pose1, K1) - K2 = Cal3Bundler(1500, 0, 0, 640, 480) + K2 = Cal3Bundler(1600, 0, 0, 650, 440) camera2 = PinholeCameraCal3Bundler(self.pose2, K2) + cameras = CameraSetCal3Bundler() + cameras.append(camera1) + cameras.append(camera2) + # landmark ~5 meters infront of camera landmark = Point3(5, 0.5, 1.2) # 1. Project two landmarks into two cameras and triangulate z1 = camera1.project(landmark) z2 = camera2.project(landmark) - # two cameras - measurements = Point2Vector() - cameras = CameraSetCal3Bundler() + measurements = Point2Vector() measurements.append(z1) measurements.append(z2) - cameras.append(camera1) - cameras.append(camera2) optimize = True rank_tol = 1e-9 triangulated_landmark = triangulatePoint3(cameras, measurements, rank_tol, optimize) - self.gtsamAssertEquals(landmark, triangulated_landmark, 1e-2) - - + self.gtsamAssertEquals(landmark, triangulated_landmark, 1e-9) if __name__ == "__main__": unittest.main() From 362afce86443afffbf9e539c3b0b527187f7a14d Mon Sep 17 00:00:00 2001 From: Sushmita Date: Sat, 28 Nov 2020 17:34:04 -0500 Subject: [PATCH 03/17] moved landmark variable to setup --- python/gtsam/tests/test_Triangulation.py | 42 ++++++++++-------------- 1 file changed, 18 insertions(+), 24 deletions(-) diff --git a/python/gtsam/tests/test_Triangulation.py b/python/gtsam/tests/test_Triangulation.py index 96a6a5c4a..8e0af3094 100644 --- a/python/gtsam/tests/test_Triangulation.py +++ b/python/gtsam/tests/test_Triangulation.py @@ -29,6 +29,9 @@ class TestVisualISAMExample(GtsamTestCase): # create second camera 1 meter to the right of first camera self.pose2 = self.pose1.compose(Pose3(Rot3(), Point3(1, 0, 0))) + # landmark ~5 meters infront of camera + self.landmark = Point3(5, 0.5, 1.2) + def test_TriangulationExample(self): # Some common constants @@ -36,12 +39,9 @@ class TestVisualISAMExample(GtsamTestCase): camera1 = PinholeCameraCal3_S2(self.pose1, sharedCal) camera2 = PinholeCameraCal3_S2(self.pose2, sharedCal) - # landmark ~5 meters infront of camera - landmark = Point3(5, 0.5, 1.2) - # 1. Project two landmarks into two cameras and triangulate - z1 = camera1.project(landmark) - z2 = camera2.project(landmark) + z1 = camera1.project(self.landmark) + z2 = camera2.project(self.landmark) # twoPoses poses = Pose3Vector() @@ -56,7 +56,7 @@ class TestVisualISAMExample(GtsamTestCase): rank_tol = 1e-9 triangulated_landmark = triangulatePoint3(poses,sharedCal, measurements, rank_tol, optimize) - self.gtsamAssertEquals(landmark, triangulated_landmark,1e-9) + self.gtsamAssertEquals(self.landmark, triangulated_landmark,1e-9) # 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) measurements = Point2Vector() @@ -64,22 +64,22 @@ class TestVisualISAMExample(GtsamTestCase): measurements.append(z2 - np.array([-0.2, 0.3])) triangulated_landmark = triangulatePoint3(poses,sharedCal, measurements, rank_tol, optimize) - self.gtsamAssertEquals(landmark, triangulated_landmark,1e-2) + self.gtsamAssertEquals(self.landmark, triangulated_landmark,1e-2) # # # two Poses with Bundler Calibration # bundlerCal = Cal3Bundler(1500, 0, 0, 640, 480) # camera1 = PinholeCameraCal3Bundler(pose1, bundlerCal) # camera2 = PinholeCameraCal3Bundler(pose2, bundlerCal) # - # z1 = camera1.project(landmark) - # z2 = camera2.project(landmark) + # z1 = camera1.project(self.landmark) + # z2 = camera2.project(self.landmark) # # measurements = Point2Vector() # measurements.append(z1) # measurements.append(z2) # # triangulated_landmark = triangulatePoint3(poses,bundlerCal, measurements, rank_tol, optimize) - # self.gtsamAssertEquals(landmark, triangulated_landmark,1e-9) + # self.gtsamAssertEquals(self.landmark, triangulated_landmark,1e-9) def test_distinct_Ks(self): # two cameras @@ -93,12 +93,9 @@ class TestVisualISAMExample(GtsamTestCase): cameras.append(camera1) cameras.append(camera2) - # landmark ~5 meters infront of camera - landmark = Point3(5, 0.5, 1.2) - - # 1. Project two landmarks into two cameras and triangulate - z1 = camera1.project(landmark) - z2 = camera2.project(landmark) + # Project two landmarks into two cameras and triangulate + z1 = camera1.project(self.landmark) + z2 = camera2.project(self.landmark) measurements = Point2Vector() measurements.append(z1) @@ -108,7 +105,7 @@ class TestVisualISAMExample(GtsamTestCase): rank_tol = 1e-9 triangulated_landmark = triangulatePoint3(cameras, measurements, rank_tol, optimize) - self.gtsamAssertEquals(landmark, triangulated_landmark, 1e-9) + self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9) def test_distinct_Ks_Bundler(self): # two cameras @@ -122,12 +119,9 @@ class TestVisualISAMExample(GtsamTestCase): cameras.append(camera1) cameras.append(camera2) - # landmark ~5 meters infront of camera - landmark = Point3(5, 0.5, 1.2) - - # 1. Project two landmarks into two cameras and triangulate - z1 = camera1.project(landmark) - z2 = camera2.project(landmark) + # Project two landmarks into two cameras and triangulate + z1 = camera1.project(self.landmark) + z2 = camera2.project(self.landmark) measurements = Point2Vector() measurements.append(z1) @@ -137,7 +131,7 @@ class TestVisualISAMExample(GtsamTestCase): rank_tol = 1e-9 triangulated_landmark = triangulatePoint3(cameras, measurements, rank_tol, optimize) - self.gtsamAssertEquals(landmark, triangulated_landmark, 1e-9) + self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9) if __name__ == "__main__": unittest.main() From b4f1db5a013a297bd6f89bad22ef2b9b0a90d621 Mon Sep 17 00:00:00 2001 From: Sushmita Date: Sat, 28 Nov 2020 22:50:05 -0500 Subject: [PATCH 04/17] push back arguments changed to const reference --- gtsam/gtsam.i | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 28ed01111..f6c2da853 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -1119,7 +1119,7 @@ class CameraSetCal3Bundler { // structure specific methods gtsam::PinholeCameraCal3Bundler at(size_t i) const; - void push_back(gtsam::PinholeCameraCal3Bundler& cam) const; + void push_back(const gtsam::PinholeCameraCal3Bundler& cam); }; class CameraSetCal3_S2 { @@ -1132,7 +1132,7 @@ class CameraSetCal3_S2 { // structure specific methods gtsam::PinholeCameraCal3_S2 at(size_t i) const; - void push_back(gtsam::PinholeCameraCal3_S2& cam) const; + void push_back(const gtsam::PinholeCameraCal3_S2& cam); }; #include From e484a70b5f5aa588dae4c74130228092bf20e160 Mon Sep 17 00:00:00 2001 From: Sushmita Date: Sat, 28 Nov 2020 23:21:55 -0500 Subject: [PATCH 05/17] removed commented code --- python/gtsam/tests/test_Triangulation.py | 15 --------------- 1 file changed, 15 deletions(-) diff --git a/python/gtsam/tests/test_Triangulation.py b/python/gtsam/tests/test_Triangulation.py index 8e0af3094..e57fbb6ab 100644 --- a/python/gtsam/tests/test_Triangulation.py +++ b/python/gtsam/tests/test_Triangulation.py @@ -65,21 +65,6 @@ class TestVisualISAMExample(GtsamTestCase): triangulated_landmark = triangulatePoint3(poses,sharedCal, measurements, rank_tol, optimize) self.gtsamAssertEquals(self.landmark, triangulated_landmark,1e-2) - # - # # two Poses with Bundler Calibration - # bundlerCal = Cal3Bundler(1500, 0, 0, 640, 480) - # camera1 = PinholeCameraCal3Bundler(pose1, bundlerCal) - # camera2 = PinholeCameraCal3Bundler(pose2, bundlerCal) - # - # z1 = camera1.project(self.landmark) - # z2 = camera2.project(self.landmark) - # - # measurements = Point2Vector() - # measurements.append(z1) - # measurements.append(z2) - # - # triangulated_landmark = triangulatePoint3(poses,bundlerCal, measurements, rank_tol, optimize) - # self.gtsamAssertEquals(self.landmark, triangulated_landmark,1e-9) def test_distinct_Ks(self): # two cameras From 8fd2d9842426e452207b39daab0c8901243889d5 Mon Sep 17 00:00:00 2001 From: Sushmita Date: Tue, 1 Dec 2020 19:31:44 -0500 Subject: [PATCH 06/17] templated functions where possible --- gtsam/geometry/triangulation.h | 7 +++-- gtsam/gtsam.i | 34 +++++++++--------------- python/CMakeLists.txt | 2 -- python/gtsam/preamble.h | 4 +-- python/gtsam/specializations.h | 4 +-- python/gtsam/tests/test_Triangulation.py | 2 +- 6 files changed, 22 insertions(+), 31 deletions(-) diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 23ea7e50b..617d09da7 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -497,8 +497,11 @@ TriangulationResult triangulateSafe(const CameraSet& cameras, } // Vector of Cameras - used by the Python/MATLAB wrapper -typedef CameraSet> CameraSetCal3Bundler; -typedef CameraSet> CameraSetCal3_S2; +//typedef CameraSet> CameraSetCal3Bundler; +//typedef CameraSet> CameraSetCal3_S2; + +using CameraSetCal3Bundler = CameraSet>; +using CameraSetCal3_S2 = CameraSet>; } // \namespace gtsam diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index f6c2da853..a8b1cf4f4 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -1109,31 +1109,22 @@ typedef gtsam::PinholeCamera PinholeCameraCal3_S2; //typedef gtsam::PinholeCamera PinholeCameraCal3Unified; typedef gtsam::PinholeCamera PinholeCameraCal3Bundler; -class CameraSetCal3Bundler { - CameraSetCal3Bundler(); +template +class CameraSet { + CameraSet(); - // common STL methods - size_t size() const; - bool empty() const; - void clear(); + // // common STL methods + // size_t size() const; + // bool empty() const; + // void clear(); - // structure specific methods - gtsam::PinholeCameraCal3Bundler at(size_t i) const; - void push_back(const gtsam::PinholeCameraCal3Bundler& cam); + // // structure specific methods + // T at(size_t i) const; + void push_back(const T& cam); }; -class CameraSetCal3_S2 { - CameraSetCal3_S2(); - - // common STL methods - size_t size() const; - bool empty() const; - void clear(); - - // structure specific methods - gtsam::PinholeCameraCal3_S2 at(size_t i) const; - void push_back(const gtsam::PinholeCameraCal3_S2& cam); -}; +typedef gtsam::CameraSet CameraSetCal3_S2; +typedef gtsam::CameraSet CameraSetCal3Bundler; #include class StereoCamera { @@ -1166,7 +1157,6 @@ class StereoCamera { #include -// Templates appear not yet supported for free functions gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, gtsam::Cal3_S2* sharedCal, const gtsam::Point2Vector& measurements, double rank_tol, bool optimize); diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index a318a483b..00b537340 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -38,8 +38,6 @@ set(ignore gtsam::Pose3Vector gtsam::KeyVector gtsam::BinaryMeasurementsUnit3 - gtsam::CameraSetCal3Bundler - gtsam::CameraSetCal3_S2 gtsam::KeyPairDoubleMap) pybind_wrap(gtsam_py # target diff --git a/python/gtsam/preamble.h b/python/gtsam/preamble.h index b56766c72..92ad14797 100644 --- a/python/gtsam/preamble.h +++ b/python/gtsam/preamble.h @@ -10,5 +10,5 @@ PYBIND11_MAKE_OPAQUE(std::vector); PYBIND11_MAKE_OPAQUE(std::vector > >); PYBIND11_MAKE_OPAQUE(std::vector > >); 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 >); diff --git a/python/gtsam/specializations.h b/python/gtsam/specializations.h index 431697aac..8f97c2dae 100644 --- a/python/gtsam/specializations.h +++ b/python/gtsam/specializations.h @@ -13,5 +13,5 @@ py::bind_vector > >(m_, "Bina py::bind_map(m_, "IndexPairSetMap"); py::bind_vector(m_, "IndexPairVector"); py::bind_map(m_, "KeyPairDoubleMap"); -py::bind_vector > >(m_, "CameraSetCal3_S2"); -py::bind_vector > >(m_, "CameraSetCal3Bundler"); +// py::bind_vector > >(m_, "CameraSetCal3_S2"); +// py::bind_vector > >(m_, "CameraSetCal3Bundler"); diff --git a/python/gtsam/tests/test_Triangulation.py b/python/gtsam/tests/test_Triangulation.py index e57fbb6ab..a1adcd2c9 100644 --- a/python/gtsam/tests/test_Triangulation.py +++ b/python/gtsam/tests/test_Triangulation.py @@ -56,7 +56,7 @@ class TestVisualISAMExample(GtsamTestCase): rank_tol = 1e-9 triangulated_landmark = triangulatePoint3(poses,sharedCal, measurements, rank_tol, optimize) - self.gtsamAssertEquals(self.landmark, triangulated_landmark,1e-9) + self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9) # 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) measurements = Point2Vector() From 77b6595998a9fc52589d9e60001ac9d7ba2eeac8 Mon Sep 17 00:00:00 2001 From: Sushmita Date: Tue, 1 Dec 2020 21:56:05 -0500 Subject: [PATCH 07/17] removed push_back method from cameraset wrapper --- gtsam/gtsam.i | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index b7a49b9ac..954d74f05 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -1079,7 +1079,7 @@ class CameraSet { // // structure specific methods // T at(size_t i) const; - void push_back(const T& cam); + // void push_back(const T& cam); }; typedef gtsam::CameraSet CameraSetCal3_S2; From 2e3943346936f31de9649d503d8d9ebca3c9bc7d Mon Sep 17 00:00:00 2001 From: Sushmita Date: Tue, 1 Dec 2020 23:21:21 -0500 Subject: [PATCH 08/17] added utility functions and code cleanup --- gtsam/geometry/triangulation.h | 3 --- gtsam/gtsam.i | 16 ++++++++-------- python/gtsam/preamble.h | 2 -- python/gtsam/specializations.h | 2 -- python/gtsam/tests/test_Triangulation.py | 8 ++++---- 5 files changed, 12 insertions(+), 19 deletions(-) diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 617d09da7..1df9efd22 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -497,9 +497,6 @@ TriangulationResult triangulateSafe(const CameraSet& cameras, } // Vector of Cameras - used by the Python/MATLAB wrapper -//typedef CameraSet> CameraSetCal3Bundler; -//typedef CameraSet> CameraSetCal3_S2; - using CameraSetCal3Bundler = CameraSet>; using CameraSetCal3_S2 = CameraSet>; diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 954d74f05..8596106e4 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -1068,18 +1068,18 @@ typedef gtsam::PinholeCamera PinholeCameraCal3DS2; typedef gtsam::PinholeCamera PinholeCameraCal3Unified; typedef gtsam::PinholeCamera PinholeCameraCal3Bundler; -template +template class CameraSet { CameraSet(); - // // common STL methods - // size_t size() const; - // bool empty() const; - // void clear(); + // common STL methods + size_t size() const; + bool empty() const; + void clear(); - // // structure specific methods - // T at(size_t i) const; - // void push_back(const T& cam); + // structure specific methods + T at(size_t i) const; + void push_back(const T& cam); }; typedef gtsam::CameraSet CameraSetCal3_S2; diff --git a/python/gtsam/preamble.h b/python/gtsam/preamble.h index 92ad14797..6166f615e 100644 --- a/python/gtsam/preamble.h +++ b/python/gtsam/preamble.h @@ -10,5 +10,3 @@ PYBIND11_MAKE_OPAQUE(std::vector); PYBIND11_MAKE_OPAQUE(std::vector > >); PYBIND11_MAKE_OPAQUE(std::vector > >); PYBIND11_MAKE_OPAQUE(std::vector); -//PYBIND11_MAKE_OPAQUE(gtsam::CameraSet >); -//PYBIND11_MAKE_OPAQUE(gtsam::CameraSet >); diff --git a/python/gtsam/specializations.h b/python/gtsam/specializations.h index 8f97c2dae..cacad874c 100644 --- a/python/gtsam/specializations.h +++ b/python/gtsam/specializations.h @@ -13,5 +13,3 @@ py::bind_vector > >(m_, "Bina py::bind_map(m_, "IndexPairSetMap"); py::bind_vector(m_, "IndexPairVector"); py::bind_map(m_, "KeyPairDoubleMap"); -// py::bind_vector > >(m_, "CameraSetCal3_S2"); -// py::bind_vector > >(m_, "CameraSetCal3Bundler"); diff --git a/python/gtsam/tests/test_Triangulation.py b/python/gtsam/tests/test_Triangulation.py index a1adcd2c9..2a9851d04 100644 --- a/python/gtsam/tests/test_Triangulation.py +++ b/python/gtsam/tests/test_Triangulation.py @@ -75,8 +75,8 @@ class TestVisualISAMExample(GtsamTestCase): camera2 = PinholeCameraCal3_S2(self.pose2, K2) cameras = CameraSetCal3_S2() - cameras.append(camera1) - cameras.append(camera2) + cameras.push_back(camera1) + cameras.push_back(camera2) # Project two landmarks into two cameras and triangulate z1 = camera1.project(self.landmark) @@ -101,8 +101,8 @@ class TestVisualISAMExample(GtsamTestCase): camera2 = PinholeCameraCal3Bundler(self.pose2, K2) cameras = CameraSetCal3Bundler() - cameras.append(camera1) - cameras.append(camera2) + cameras.push_back(camera1) + cameras.push_back(camera2) # Project two landmarks into two cameras and triangulate z1 = camera1.project(self.landmark) From 7125179e4ba318094c4fb6c685e2c774675175ad Mon Sep 17 00:00:00 2001 From: Sushmita Date: Thu, 3 Dec 2020 20:58:51 -0500 Subject: [PATCH 09/17] added cmake and preamble --- gtsam/gtsam.i | 14 ++++++++------ python/CMakeLists.txt | 2 ++ python/gtsam/preamble.h | 2 ++ python/gtsam/specializations.h | 2 ++ 4 files changed, 14 insertions(+), 6 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 8596106e4..127a6fe45 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -1072,18 +1072,19 @@ template class CameraSet { CameraSet(); - // common STL methods - size_t size() const; - bool empty() const; - void clear(); + // // common STL methods + // size_t size() const; + // bool empty() const; + // void clear(); // structure specific methods T at(size_t i) const; void push_back(const T& cam); }; -typedef gtsam::CameraSet CameraSetCal3_S2; -typedef gtsam::CameraSet CameraSetCal3Bundler; +// typedefs added here for shorter type name and to enforce uniformity in naming conventions +//typedef gtsam::CameraSet CameraSetCal3_S2; +//typedef gtsam::CameraSet CameraSetCal3Bundler; #include class StereoCamera { @@ -1116,6 +1117,7 @@ class StereoCamera { #include +// Templates appear not yet supported for free functions - issue raised at borglab/wrap#14 to add support gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, gtsam::Cal3_S2* sharedCal, const gtsam::Point2Vector& measurements, double rank_tol, bool optimize); diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 00b537340..bfe08a76a 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -82,6 +82,8 @@ set(ignore gtsam::KeyVector gtsam::FixedLagSmootherKeyTimestampMapValue gtsam::BinaryMeasurementsUnit3 + gtsam::CameraSetCal3_S2 + gtsam::CameraSetCal3Bundler gtsam::KeyPairDoubleMap) pybind_wrap(gtsam_unstable_py # target diff --git a/python/gtsam/preamble.h b/python/gtsam/preamble.h index 6166f615e..c8a577431 100644 --- a/python/gtsam/preamble.h +++ b/python/gtsam/preamble.h @@ -10,3 +10,5 @@ PYBIND11_MAKE_OPAQUE(std::vector); PYBIND11_MAKE_OPAQUE(std::vector > >); PYBIND11_MAKE_OPAQUE(std::vector > >); PYBIND11_MAKE_OPAQUE(std::vector); +PYBIND11_MAKE_OPAQUE(gtsam::CameraSet >); +PYBIND11_MAKE_OPAQUE(gtsam::CameraSet >); \ No newline at end of file diff --git a/python/gtsam/specializations.h b/python/gtsam/specializations.h index cacad874c..431697aac 100644 --- a/python/gtsam/specializations.h +++ b/python/gtsam/specializations.h @@ -13,3 +13,5 @@ py::bind_vector > >(m_, "Bina py::bind_map(m_, "IndexPairSetMap"); py::bind_vector(m_, "IndexPairVector"); py::bind_map(m_, "KeyPairDoubleMap"); +py::bind_vector > >(m_, "CameraSetCal3_S2"); +py::bind_vector > >(m_, "CameraSetCal3Bundler"); From adf3ce557484f9a3937b3d1ae238ddbb16e60ebd Mon Sep 17 00:00:00 2001 From: Sushmita Date: Thu, 3 Dec 2020 20:59:16 -0500 Subject: [PATCH 10/17] moved measurement generation to separate function --- python/gtsam/tests/test_Triangulation.py | 118 ++++++++++------------- 1 file changed, 53 insertions(+), 65 deletions(-) diff --git a/python/gtsam/tests/test_Triangulation.py b/python/gtsam/tests/test_Triangulation.py index 2a9851d04..c04766804 100644 --- a/python/gtsam/tests/test_Triangulation.py +++ b/python/gtsam/tests/test_Triangulation.py @@ -18,104 +18,92 @@ from gtsam import Cal3_S2, Cal3Bundler, Rot3, Pose3, \ PinholeCameraCal3_S2, PinholeCameraCal3Bundler, Point3, \ Point2Vector, Pose3Vector, triangulatePoint3, \ CameraSetCal3_S2, CameraSetCal3Bundler +from numpy.core.records import array class TestVisualISAMExample(GtsamTestCase): + """ Tests for triangulation with shared and individual calibrations """ + def setUp(self): - # Set up two camera poses + """ Set up two camera poses """ # Looking along X-axis, 1 meter above ground plane (x-y) upright = Rot3.Ypr(-np.pi / 2, 0., -np.pi / 2) - self.pose1 = Pose3(upright, Point3(0, 0, 1)) + pose1 = Pose3(upright, Point3(0, 0, 1)) # create second camera 1 meter to the right of first camera - self.pose2 = self.pose1.compose(Pose3(Rot3(), Point3(1, 0, 0))) + pose2 = pose1.compose(Pose3(Rot3(), Point3(1, 0, 0))) + # twoPoses + self.poses = Pose3Vector() + self.poses.append(pose1) + self.poses.append(pose2) # landmark ~5 meters infront of camera self.landmark = Point3(5, 0.5, 1.2) + + def generate_measurements(self, calibration, camera_model, *cal_params): + """ Generate vector of measurements for given calibration and camera model + Args: + calibration: Camera calibration e.g. Cal3_S2 + camera_model: Camera model e.g. PinholeCameraCal3_S2 + cal_params: (list of) camera parameters e.g. K1, K2 + Returns: + vector of measurements and cameras + """ + + cameras = [] + measurements = Point2Vector() + for k, pose in zip(cal_params, self.poses): + K = calibration(*k) + camera = camera_model(pose, K) + cameras.append(camera) + z = camera.project(self.landmark) + measurements.append(z) + + return measurements, cameras def test_TriangulationExample(self): + """ Tests triangulation with shared Cal3_S2 calibration""" # Some common constants - sharedCal = Cal3_S2(1500, 1200, 0, 640, 480) - camera1 = PinholeCameraCal3_S2(self.pose1, sharedCal) - camera2 = PinholeCameraCal3_S2(self.pose2, sharedCal) + sharedCal = (1500, 1200, 0, 640, 480) + measurements, _ = self.generate_measurements(Cal3_S2, PinholeCameraCal3_S2, sharedCal, sharedCal) - # 1. Project two landmarks into two cameras and triangulate - z1 = camera1.project(self.landmark) - z2 = camera2.project(self.landmark) - - # twoPoses - poses = Pose3Vector() - measurements = Point2Vector() - - poses.append(self.pose1) - poses.append(self.pose2) - measurements.append(z1) - measurements.append(z2) - - optimize = True - rank_tol = 1e-9 - - triangulated_landmark = triangulatePoint3(poses,sharedCal, measurements, rank_tol, optimize) + triangulated_landmark = triangulatePoint3(self.poses,Cal3_S2(sharedCal), measurements, rank_tol=1e-9, optimize=True) self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9) # 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) - measurements = Point2Vector() - measurements.append(z1 - np.array([0.1, 0.5])) - measurements.append(z2 - np.array([-0.2, 0.3])) + measurements_noisy = Point2Vector() + measurements_noisy.append(measurements[0] - np.array([0.1, 0.5])) + measurements_noisy.append(measurements[1] - np.array([-0.2, 0.3])) - triangulated_landmark = triangulatePoint3(poses,sharedCal, measurements, rank_tol, optimize) + triangulated_landmark = triangulatePoint3(self.poses,Cal3_S2(sharedCal), measurements_noisy, rank_tol=1e-9, optimize=True) self.gtsamAssertEquals(self.landmark, triangulated_landmark,1e-2) def test_distinct_Ks(self): + """ Tests triangulation with individual Cal3_S2 calibrations """ # two cameras - K1 = Cal3_S2(1500, 1200, 0, 640, 480) - camera1 = PinholeCameraCal3_S2(self.pose1, K1) - - K2 = Cal3_S2(1600, 1300, 0, 650, 440) - camera2 = PinholeCameraCal3_S2(self.pose2, K2) + K1 = (1500, 1200, 0, 640, 480) + K2 = (1600, 1300, 0, 650, 440) + measurements, camera_list = self.generate_measurements(Cal3_S2, PinholeCameraCal3_S2, K1, K2) cameras = CameraSetCal3_S2() - cameras.push_back(camera1) - cameras.push_back(camera2) + for camera in camera_list: + cameras.append(camera) - # Project two landmarks into two cameras and triangulate - z1 = camera1.project(self.landmark) - z2 = camera2.project(self.landmark) - - measurements = Point2Vector() - measurements.append(z1) - measurements.append(z2) - - optimize = True - rank_tol = 1e-9 - - triangulated_landmark = triangulatePoint3(cameras, measurements, rank_tol, optimize) + triangulated_landmark = triangulatePoint3(cameras, measurements, rank_tol=1e-9, optimize=True) self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9) def test_distinct_Ks_Bundler(self): + """ Tests triangulation with individual Cal3Bundler calibrations""" # two cameras - K1 = Cal3Bundler(1500, 0, 0, 640, 480) - camera1 = PinholeCameraCal3Bundler(self.pose1, K1) - - K2 = Cal3Bundler(1600, 0, 0, 650, 440) - camera2 = PinholeCameraCal3Bundler(self.pose2, K2) + K1 = (1500, 0, 0, 640, 480) + K2 = (1600, 0, 0, 650, 440) + measurements, camera_list = self.generate_measurements(Cal3Bundler, PinholeCameraCal3Bundler, K1, K2) cameras = CameraSetCal3Bundler() - cameras.push_back(camera1) - cameras.push_back(camera2) + for camera in camera_list: + cameras.append(camera) - # Project two landmarks into two cameras and triangulate - z1 = camera1.project(self.landmark) - z2 = camera2.project(self.landmark) - - measurements = Point2Vector() - measurements.append(z1) - measurements.append(z2) - - optimize = True - rank_tol = 1e-9 - - triangulated_landmark = triangulatePoint3(cameras, measurements, rank_tol, optimize) + triangulated_landmark = triangulatePoint3(cameras, measurements, rank_tol=1e-9, optimize=True) self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9) if __name__ == "__main__": From 8be6890b206c761bb1f334f4f7ab6fcfa7ccbff0 Mon Sep 17 00:00:00 2001 From: Sushmita Date: Thu, 3 Dec 2020 21:10:10 -0500 Subject: [PATCH 11/17] code formatted --- gtsam/geometry/triangulation.h | 702 +-- gtsam/gtsam.i | 5372 +++++++++++----------- python/gtsam/preamble.h | 10 +- python/gtsam/specializations.h | 18 +- python/gtsam/tests/test_Triangulation.py | 14 +- 5 files changed, 3155 insertions(+), 2961 deletions(-) diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 1df9efd22..e22b35e56 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -27,49 +27,52 @@ #include #include -namespace gtsam { +namespace gtsam +{ -/// Exception thrown by triangulateDLT when SVD returns rank < 3 -class GTSAM_EXPORT TriangulationUnderconstrainedException: public std::runtime_error { -public: - TriangulationUnderconstrainedException() : - std::runtime_error("Triangulation Underconstrained Exception.") { - } -}; + /// Exception thrown by triangulateDLT when SVD returns rank < 3 + class GTSAM_EXPORT TriangulationUnderconstrainedException : public std::runtime_error + { + public: + TriangulationUnderconstrainedException() : std::runtime_error("Triangulation Underconstrained Exception.") + { + } + }; -/// Exception thrown by triangulateDLT when landmark is behind one or more of the cameras -class GTSAM_EXPORT TriangulationCheiralityException: public std::runtime_error { -public: - TriangulationCheiralityException() : - std::runtime_error( - "Triangulation Cheirality Exception: The resulting landmark is behind one or more cameras.") { - } -}; + /// Exception thrown by triangulateDLT when landmark is behind one or more of the cameras + class GTSAM_EXPORT TriangulationCheiralityException : public std::runtime_error + { + public: + TriangulationCheiralityException() : std::runtime_error( + "Triangulation Cheirality Exception: The resulting landmark is behind one or more cameras.") + { + } + }; -/** + /** * DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312 * @param projection_matrices Projection matrices (K*P^-1) * @param measurements 2D measurements * @param rank_tol SVD rank tolerance * @return Triangulated point, in homogeneous coordinates */ -GTSAM_EXPORT Vector4 triangulateHomogeneousDLT( - const std::vector>& projection_matrices, - const Point2Vector& measurements, double rank_tol = 1e-9); + GTSAM_EXPORT Vector4 triangulateHomogeneousDLT( + const std::vector> &projection_matrices, + const Point2Vector &measurements, double rank_tol = 1e-9); -/** + /** * DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312 * @param projection_matrices Projection matrices (K*P^-1) * @param measurements 2D measurements * @param rank_tol SVD rank tolerance * @return Triangulated Point3 */ -GTSAM_EXPORT Point3 triangulateDLT( - const std::vector>& projection_matrices, - const Point2Vector& measurements, - double rank_tol = 1e-9); + GTSAM_EXPORT Point3 triangulateDLT( + const std::vector> &projection_matrices, + const Point2Vector &measurements, + double rank_tol = 1e-9); -/** + /** * Create a factor graph with projection factors from poses and one calibration * @param poses Camera poses * @param sharedCal shared pointer to single calibration object (monocular only!) @@ -78,27 +81,29 @@ GTSAM_EXPORT Point3 triangulateDLT( * @param initialEstimate * @return graph and initial values */ -template -std::pair triangulationGraph( - const std::vector& poses, boost::shared_ptr sharedCal, - const Point2Vector& measurements, Key landmarkKey, - const Point3& initialEstimate) { - Values values; - values.insert(landmarkKey, initialEstimate); // Initial landmark value - NonlinearFactorGraph graph; - static SharedNoiseModel unit2(noiseModel::Unit::Create(2)); - static SharedNoiseModel prior_model(noiseModel::Isotropic::Sigma(6, 1e-6)); - for (size_t i = 0; i < measurements.size(); i++) { - const Pose3& pose_i = poses[i]; - typedef PinholePose Camera; - Camera camera_i(pose_i, sharedCal); - graph.emplace_shared > // - (camera_i, measurements[i], unit2, landmarkKey); + template + std::pair triangulationGraph( + const std::vector &poses, boost::shared_ptr sharedCal, + const Point2Vector &measurements, Key landmarkKey, + const Point3 &initialEstimate) + { + Values values; + values.insert(landmarkKey, initialEstimate); // Initial landmark value + NonlinearFactorGraph graph; + static SharedNoiseModel unit2(noiseModel::Unit::Create(2)); + static SharedNoiseModel prior_model(noiseModel::Isotropic::Sigma(6, 1e-6)); + for (size_t i = 0; i < measurements.size(); i++) + { + const Pose3 &pose_i = poses[i]; + typedef PinholePose Camera; + Camera camera_i(pose_i, sharedCal); + graph.emplace_shared> // + (camera_i, measurements[i], unit2, landmarkKey); + } + return std::make_pair(graph, values); } - return std::make_pair(graph, values); -} -/** + /** * Create a factor graph with projection factors from pinhole cameras * (each camera has a pose and calibration) * @param cameras pinhole cameras (monocular or stereo) @@ -107,35 +112,37 @@ std::pair triangulationGraph( * @param initialEstimate * @return graph and initial values */ -template -std::pair triangulationGraph( - const CameraSet& cameras, - const typename CAMERA::MeasurementVector& measurements, Key landmarkKey, - const Point3& initialEstimate) { - Values values; - values.insert(landmarkKey, initialEstimate); // Initial landmark value - NonlinearFactorGraph graph; - static SharedNoiseModel unit(noiseModel::Unit::Create( - traits::dimension)); - for (size_t i = 0; i < measurements.size(); i++) { - const CAMERA& camera_i = cameras[i]; - graph.emplace_shared > // - (camera_i, measurements[i], unit, landmarkKey); + template + std::pair triangulationGraph( + const CameraSet &cameras, + const typename CAMERA::MeasurementVector &measurements, Key landmarkKey, + const Point3 &initialEstimate) + { + Values values; + values.insert(landmarkKey, initialEstimate); // Initial landmark value + NonlinearFactorGraph graph; + static SharedNoiseModel unit(noiseModel::Unit::Create( + traits::dimension)); + for (size_t i = 0; i < measurements.size(); i++) + { + const CAMERA &camera_i = cameras[i]; + graph.emplace_shared> // + (camera_i, measurements[i], unit, landmarkKey); + } + return std::make_pair(graph, values); } - return std::make_pair(graph, values); -} -/** + /** * Optimize for triangulation * @param graph nonlinear factors for projection * @param values initial values * @param landmarkKey to refer to landmark * @return refined Point3 */ -GTSAM_EXPORT Point3 optimize(const NonlinearFactorGraph& graph, - const Values& values, Key landmarkKey); + GTSAM_EXPORT Point3 optimize(const NonlinearFactorGraph &graph, + const Values &values, Key landmarkKey); -/** + /** * Given an initial estimate , refine a point using measurements in several cameras * @param poses Camera poses * @param sharedCal shared pointer to single calibration object @@ -143,63 +150,69 @@ GTSAM_EXPORT Point3 optimize(const NonlinearFactorGraph& graph, * @param initialEstimate * @return refined Point3 */ -template -Point3 triangulateNonlinear(const std::vector& poses, - boost::shared_ptr sharedCal, - const Point2Vector& measurements, const Point3& initialEstimate) { + template + Point3 triangulateNonlinear(const std::vector &poses, + boost::shared_ptr sharedCal, + const Point2Vector &measurements, const Point3 &initialEstimate) + { - // Create a factor graph and initial values - Values values; - NonlinearFactorGraph graph; - boost::tie(graph, values) = triangulationGraph // - (poses, sharedCal, measurements, Symbol('p', 0), initialEstimate); + // Create a factor graph and initial values + Values values; + NonlinearFactorGraph graph; + boost::tie(graph, values) = triangulationGraph // + (poses, sharedCal, measurements, Symbol('p', 0), initialEstimate); - return optimize(graph, values, Symbol('p', 0)); -} + return optimize(graph, values, Symbol('p', 0)); + } -/** + /** * Given an initial estimate , refine a point using measurements in several cameras * @param cameras pinhole cameras (monocular or stereo) * @param measurements 2D measurements * @param initialEstimate * @return refined Point3 */ -template -Point3 triangulateNonlinear( - const CameraSet& cameras, - const typename CAMERA::MeasurementVector& measurements, const Point3& initialEstimate) { + template + Point3 triangulateNonlinear( + const CameraSet &cameras, + const typename CAMERA::MeasurementVector &measurements, const Point3 &initialEstimate) + { - // Create a factor graph and initial values - Values values; - NonlinearFactorGraph graph; - boost::tie(graph, values) = triangulationGraph // - (cameras, measurements, Symbol('p', 0), initialEstimate); + // Create a factor graph and initial values + Values values; + NonlinearFactorGraph graph; + boost::tie(graph, values) = triangulationGraph // + (cameras, measurements, Symbol('p', 0), initialEstimate); - return optimize(graph, values, Symbol('p', 0)); -} + return optimize(graph, values, Symbol('p', 0)); + } -/** + /** * Create a 3*4 camera projection matrix from calibration and pose. * Functor for partial application on calibration * @param pose The camera pose * @param cal The calibration * @return Returns a Matrix34 */ -template -struct CameraProjectionMatrix { - CameraProjectionMatrix(const CALIBRATION& calibration) : - K_(calibration.K()) { - } - Matrix34 operator()(const Pose3& pose) const { - return K_ * (pose.inverse().matrix()).block<3, 4>(0, 0); - } -private: - const Matrix3 K_; -public: - GTSAM_MAKE_ALIGNED_OPERATOR_NEW -}; + template + struct CameraProjectionMatrix + { + CameraProjectionMatrix(const CALIBRATION &calibration) : K_(calibration.K()) + { + } + Matrix34 operator()(const Pose3 &pose) const + { + return K_ * (pose.inverse().matrix()).block<3, 4>(0, 0); + } -/** + private: + const Matrix3 K_; + + public: + GTSAM_MAKE_ALIGNED_OPERATOR_NEW + }; + + /** * Function to triangulate 3D landmark point from an arbitrary number * of poses (at least 2) using the DLT. The function checks that the * resulting point lies in front of all cameras, but has no other checks @@ -211,43 +224,45 @@ public: * @param optimize Flag to turn on nonlinear refinement of triangulation * @return Returns a Point3 */ -template -Point3 triangulatePoint3(const std::vector& poses, - boost::shared_ptr sharedCal, - const Point2Vector& measurements, double rank_tol = 1e-9, - bool optimize = false) { + template + Point3 triangulatePoint3(const std::vector &poses, + boost::shared_ptr sharedCal, + const Point2Vector &measurements, double rank_tol = 1e-9, + bool optimize = false) + { - assert(poses.size() == measurements.size()); - if (poses.size() < 2) - throw(TriangulationUnderconstrainedException()); + assert(poses.size() == measurements.size()); + if (poses.size() < 2) + throw(TriangulationUnderconstrainedException()); - // construct projection matrices from poses & calibration - std::vector> projection_matrices; - CameraProjectionMatrix createP(*sharedCal); // partially apply - for(const Pose3& pose: poses) - projection_matrices.push_back(createP(pose)); + // construct projection matrices from poses & calibration + std::vector> projection_matrices; + CameraProjectionMatrix createP(*sharedCal); // partially apply + for (const Pose3 &pose : poses) + projection_matrices.push_back(createP(pose)); - // Triangulate linearly - Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol); + // Triangulate linearly + Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol); - // Then refine using non-linear optimization - if (optimize) - point = triangulateNonlinear // - (poses, sharedCal, measurements, point); + // Then refine using non-linear optimization + if (optimize) + point = triangulateNonlinear // + (poses, sharedCal, measurements, point); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION - // verify that the triangulated point lies in front of all cameras - for(const Pose3& pose: poses) { - const Point3& p_local = pose.transformTo(point); - if (p_local.z() <= 0) - throw(TriangulationCheiralityException()); - } + // verify that the triangulated point lies in front of all cameras + for (const Pose3 &pose : poses) + { + const Point3 &p_local = pose.transformTo(point); + if (p_local.z() <= 0) + throw(TriangulationCheiralityException()); + } #endif - return point; -} + return point; + } -/** + /** * Function to triangulate 3D landmark point from an arbitrary number * of poses (at least 2) using the DLT. This function is similar to the one * above, except that each camera has its own calibration. The function @@ -259,72 +274,76 @@ Point3 triangulatePoint3(const std::vector& poses, * @param optimize Flag to turn on nonlinear refinement of triangulation * @return Returns a Point3 */ -template -Point3 triangulatePoint3( - const CameraSet& cameras, - const typename CAMERA::MeasurementVector& measurements, double rank_tol = 1e-9, - bool optimize = false) { + template + Point3 triangulatePoint3( + const CameraSet &cameras, + const typename CAMERA::MeasurementVector &measurements, double rank_tol = 1e-9, + bool optimize = false) + { - size_t m = cameras.size(); - assert(measurements.size() == m); + size_t m = cameras.size(); + assert(measurements.size() == m); - if (m < 2) - throw(TriangulationUnderconstrainedException()); + if (m < 2) + throw(TriangulationUnderconstrainedException()); - // construct projection matrices from poses & calibration - std::vector> projection_matrices; - for(const CAMERA& camera: cameras) - projection_matrices.push_back( - CameraProjectionMatrix(camera.calibration())( - camera.pose())); - Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol); + // construct projection matrices from poses & calibration + std::vector> projection_matrices; + for (const CAMERA &camera : cameras) + projection_matrices.push_back( + CameraProjectionMatrix(camera.calibration())( + camera.pose())); + Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol); - // The n refine using non-linear optimization - if (optimize) - point = triangulateNonlinear(cameras, measurements, point); + // The n refine using non-linear optimization + if (optimize) + point = triangulateNonlinear(cameras, measurements, point); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION - // verify that the triangulated point lies in front of all cameras - for(const CAMERA& camera: cameras) { - const Point3& p_local = camera.pose().transformTo(point); - if (p_local.z() <= 0) - throw(TriangulationCheiralityException()); - } + // verify that the triangulated point lies in front of all cameras + for (const CAMERA &camera : cameras) + { + const Point3 &p_local = camera.pose().transformTo(point); + if (p_local.z() <= 0) + throw(TriangulationCheiralityException()); + } #endif - return point; -} + return point; + } -/// Pinhole-specific version -template -Point3 triangulatePoint3( - const CameraSet >& cameras, - const Point2Vector& measurements, double rank_tol = 1e-9, - bool optimize = false) { - return triangulatePoint3 > // - (cameras, measurements, rank_tol, optimize); -} + /// Pinhole-specific version + template + Point3 triangulatePoint3( + const CameraSet> &cameras, + const Point2Vector &measurements, double rank_tol = 1e-9, + bool optimize = false) + { + return triangulatePoint3> // + (cameras, measurements, rank_tol, optimize); + } -struct GTSAM_EXPORT TriangulationParameters { + struct GTSAM_EXPORT TriangulationParameters + { - double rankTolerance; ///< threshold to decide whether triangulation is result.degenerate - ///< (the rank is the number of singular values of the triangulation matrix which are larger than rankTolerance) - bool enableEPI; ///< if set to true, will refine triangulation using LM + double rankTolerance; ///< threshold to decide whether triangulation is result.degenerate + ///< (the rank is the number of singular values of the triangulation matrix which are larger than rankTolerance) + bool enableEPI; ///< if set to true, will refine triangulation using LM - /** + /** * if the landmark is triangulated at distance larger than this, * result is flagged as degenerate. */ - double landmarkDistanceThreshold; // + double landmarkDistanceThreshold; // - /** + /** * If this is nonnegative the we will check if the average reprojection error * is smaller than this threshold after triangulation, otherwise result is * flagged as degenerate. */ - double dynamicOutlierRejectionThreshold; + double dynamicOutlierRejectionThreshold; - /** + /** * Constructor * @param rankTol tolerance used to check if point triangulation is degenerate * @param enableEPI if true refine triangulation with embedded LM iterations @@ -332,173 +351,194 @@ struct GTSAM_EXPORT TriangulationParameters { * @param dynamicOutlierRejectionThreshold or if average error larger than this * */ - TriangulationParameters(const double _rankTolerance = 1.0, - const bool _enableEPI = false, double _landmarkDistanceThreshold = -1, - double _dynamicOutlierRejectionThreshold = -1) : - rankTolerance(_rankTolerance), enableEPI(_enableEPI), // - landmarkDistanceThreshold(_landmarkDistanceThreshold), // - dynamicOutlierRejectionThreshold(_dynamicOutlierRejectionThreshold) { - } + TriangulationParameters(const double _rankTolerance = 1.0, + const bool _enableEPI = false, double _landmarkDistanceThreshold = -1, + double _dynamicOutlierRejectionThreshold = -1) : rankTolerance(_rankTolerance), enableEPI(_enableEPI), // + landmarkDistanceThreshold(_landmarkDistanceThreshold), // + dynamicOutlierRejectionThreshold(_dynamicOutlierRejectionThreshold) + { + } - // stream to output - friend std::ostream &operator<<(std::ostream &os, - const TriangulationParameters& p) { - os << "rankTolerance = " << p.rankTolerance << std::endl; - os << "enableEPI = " << p.enableEPI << std::endl; - os << "landmarkDistanceThreshold = " << p.landmarkDistanceThreshold - << std::endl; - os << "dynamicOutlierRejectionThreshold = " - << p.dynamicOutlierRejectionThreshold << std::endl; - return os; - } + // stream to output + friend std::ostream &operator<<(std::ostream &os, + const TriangulationParameters &p) + { + os << "rankTolerance = " << p.rankTolerance << std::endl; + os << "enableEPI = " << p.enableEPI << std::endl; + os << "landmarkDistanceThreshold = " << p.landmarkDistanceThreshold + << std::endl; + os << "dynamicOutlierRejectionThreshold = " + << p.dynamicOutlierRejectionThreshold << std::endl; + return os; + } -private: + private: + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE &ar, const unsigned int version) + { + ar &BOOST_SERIALIZATION_NVP(rankTolerance); + ar &BOOST_SERIALIZATION_NVP(enableEPI); + ar &BOOST_SERIALIZATION_NVP(landmarkDistanceThreshold); + ar &BOOST_SERIALIZATION_NVP(dynamicOutlierRejectionThreshold); + } + }; - /// Serialization function - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(rankTolerance); - ar & BOOST_SERIALIZATION_NVP(enableEPI); - ar & BOOST_SERIALIZATION_NVP(landmarkDistanceThreshold); - ar & BOOST_SERIALIZATION_NVP(dynamicOutlierRejectionThreshold); - } -}; - -/** + /** * TriangulationResult is an optional point, along with the reasons why it is invalid. */ -class TriangulationResult: public boost::optional { - enum Status { - VALID, DEGENERATE, BEHIND_CAMERA, OUTLIER, FAR_POINT - }; - Status status_; - TriangulationResult(Status s) : - status_(s) { - } -public: + class TriangulationResult : public boost::optional + { + enum Status + { + VALID, + DEGENERATE, + BEHIND_CAMERA, + OUTLIER, + FAR_POINT + }; + Status status_; + TriangulationResult(Status s) : status_(s) + { + } - /** + public: + /** * Default constructor, only for serialization */ - TriangulationResult() {} + TriangulationResult() {} - /** + /** * Constructor */ - TriangulationResult(const Point3& p) : - status_(VALID) { - reset(p); - } - static TriangulationResult Degenerate() { - return TriangulationResult(DEGENERATE); - } - static TriangulationResult Outlier() { - return TriangulationResult(OUTLIER); - } - static TriangulationResult FarPoint() { - return TriangulationResult(FAR_POINT); - } - static TriangulationResult BehindCamera() { - return TriangulationResult(BEHIND_CAMERA); - } - bool valid() const { - return status_ == VALID; - } - bool degenerate() const { - return status_ == DEGENERATE; - } - bool outlier() const { - return status_ == OUTLIER; - } - bool farPoint() const { - return status_ == FAR_POINT; - } - bool behindCamera() const { - return status_ == BEHIND_CAMERA; - } - // stream to output - friend std::ostream &operator<<(std::ostream &os, - const TriangulationResult& result) { - if (result) - os << "point = " << *result << std::endl; - else - os << "no point, status = " << result.status_ << std::endl; - return os; - } - -private: - - /// Serialization function - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(status_); - } -}; - -/// triangulateSafe: extensive checking of the outcome -template -TriangulationResult triangulateSafe(const CameraSet& cameras, - const typename CAMERA::MeasurementVector& measured, - const TriangulationParameters& params) { - - size_t m = cameras.size(); - - // if we have a single pose the corresponding factor is uninformative - if (m < 2) - return TriangulationResult::Degenerate(); - else - // We triangulate the 3D position of the landmark - try { - Point3 point = triangulatePoint3(cameras, measured, - params.rankTolerance, params.enableEPI); - - // Check landmark distance and re-projection errors to avoid outliers - size_t i = 0; - double maxReprojError = 0.0; - for(const CAMERA& camera: cameras) { - const Pose3& pose = camera.pose(); - if (params.landmarkDistanceThreshold > 0 - && distance3(pose.translation(), point) - > params.landmarkDistanceThreshold) - return TriangulationResult::FarPoint(); -#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION - // verify that the triangulated point lies in front of all cameras - // Only needed if this was not yet handled by exception - const Point3& p_local = pose.transformTo(point); - if (p_local.z() <= 0) - return TriangulationResult::BehindCamera(); -#endif - // Check reprojection error - if (params.dynamicOutlierRejectionThreshold > 0) { - const Point2& zi = measured.at(i); - Point2 reprojectionError(camera.project(point) - zi); - maxReprojError = std::max(maxReprojError, reprojectionError.norm()); - } - i += 1; - } - // Flag as degenerate if average reprojection error is too large - if (params.dynamicOutlierRejectionThreshold > 0 - && maxReprojError > params.dynamicOutlierRejectionThreshold) - return TriangulationResult::Outlier(); - - // all good! - return TriangulationResult(point); - } catch (TriangulationUnderconstrainedException&) { - // This exception is thrown if - // 1) There is a single pose for triangulation - this should not happen because we checked the number of poses before - // 2) The rank of the matrix used for triangulation is < 3: rotation-only, parallel cameras (or motion towards the landmark) - return TriangulationResult::Degenerate(); - } catch (TriangulationCheiralityException&) { - // point is behind one of the cameras: can be the case of close-to-parallel cameras or may depend on outliers - return TriangulationResult::BehindCamera(); + TriangulationResult(const Point3 &p) : status_(VALID) + { + reset(p); + } + static TriangulationResult Degenerate() + { + return TriangulationResult(DEGENERATE); + } + static TriangulationResult Outlier() + { + return TriangulationResult(OUTLIER); + } + static TriangulationResult FarPoint() + { + return TriangulationResult(FAR_POINT); + } + static TriangulationResult BehindCamera() + { + return TriangulationResult(BEHIND_CAMERA); + } + bool valid() const + { + return status_ == VALID; + } + bool degenerate() const + { + return status_ == DEGENERATE; + } + bool outlier() const + { + return status_ == OUTLIER; + } + bool farPoint() const + { + return status_ == FAR_POINT; + } + bool behindCamera() const + { + return status_ == BEHIND_CAMERA; + } + // stream to output + friend std::ostream &operator<<(std::ostream &os, + const TriangulationResult &result) + { + if (result) + os << "point = " << *result << std::endl; + else + os << "no point, status = " << result.status_ << std::endl; + return os; } -} -// Vector of Cameras - used by the Python/MATLAB wrapper -using CameraSetCal3Bundler = CameraSet>; -using CameraSetCal3_S2 = CameraSet>; + private: + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE &ar, const unsigned int version) + { + ar &BOOST_SERIALIZATION_NVP(status_); + } + }; -} // \namespace gtsam + /// triangulateSafe: extensive checking of the outcome + template + TriangulationResult triangulateSafe(const CameraSet &cameras, + const typename CAMERA::MeasurementVector &measured, + const TriangulationParameters ¶ms) + { + size_t m = cameras.size(); + + // if we have a single pose the corresponding factor is uninformative + if (m < 2) + return TriangulationResult::Degenerate(); + else + // We triangulate the 3D position of the landmark + try + { + Point3 point = triangulatePoint3(cameras, measured, + params.rankTolerance, params.enableEPI); + + // Check landmark distance and re-projection errors to avoid outliers + size_t i = 0; + double maxReprojError = 0.0; + for (const CAMERA &camera : cameras) + { + const Pose3 &pose = camera.pose(); + if (params.landmarkDistanceThreshold > 0 && distance3(pose.translation(), point) > params.landmarkDistanceThreshold) + return TriangulationResult::FarPoint(); +#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION + // verify that the triangulated point lies in front of all cameras + // Only needed if this was not yet handled by exception + const Point3 &p_local = pose.transformTo(point); + if (p_local.z() <= 0) + return TriangulationResult::BehindCamera(); +#endif + // Check reprojection error + if (params.dynamicOutlierRejectionThreshold > 0) + { + const Point2 &zi = measured.at(i); + Point2 reprojectionError(camera.project(point) - zi); + maxReprojError = std::max(maxReprojError, reprojectionError.norm()); + } + i += 1; + } + // Flag as degenerate if average reprojection error is too large + if (params.dynamicOutlierRejectionThreshold > 0 && maxReprojError > params.dynamicOutlierRejectionThreshold) + return TriangulationResult::Outlier(); + + // all good! + return TriangulationResult(point); + } + catch (TriangulationUnderconstrainedException &) + { + // This exception is thrown if + // 1) There is a single pose for triangulation - this should not happen because we checked the number of poses before + // 2) The rank of the matrix used for triangulation is < 3: rotation-only, parallel cameras (or motion towards the landmark) + return TriangulationResult::Degenerate(); + } + catch (TriangulationCheiralityException &) + { + // point is behind one of the cameras: can be the case of close-to-parallel cameras or may depend on outliers + return TriangulationResult::BehindCamera(); + } + } + + // Vector of Cameras - used by the Python/MATLAB wrapper + using CameraSetCal3Bundler = CameraSet>; + using CameraSetCal3_S2 = CameraSet>; + +} // namespace gtsam diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 127a6fe45..1ab2425ec 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -118,1140 +118,1176 @@ * - TODO: Add generalized serialization support via boost.serialization with hooks to matlab save/load */ -namespace gtsam { +namespace gtsam +{ // Actually a FastList #include -class KeyList { - KeyList(); - KeyList(const gtsam::KeyList& other); + class KeyList + { + KeyList(); + KeyList(const gtsam::KeyList &other); - // Note: no print function + // Note: no print function - // common STL methods - size_t size() const; - bool empty() const; - void clear(); + // common STL methods + size_t size() const; + bool empty() const; + void clear(); - // structure specific methods - size_t front() const; - size_t back() const; - void push_back(size_t key); - void push_front(size_t key); - void pop_back(); - void pop_front(); - void sort(); - void remove(size_t key); + // structure specific methods + size_t front() const; + size_t back() const; + void push_back(size_t key); + void push_front(size_t key); + void pop_back(); + void pop_front(); + void sort(); + void remove(size_t key); - void serialize() const; -}; + void serialize() const; + }; -// Actually a FastSet -class KeySet { - KeySet(); - KeySet(const gtsam::KeySet& set); - KeySet(const gtsam::KeyVector& vector); - KeySet(const gtsam::KeyList& list); + // Actually a FastSet + class KeySet + { + KeySet(); + KeySet(const gtsam::KeySet &set); + KeySet(const gtsam::KeyVector &vector); + KeySet(const gtsam::KeyList &list); - // Testable - void print(string s) const; - bool equals(const gtsam::KeySet& other) const; + // Testable + void print(string s) const; + bool equals(const gtsam::KeySet &other) const; - // common STL methods - size_t size() const; - bool empty() const; - void clear(); + // common STL methods + size_t size() const; + bool empty() const; + void clear(); - // structure specific methods - void insert(size_t key); - void merge(const gtsam::KeySet& other); - bool erase(size_t key); // returns true if value was removed - bool count(size_t key) const; // returns true if value exists + // structure specific methods + void insert(size_t key); + void merge(const gtsam::KeySet &other); + bool erase(size_t key); // returns true if value was removed + bool count(size_t key) const; // returns true if value exists - void serialize() const; -}; + void serialize() const; + }; -// Actually a vector -class KeyVector { - KeyVector(); - KeyVector(const gtsam::KeyVector& other); + // Actually a vector + class KeyVector + { + KeyVector(); + KeyVector(const gtsam::KeyVector &other); - // Note: no print function + // Note: no print function - // common STL methods - size_t size() const; - bool empty() const; - void clear(); + // 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; + // 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; -}; + void serialize() const; + }; -// Actually a FastMap -class KeyGroupMap { - KeyGroupMap(); + // Actually a FastMap + class KeyGroupMap + { + KeyGroupMap(); - // Note: no print function + // Note: no print function - // common STL methods - size_t size() const; - bool empty() const; - void clear(); + // common STL methods + size_t size() const; + bool empty() const; + void clear(); - // structure specific methods - size_t at(size_t key) const; - int erase(size_t key); - bool insert2(size_t key, int val); -}; + // structure specific methods + size_t at(size_t key) const; + int erase(size_t key); + bool insert2(size_t key, int val); + }; -// Actually a FastSet -class FactorIndexSet { - FactorIndexSet(); - FactorIndexSet(const gtsam::FactorIndexSet& set); + // Actually a FastSet + class FactorIndexSet + { + FactorIndexSet(); + FactorIndexSet(const gtsam::FactorIndexSet &set); - // common STL methods - size_t size() const; - bool empty() const; - void clear(); + // 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 -}; + // 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); + // Actually a vector + class FactorIndices + { + FactorIndices(); + FactorIndices(const gtsam::FactorIndices &other); - // common STL methods - size_t size() const; - bool empty() const; - void clear(); + // 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; -}; + // 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; + }; -//************************************************************************* -// base -//************************************************************************* + //************************************************************************* + // base + //************************************************************************* -/** gtsam namespace functions */ + /** gtsam namespace functions */ #include -bool isDebugVersion(); + bool isDebugVersion(); #include -class IndexPair { - IndexPair(); - IndexPair(size_t i, size_t j); - size_t i() const; - size_t j() const; -}; + class IndexPair + { + IndexPair(); + IndexPair(size_t i, size_t j); + size_t i() const; + size_t j() const; + }; -// template -// class DSFMap { -// DSFMap(); -// KEY find(const KEY& key) const; -// void merge(const KEY& x, const KEY& y); -// std::map sets(); -// }; + // template + // class DSFMap { + // DSFMap(); + // KEY find(const KEY& key) const; + // void merge(const KEY& x, const KEY& y); + // std::map sets(); + // }; -class IndexPairSet { - IndexPairSet(); - // common STL methods - size_t size() const; - bool empty() const; - void clear(); + 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 -}; + // 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); + class IndexPairVector + { + IndexPairVector(); + IndexPairVector(const gtsam::IndexPairVector &other); - // common STL methods - size_t size() const; - bool empty() const; - void clear(); + // 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; -}; + // structure specific methods + gtsam::IndexPair at(size_t i) const; + void push_back(gtsam::IndexPair key) const; + }; -gtsam::IndexPairVector IndexPairSetAsArray(gtsam::IndexPairSet& set); + gtsam::IndexPairVector IndexPairSetAsArray(gtsam::IndexPairSet &set); -class IndexPairSetMap { - IndexPairSetMap(); - // common STL methods - size_t size() const; - bool empty() const; - void clear(); + class IndexPairSetMap + { + IndexPairSetMap(); + // common STL methods + size_t size() const; + bool empty() const; + void clear(); - // structure specific methods - gtsam::IndexPairSet at(gtsam::IndexPair& key); -}; + // structure specific methods + gtsam::IndexPairSet at(gtsam::IndexPair &key); + }; -class DSFMapIndexPair { - DSFMapIndexPair(); - gtsam::IndexPair find(const gtsam::IndexPair& key) const; - void merge(const gtsam::IndexPair& x, const gtsam::IndexPair& y); - gtsam::IndexPairSetMap sets(); -}; + class DSFMapIndexPair + { + DSFMapIndexPair(); + gtsam::IndexPair find(const gtsam::IndexPair &key) const; + void merge(const gtsam::IndexPair &x, const gtsam::IndexPair &y); + gtsam::IndexPairSetMap sets(); + }; #include -bool linear_independent(Matrix A, Matrix B, double tol); + bool linear_independent(Matrix A, Matrix B, double tol); #include -virtual class Value { - // No constructors because this is an abstract class + virtual class Value + { + // No constructors because this is an abstract class - // Testable - void print(string s) const; + // Testable + void print(string s) const; - // Manifold - size_t dim() const; -}; + // Manifold + size_t dim() const; + }; #include -template -virtual class GenericValue : gtsam::Value { - void serializable() const; -}; + template + virtual class GenericValue : gtsam::Value + { + void serializable() const; + }; -//************************************************************************* -// geometry -//************************************************************************* + //************************************************************************* + // geometry + //************************************************************************* #include -class Point2 { - // Standard Constructors - Point2(); - Point2(double x, double y); - Point2(Vector v); + class Point2 + { + // Standard Constructors + Point2(); + Point2(double x, double y); + Point2(Vector v); - // Testable - void print(string s) const; - bool equals(const gtsam::Point2& point, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::Point2 &point, double tol) const; - // Group - static gtsam::Point2 identity(); + // Group + static gtsam::Point2 identity(); - // Standard Interface - double x() const; - double y() const; - Vector vector() const; - double distance(const gtsam::Point2& p2) const; - double norm() const; + // Standard Interface + double x() const; + double y() const; + Vector vector() const; + double distance(const gtsam::Point2 &p2) const; + double norm() const; - // enabling serialization functionality - void serialize() const; -}; + // enabling serialization functionality + void serialize() const; + }; // std::vector #include -class Point2Vector -{ - // Constructors - Point2Vector(); - Point2Vector(const gtsam::Point2Vector& v); + 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); + //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; + //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(); -}; + //Modifiers + void assign(size_t n, const gtsam::Point2 &u); + void push_back(const gtsam::Point2 &x); + void pop_back(); + }; #include -class StereoPoint2 { - // Standard Constructors - StereoPoint2(); - StereoPoint2(double uL, double uR, double v); + class StereoPoint2 + { + // Standard Constructors + StereoPoint2(); + StereoPoint2(double uL, double uR, double v); - // Testable - void print(string s) const; - bool equals(const gtsam::StereoPoint2& point, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::StereoPoint2 &point, double tol) const; - // Group - static gtsam::StereoPoint2 identity(); - gtsam::StereoPoint2 inverse() const; - gtsam::StereoPoint2 compose(const gtsam::StereoPoint2& p2) const; - gtsam::StereoPoint2 between(const gtsam::StereoPoint2& p2) const; + // Group + static gtsam::StereoPoint2 identity(); + gtsam::StereoPoint2 inverse() const; + gtsam::StereoPoint2 compose(const gtsam::StereoPoint2 &p2) const; + gtsam::StereoPoint2 between(const gtsam::StereoPoint2 &p2) const; - // Manifold - gtsam::StereoPoint2 retract(Vector v) const; - Vector localCoordinates(const gtsam::StereoPoint2& p) const; + // Manifold + gtsam::StereoPoint2 retract(Vector v) const; + Vector localCoordinates(const gtsam::StereoPoint2 &p) const; - // Lie Group - static gtsam::StereoPoint2 Expmap(Vector v); - static Vector Logmap(const gtsam::StereoPoint2& p); + // Lie Group + static gtsam::StereoPoint2 Expmap(Vector v); + static Vector Logmap(const gtsam::StereoPoint2 &p); - // Standard Interface - Vector vector() const; - double uL() const; - double uR() const; - double v() const; + // Standard Interface + Vector vector() const; + double uL() const; + double uR() const; + double v() const; - // enabling serialization functionality - void serialize() const; -}; + // enabling serialization functionality + void serialize() const; + }; #include -class Point3 { - // Standard Constructors - Point3(); - Point3(double x, double y, double z); - Point3(Vector v); + class Point3 + { + // Standard Constructors + Point3(); + Point3(double x, double y, double z); + Point3(Vector v); - // Testable - void print(string s) const; - bool equals(const gtsam::Point3& p, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::Point3 &p, double tol) const; - // Group - static gtsam::Point3 identity(); + // Group + static gtsam::Point3 identity(); - // Standard Interface - Vector vector() const; - double x() const; - double y() const; - double z() const; + // Standard Interface + Vector vector() const; + double x() const; + double y() const; + double z() const; - // enabling serialization functionality - void serialize() const; -}; + // enabling serialization functionality + void serialize() const; + }; #include -class Rot2 { - // Standard Constructors and Named Constructors - Rot2(); - Rot2(double theta); - static gtsam::Rot2 fromAngle(double theta); - static gtsam::Rot2 fromDegrees(double theta); - static gtsam::Rot2 fromCosSin(double c, double s); + class Rot2 + { + // Standard Constructors and Named Constructors + Rot2(); + Rot2(double theta); + static gtsam::Rot2 fromAngle(double theta); + static gtsam::Rot2 fromDegrees(double theta); + static gtsam::Rot2 fromCosSin(double c, double s); - // Testable - void print(string s) const; - bool equals(const gtsam::Rot2& rot, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::Rot2 &rot, double tol) const; - // Group - static gtsam::Rot2 identity(); - gtsam::Rot2 inverse(); - gtsam::Rot2 compose(const gtsam::Rot2& p2) const; - gtsam::Rot2 between(const gtsam::Rot2& p2) const; + // Group + static gtsam::Rot2 identity(); + gtsam::Rot2 inverse(); + gtsam::Rot2 compose(const gtsam::Rot2 &p2) const; + gtsam::Rot2 between(const gtsam::Rot2 &p2) const; - // Manifold - gtsam::Rot2 retract(Vector v) const; - Vector localCoordinates(const gtsam::Rot2& p) const; + // Manifold + gtsam::Rot2 retract(Vector v) const; + Vector localCoordinates(const gtsam::Rot2 &p) const; - // Lie Group - static gtsam::Rot2 Expmap(Vector v); - static Vector Logmap(const gtsam::Rot2& p); - Vector logmap(const gtsam::Rot2& p); + // Lie Group + static gtsam::Rot2 Expmap(Vector v); + static Vector Logmap(const gtsam::Rot2 &p); + Vector logmap(const gtsam::Rot2 &p); - // Group Action on Point2 - gtsam::Point2 rotate(const gtsam::Point2& point) const; - gtsam::Point2 unrotate(const gtsam::Point2& point) const; + // Group Action on Point2 + gtsam::Point2 rotate(const gtsam::Point2 &point) const; + gtsam::Point2 unrotate(const gtsam::Point2 &point) const; - // Standard Interface - static gtsam::Rot2 relativeBearing(const gtsam::Point2& d); // Ignoring derivative - static gtsam::Rot2 atan2(double y, double x); - double theta() const; - double degrees() const; - double c() const; - double s() const; - Matrix matrix() const; + // Standard Interface + static gtsam::Rot2 relativeBearing(const gtsam::Point2 &d); // Ignoring derivative + static gtsam::Rot2 atan2(double y, double x); + double theta() const; + double degrees() const; + double c() const; + double s() const; + Matrix matrix() const; - // enabling serialization functionality - void serialize() const; -}; + // enabling serialization functionality + void serialize() const; + }; #include -class SO3 { - // Standard Constructors - SO3(); - SO3(Matrix R); - static gtsam::SO3 FromMatrix(Matrix R); - static gtsam::SO3 AxisAngle(const Vector axis, double theta); - static gtsam::SO3 ClosestTo(const Matrix M); + class SO3 + { + // Standard Constructors + SO3(); + SO3(Matrix R); + static gtsam::SO3 FromMatrix(Matrix R); + static gtsam::SO3 AxisAngle(const Vector axis, double theta); + static gtsam::SO3 ClosestTo(const Matrix M); - // Testable - void print(string s) const; - bool equals(const gtsam::SO3& other, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::SO3 &other, double tol) const; - // Group - static gtsam::SO3 identity(); - gtsam::SO3 inverse() const; - gtsam::SO3 between(const gtsam::SO3& R) const; - gtsam::SO3 compose(const gtsam::SO3& R) const; + // Group + static gtsam::SO3 identity(); + gtsam::SO3 inverse() const; + gtsam::SO3 between(const gtsam::SO3 &R) const; + gtsam::SO3 compose(const gtsam::SO3 &R) const; - // Manifold - gtsam::SO3 retract(Vector v) const; - Vector localCoordinates(const gtsam::SO3& R) const; - static gtsam::SO3 Expmap(Vector v); + // Manifold + gtsam::SO3 retract(Vector v) const; + Vector localCoordinates(const gtsam::SO3 &R) const; + static gtsam::SO3 Expmap(Vector v); - // Other methods - Vector vec() const; - Matrix matrix() const; -}; + // Other methods + Vector vec() const; + Matrix matrix() const; + }; #include -class SO4 { - // Standard Constructors - SO4(); - SO4(Matrix R); - static gtsam::SO4 FromMatrix(Matrix R); + class SO4 + { + // Standard Constructors + SO4(); + SO4(Matrix R); + static gtsam::SO4 FromMatrix(Matrix R); - // Testable - void print(string s) const; - bool equals(const gtsam::SO4& other, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::SO4 &other, double tol) const; - // Group - static gtsam::SO4 identity(); - gtsam::SO4 inverse() const; - gtsam::SO4 between(const gtsam::SO4& Q) const; - gtsam::SO4 compose(const gtsam::SO4& Q) const; + // Group + static gtsam::SO4 identity(); + gtsam::SO4 inverse() const; + gtsam::SO4 between(const gtsam::SO4 &Q) const; + gtsam::SO4 compose(const gtsam::SO4 &Q) const; - // Manifold - gtsam::SO4 retract(Vector v) const; - Vector localCoordinates(const gtsam::SO4& Q) const; - static gtsam::SO4 Expmap(Vector v); + // Manifold + gtsam::SO4 retract(Vector v) const; + Vector localCoordinates(const gtsam::SO4 &Q) const; + static gtsam::SO4 Expmap(Vector v); - // Other methods - Vector vec() const; - Matrix matrix() const; -}; + // Other methods + Vector vec() const; + Matrix matrix() const; + }; #include -class SOn { - // Standard Constructors - SOn(size_t n); - static gtsam::SOn FromMatrix(Matrix R); - static gtsam::SOn Lift(size_t n, Matrix R); + class SOn + { + // Standard Constructors + SOn(size_t n); + static gtsam::SOn FromMatrix(Matrix R); + static gtsam::SOn Lift(size_t n, Matrix R); - // Testable - void print(string s) const; - bool equals(const gtsam::SOn& other, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::SOn &other, double tol) const; - // Group - static gtsam::SOn identity(); - gtsam::SOn inverse() const; - gtsam::SOn between(const gtsam::SOn& Q) const; - gtsam::SOn compose(const gtsam::SOn& Q) const; + // Group + static gtsam::SOn identity(); + gtsam::SOn inverse() const; + gtsam::SOn between(const gtsam::SOn &Q) const; + gtsam::SOn compose(const gtsam::SOn &Q) const; - // Manifold - gtsam::SOn retract(Vector v) const; - Vector localCoordinates(const gtsam::SOn& Q) const; - static gtsam::SOn Expmap(Vector v); + // Manifold + gtsam::SOn retract(Vector v) const; + Vector localCoordinates(const gtsam::SOn &Q) const; + static gtsam::SOn Expmap(Vector v); - // Other methods - Vector vec() const; - Matrix matrix() const; + // Other methods + Vector vec() const; + Matrix matrix() const; - // enabling serialization functionality - void serialize() const; -}; + // enabling serialization functionality + void serialize() const; + }; #include -class Quaternion { - double w() const; - double x() const; - double y() const; - double z() const; - Vector coeffs() const; -}; + class Quaternion + { + double w() const; + double x() const; + double y() const; + double z() const; + Vector coeffs() const; + }; #include -class Rot3 { - // Standard Constructors and Named Constructors - Rot3(); - Rot3(Matrix R); - Rot3(const gtsam::Point3& col1, const gtsam::Point3& col2, const gtsam::Point3& col3); - Rot3(double R11, double R12, double R13, - double R21, double R22, double R23, - double R31, double R32, double R33); - Rot3(double w, double x, double y, double z); + class Rot3 + { + // Standard Constructors and Named Constructors + Rot3(); + Rot3(Matrix R); + Rot3(const gtsam::Point3 &col1, const gtsam::Point3 &col2, const gtsam::Point3 &col3); + Rot3(double R11, double R12, double R13, + double R21, double R22, double R23, + double R31, double R32, double R33); + Rot3(double w, double x, double y, double z); - static gtsam::Rot3 Rx(double t); - static gtsam::Rot3 Ry(double t); - static gtsam::Rot3 Rz(double t); - static gtsam::Rot3 RzRyRx(double x, double y, double z); - static gtsam::Rot3 RzRyRx(Vector xyz); - static gtsam::Rot3 Yaw(double t); // positive yaw is to right (as in aircraft heading) - static gtsam::Rot3 Pitch(double t); // positive pitch is up (increasing aircraft altitude) - static gtsam::Rot3 Roll(double t); // positive roll is to right (increasing yaw in aircraft) - static gtsam::Rot3 Ypr(double y, double p, double r); - static gtsam::Rot3 Quaternion(double w, double x, double y, double z); - static gtsam::Rot3 AxisAngle(const gtsam::Point3& axis, double angle); - static gtsam::Rot3 Rodrigues(Vector v); - static gtsam::Rot3 Rodrigues(double wx, double wy, double wz); - static gtsam::Rot3 ClosestTo(const Matrix M); + static gtsam::Rot3 Rx(double t); + static gtsam::Rot3 Ry(double t); + static gtsam::Rot3 Rz(double t); + static gtsam::Rot3 RzRyRx(double x, double y, double z); + static gtsam::Rot3 RzRyRx(Vector xyz); + static gtsam::Rot3 Yaw(double t); // positive yaw is to right (as in aircraft heading) + static gtsam::Rot3 Pitch(double t); // positive pitch is up (increasing aircraft altitude) + static gtsam::Rot3 Roll(double t); // positive roll is to right (increasing yaw in aircraft) + static gtsam::Rot3 Ypr(double y, double p, double r); + static gtsam::Rot3 Quaternion(double w, double x, double y, double z); + static gtsam::Rot3 AxisAngle(const gtsam::Point3 &axis, double angle); + static gtsam::Rot3 Rodrigues(Vector v); + static gtsam::Rot3 Rodrigues(double wx, double wy, double wz); + static gtsam::Rot3 ClosestTo(const Matrix M); - // Testable - void print(string s) const; - bool equals(const gtsam::Rot3& rot, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::Rot3 &rot, double tol) const; - // Group - static gtsam::Rot3 identity(); + // Group + static gtsam::Rot3 identity(); gtsam::Rot3 inverse() const; - gtsam::Rot3 compose(const gtsam::Rot3& p2) const; - gtsam::Rot3 between(const gtsam::Rot3& p2) const; + gtsam::Rot3 compose(const gtsam::Rot3 &p2) const; + gtsam::Rot3 between(const gtsam::Rot3 &p2) const; - // Manifold - //gtsam::Rot3 retractCayley(Vector v) const; // TODO, does not exist in both Matrix and Quaternion options - gtsam::Rot3 retract(Vector v) const; - Vector localCoordinates(const gtsam::Rot3& p) const; + // Manifold + //gtsam::Rot3 retractCayley(Vector v) const; // TODO, does not exist in both Matrix and Quaternion options + gtsam::Rot3 retract(Vector v) const; + Vector localCoordinates(const gtsam::Rot3 &p) const; - // Group Action on Point3 - gtsam::Point3 rotate(const gtsam::Point3& p) const; - gtsam::Point3 unrotate(const gtsam::Point3& p) const; + // Group Action on Point3 + gtsam::Point3 rotate(const gtsam::Point3 &p) const; + gtsam::Point3 unrotate(const gtsam::Point3 &p) const; - // Standard Interface - static gtsam::Rot3 Expmap(Vector v); - static Vector Logmap(const gtsam::Rot3& p); - Vector logmap(const gtsam::Rot3& p); - Matrix matrix() const; - Matrix transpose() const; - gtsam::Point3 column(size_t index) const; - Vector xyz() const; - Vector ypr() const; - Vector rpy() const; - double roll() const; - double pitch() const; - double yaw() const; - pair axisAngle() const; - gtsam::Quaternion toQuaternion() const; - Vector quaternion() const; - gtsam::Rot3 slerp(double t, const gtsam::Rot3& other) const; + // Standard Interface + static gtsam::Rot3 Expmap(Vector v); + static Vector Logmap(const gtsam::Rot3 &p); + Vector logmap(const gtsam::Rot3 &p); + Matrix matrix() const; + Matrix transpose() const; + gtsam::Point3 column(size_t index) const; + Vector xyz() const; + Vector ypr() const; + Vector rpy() const; + double roll() const; + double pitch() const; + double yaw() const; + pair axisAngle() const; + gtsam::Quaternion toQuaternion() const; + Vector quaternion() const; + gtsam::Rot3 slerp(double t, const gtsam::Rot3 &other) const; - // enabling serialization functionality - void serialize() const; -}; + // enabling serialization functionality + void serialize() const; + }; #include -class Pose2 { - // Standard Constructor - Pose2(); - Pose2(const gtsam::Pose2& other); - Pose2(double x, double y, double theta); - Pose2(double theta, const gtsam::Point2& t); - Pose2(const gtsam::Rot2& r, const gtsam::Point2& t); - Pose2(Vector v); + class Pose2 + { + // Standard Constructor + Pose2(); + Pose2(const gtsam::Pose2 &other); + Pose2(double x, double y, double theta); + Pose2(double theta, const gtsam::Point2 &t); + Pose2(const gtsam::Rot2 &r, const gtsam::Point2 &t); + Pose2(Vector v); - // Testable - void print(string s) const; - bool equals(const gtsam::Pose2& pose, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::Pose2 &pose, double tol) const; - // Group - static gtsam::Pose2 identity(); - gtsam::Pose2 inverse() const; - gtsam::Pose2 compose(const gtsam::Pose2& p2) const; - gtsam::Pose2 between(const gtsam::Pose2& p2) const; + // Group + static gtsam::Pose2 identity(); + gtsam::Pose2 inverse() const; + gtsam::Pose2 compose(const gtsam::Pose2 &p2) const; + gtsam::Pose2 between(const gtsam::Pose2 &p2) const; - // Manifold - gtsam::Pose2 retract(Vector v) const; - Vector localCoordinates(const gtsam::Pose2& p) const; + // Manifold + gtsam::Pose2 retract(Vector v) const; + Vector localCoordinates(const gtsam::Pose2 &p) const; - // Lie Group - static gtsam::Pose2 Expmap(Vector v); - static Vector Logmap(const gtsam::Pose2& p); - Vector logmap(const gtsam::Pose2& p); - static Matrix ExpmapDerivative(Vector v); - static Matrix LogmapDerivative(const gtsam::Pose2& v); - Matrix AdjointMap() const; - Vector Adjoint(Vector xi) const; - static Matrix adjointMap_(Vector v); - static Vector adjoint_(Vector xi, Vector y); - static Vector adjointTranspose(Vector xi, Vector y); - static Matrix wedge(double vx, double vy, double w); + // Lie Group + static gtsam::Pose2 Expmap(Vector v); + static Vector Logmap(const gtsam::Pose2 &p); + Vector logmap(const gtsam::Pose2 &p); + static Matrix ExpmapDerivative(Vector v); + static Matrix LogmapDerivative(const gtsam::Pose2 &v); + Matrix AdjointMap() const; + Vector Adjoint(Vector xi) const; + static Matrix adjointMap_(Vector v); + static Vector adjoint_(Vector xi, Vector y); + static Vector adjointTranspose(Vector xi, Vector y); + static Matrix wedge(double vx, double vy, double w); - // Group Actions on Point2 - gtsam::Point2 transformFrom(const gtsam::Point2& p) const; - gtsam::Point2 transformTo(const gtsam::Point2& p) const; + // Group Actions on Point2 + gtsam::Point2 transformFrom(const gtsam::Point2 &p) const; + gtsam::Point2 transformTo(const gtsam::Point2 &p) const; - // Standard Interface - double x() const; - double y() const; - double theta() const; - gtsam::Rot2 bearing(const gtsam::Point2& point) const; - double range(const gtsam::Point2& point) const; - gtsam::Point2 translation() const; - gtsam::Rot2 rotation() const; - Matrix matrix() const; + // Standard Interface + double x() const; + double y() const; + double theta() const; + gtsam::Rot2 bearing(const gtsam::Point2 &point) const; + double range(const gtsam::Point2 &point) const; + gtsam::Point2 translation() const; + gtsam::Rot2 rotation() const; + Matrix matrix() const; - // enabling serialization functionality - void serialize() const; -}; + // enabling serialization functionality + void serialize() const; + }; #include -class Pose3 { - // Standard Constructors - Pose3(); - Pose3(const gtsam::Pose3& other); - Pose3(const gtsam::Rot3& r, const gtsam::Point3& t); - Pose3(const gtsam::Pose2& pose2); - Pose3(Matrix mat); + class Pose3 + { + // Standard Constructors + Pose3(); + Pose3(const gtsam::Pose3 &other); + Pose3(const gtsam::Rot3 &r, const gtsam::Point3 &t); + Pose3(const gtsam::Pose2 &pose2); + Pose3(Matrix mat); - // Testable - void print(string s) const; - bool equals(const gtsam::Pose3& pose, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::Pose3 &pose, double tol) const; - // Group - static gtsam::Pose3 identity(); - gtsam::Pose3 inverse() const; - gtsam::Pose3 compose(const gtsam::Pose3& pose) const; - gtsam::Pose3 between(const gtsam::Pose3& pose) const; + // Group + static gtsam::Pose3 identity(); + gtsam::Pose3 inverse() const; + gtsam::Pose3 compose(const gtsam::Pose3 &pose) const; + gtsam::Pose3 between(const gtsam::Pose3 &pose) const; - // Manifold - gtsam::Pose3 retract(Vector v) const; - Vector localCoordinates(const gtsam::Pose3& pose) const; + // Manifold + gtsam::Pose3 retract(Vector v) const; + Vector localCoordinates(const gtsam::Pose3 &pose) const; - // Lie Group - static gtsam::Pose3 Expmap(Vector v); - static Vector Logmap(const gtsam::Pose3& pose); - Vector logmap(const gtsam::Pose3& pose); - Matrix AdjointMap() const; - Vector Adjoint(Vector xi) const; - static Matrix adjointMap_(Vector xi); - static Vector adjoint_(Vector xi, Vector y); - static Vector adjointTranspose(Vector xi, Vector y); - static Matrix ExpmapDerivative(Vector xi); - static Matrix LogmapDerivative(const gtsam::Pose3& xi); - static Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz); + // Lie Group + static gtsam::Pose3 Expmap(Vector v); + static Vector Logmap(const gtsam::Pose3 &pose); + Vector logmap(const gtsam::Pose3 &pose); + Matrix AdjointMap() const; + Vector Adjoint(Vector xi) const; + static Matrix adjointMap_(Vector xi); + static Vector adjoint_(Vector xi, Vector y); + static Vector adjointTranspose(Vector xi, Vector y); + static Matrix ExpmapDerivative(Vector xi); + static Matrix LogmapDerivative(const gtsam::Pose3 &xi); + static Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz); - // Group Action on Point3 - gtsam::Point3 transformFrom(const gtsam::Point3& point) const; - gtsam::Point3 transformTo(const gtsam::Point3& point) const; + // Group Action on Point3 + gtsam::Point3 transformFrom(const gtsam::Point3 &point) const; + gtsam::Point3 transformTo(const gtsam::Point3 &point) const; - // Standard Interface - gtsam::Rot3 rotation() const; - gtsam::Point3 translation() const; - double x() const; - double y() const; - double z() const; - Matrix matrix() const; - gtsam::Pose3 transformPoseFrom(const gtsam::Pose3& pose) const; - gtsam::Pose3 transformPoseTo(const gtsam::Pose3& pose) const; - double range(const gtsam::Point3& point); - double range(const gtsam::Pose3& pose); + // Standard Interface + gtsam::Rot3 rotation() const; + gtsam::Point3 translation() const; + double x() const; + double y() const; + double z() const; + Matrix matrix() const; + gtsam::Pose3 transformPoseFrom(const gtsam::Pose3 &pose) const; + gtsam::Pose3 transformPoseTo(const gtsam::Pose3 &pose) const; + double range(const gtsam::Point3 &point); + double range(const gtsam::Pose3 &pose); - // enabling serialization functionality - void serialize() const; -}; + // enabling serialization functionality + void serialize() const; + }; // std::vector #include -class Pose3Vector -{ - Pose3Vector(); - size_t size() const; - bool empty() const; - gtsam::Pose3 at(size_t n) const; - void push_back(const gtsam::Pose3& pose); -}; + 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 { - // Standard Constructors - Unit3(); - Unit3(const gtsam::Point3& pose); + class Unit3 + { + // Standard Constructors + Unit3(); + Unit3(const gtsam::Point3 &pose); - // Testable - void print(string s) const; - bool equals(const gtsam::Unit3& pose, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::Unit3 &pose, double tol) const; - // Other functionality - Matrix basis() const; - Matrix skew() const; - gtsam::Point3 point3() const; + // Other functionality + Matrix basis() const; + Matrix skew() const; + gtsam::Point3 point3() const; - // Manifold - static size_t Dim(); - size_t dim() const; - gtsam::Unit3 retract(Vector v) const; - Vector localCoordinates(const gtsam::Unit3& s) const; -}; + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::Unit3 retract(Vector v) const; + Vector localCoordinates(const gtsam::Unit3 &s) const; + }; #include -class EssentialMatrix { - // Standard Constructors - EssentialMatrix(const gtsam::Rot3& aRb, const gtsam::Unit3& aTb); + class EssentialMatrix + { + // Standard Constructors + EssentialMatrix(const gtsam::Rot3 &aRb, const gtsam::Unit3 &aTb); - // Testable - void print(string s) const; - bool equals(const gtsam::EssentialMatrix& pose, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::EssentialMatrix &pose, double tol) const; - // Manifold - static size_t Dim(); - size_t dim() const; - gtsam::EssentialMatrix retract(Vector v) const; - Vector localCoordinates(const gtsam::EssentialMatrix& s) const; + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::EssentialMatrix retract(Vector v) const; + Vector localCoordinates(const gtsam::EssentialMatrix &s) const; - // Other methods: - gtsam::Rot3 rotation() const; - gtsam::Unit3 direction() const; - Matrix matrix() const; - double error(Vector vA, Vector vB); -}; + // Other methods: + gtsam::Rot3 rotation() const; + gtsam::Unit3 direction() const; + Matrix matrix() const; + double error(Vector vA, Vector vB); + }; #include -class Cal3_S2 { - // Standard Constructors - Cal3_S2(); - Cal3_S2(double fx, double fy, double s, double u0, double v0); - Cal3_S2(Vector v); - Cal3_S2(double fov, int w, int h); + class Cal3_S2 + { + // Standard Constructors + Cal3_S2(); + Cal3_S2(double fx, double fy, double s, double u0, double v0); + Cal3_S2(Vector v); + Cal3_S2(double fov, int w, int h); - // Testable - void print(string s) const; - bool equals(const gtsam::Cal3_S2& rhs, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::Cal3_S2 &rhs, double tol) const; - // Manifold - static size_t Dim(); - size_t dim() const; - gtsam::Cal3_S2 retract(Vector v) const; - Vector localCoordinates(const gtsam::Cal3_S2& c) const; + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::Cal3_S2 retract(Vector v) const; + Vector localCoordinates(const gtsam::Cal3_S2 &c) const; - // Action on Point2 - gtsam::Point2 calibrate(const gtsam::Point2& p) const; - gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; + // Action on Point2 + gtsam::Point2 calibrate(const gtsam::Point2 &p) const; + gtsam::Point2 uncalibrate(const gtsam::Point2 &p) const; - // Standard Interface - double fx() const; - double fy() const; - double skew() const; - double px() const; - double py() const; - gtsam::Point2 principalPoint() const; - Vector vector() const; - Matrix K() const; - Matrix matrix() const; - Matrix matrix_inverse() const; + // Standard Interface + double fx() const; + double fy() const; + double skew() const; + double px() const; + double py() const; + gtsam::Point2 principalPoint() const; + Vector vector() const; + Matrix K() const; + Matrix matrix() const; + Matrix matrix_inverse() const; - // enabling serialization functionality - void serialize() const; -}; + // enabling serialization functionality + void serialize() const; + }; #include -virtual class Cal3DS2_Base { - // Standard Constructors - Cal3DS2_Base(); + virtual class Cal3DS2_Base + { + // Standard Constructors + Cal3DS2_Base(); - // Testable - void print(string s) const; + // Testable + void print(string s) const; - // Standard Interface - double fx() const; - double fy() const; - double skew() const; - double px() const; - double py() const; - double k1() const; - double k2() const; - Matrix K() const; - Vector k() const; - Vector vector() const; + // Standard Interface + double fx() const; + double fy() const; + double skew() const; + double px() const; + double py() const; + double k1() const; + double k2() const; + Matrix K() const; + Vector k() const; + Vector vector() const; - // Action on Point2 - gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; - gtsam::Point2 calibrate(const gtsam::Point2& p) const; + // Action on Point2 + gtsam::Point2 uncalibrate(const gtsam::Point2 &p) const; + gtsam::Point2 calibrate(const gtsam::Point2 &p) const; - // enabling serialization functionality - void serialize() const; -}; + // enabling serialization functionality + void serialize() const; + }; #include -virtual class Cal3DS2 : gtsam::Cal3DS2_Base { - // Standard Constructors - Cal3DS2(); - Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2); - Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double p1, double p2); - Cal3DS2(Vector v); + virtual class Cal3DS2 : gtsam::Cal3DS2_Base + { + // Standard Constructors + Cal3DS2(); + Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2); + Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double p1, double p2); + Cal3DS2(Vector v); - // Testable - bool equals(const gtsam::Cal3DS2& rhs, double tol) const; + // Testable + bool equals(const gtsam::Cal3DS2 &rhs, double tol) const; - // Manifold - size_t dim() const; - static size_t Dim(); - gtsam::Cal3DS2 retract(Vector v) const; - Vector localCoordinates(const gtsam::Cal3DS2& c) const; + // Manifold + size_t dim() const; + static size_t Dim(); + gtsam::Cal3DS2 retract(Vector v) const; + Vector localCoordinates(const gtsam::Cal3DS2 &c) const; - // enabling serialization functionality - void serialize() const; -}; + // enabling serialization functionality + void serialize() const; + }; #include -virtual class Cal3Unified : gtsam::Cal3DS2_Base { - // Standard Constructors - Cal3Unified(); - Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, double k2); - Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, double k2, double p1, double p2, double xi); - Cal3Unified(Vector v); + virtual class Cal3Unified : gtsam::Cal3DS2_Base + { + // Standard Constructors + Cal3Unified(); + Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, double k2); + Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, double k2, double p1, double p2, double xi); + Cal3Unified(Vector v); - // Testable - bool equals(const gtsam::Cal3Unified& rhs, double tol) const; + // Testable + bool equals(const gtsam::Cal3Unified &rhs, double tol) const; - // Standard Interface - double xi() const; - gtsam::Point2 spaceToNPlane(const gtsam::Point2& p) const; - gtsam::Point2 nPlaneToSpace(const gtsam::Point2& p) const; + // Standard Interface + double xi() const; + gtsam::Point2 spaceToNPlane(const gtsam::Point2 &p) const; + gtsam::Point2 nPlaneToSpace(const gtsam::Point2 &p) const; - // Manifold - size_t dim() const; - static size_t Dim(); - gtsam::Cal3Unified retract(Vector v) const; - Vector localCoordinates(const gtsam::Cal3Unified& c) const; + // Manifold + size_t dim() const; + static size_t Dim(); + gtsam::Cal3Unified retract(Vector v) const; + Vector localCoordinates(const gtsam::Cal3Unified &c) const; - // enabling serialization functionality - void serialize() const; -}; + // enabling serialization functionality + void serialize() const; + }; #include -class Cal3_S2Stereo { - // Standard Constructors - Cal3_S2Stereo(); - Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b); - Cal3_S2Stereo(Vector v); + class Cal3_S2Stereo + { + // Standard Constructors + Cal3_S2Stereo(); + Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b); + Cal3_S2Stereo(Vector v); - // Testable - void print(string s) const; - bool equals(const gtsam::Cal3_S2Stereo& K, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::Cal3_S2Stereo &K, double tol) const; - // Standard Interface - double fx() const; - double fy() const; - double skew() const; - double px() const; - double py() const; - gtsam::Point2 principalPoint() const; - double baseline() const; -}; + // Standard Interface + double fx() const; + double fy() const; + double skew() const; + double px() const; + double py() const; + gtsam::Point2 principalPoint() const; + double baseline() const; + }; #include -class Cal3Bundler { - // Standard Constructors - Cal3Bundler(); - Cal3Bundler(double fx, double k1, double k2, double u0, double v0); - Cal3Bundler(double fx, double k1, double k2, double u0, double v0, double tol); + class Cal3Bundler + { + // Standard Constructors + Cal3Bundler(); + Cal3Bundler(double fx, double k1, double k2, double u0, double v0); + Cal3Bundler(double fx, double k1, double k2, double u0, double v0, double tol); - // Testable - void print(string s) const; - bool equals(const gtsam::Cal3Bundler& rhs, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::Cal3Bundler &rhs, double tol) const; - // Manifold - static size_t Dim(); - size_t dim() const; - gtsam::Cal3Bundler retract(Vector v) const; - Vector localCoordinates(const gtsam::Cal3Bundler& c) const; + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::Cal3Bundler retract(Vector v) const; + Vector localCoordinates(const gtsam::Cal3Bundler &c) const; - // Action on Point2 - gtsam::Point2 calibrate(const gtsam::Point2& p) const; - gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; + // Action on Point2 + gtsam::Point2 calibrate(const gtsam::Point2 &p) const; + gtsam::Point2 uncalibrate(const gtsam::Point2 &p) const; - // Standard Interface - double fx() const; - double fy() const; - double k1() const; - double k2() const; - double px() const; - double py() const; - Vector vector() const; - Vector k() const; - Matrix K() const; + // Standard Interface + double fx() const; + double fy() const; + double k1() const; + double k2() const; + double px() const; + double py() const; + Vector vector() const; + Vector k() const; + Matrix K() const; - // enabling serialization functionality - void serialize() const; -}; + // enabling serialization functionality + void serialize() const; + }; #include -class CalibratedCamera { - // Standard Constructors and Named Constructors - CalibratedCamera(); - CalibratedCamera(const gtsam::Pose3& pose); - CalibratedCamera(Vector v); - static gtsam::CalibratedCamera Level(const gtsam::Pose2& pose2, double height); + class CalibratedCamera + { + // Standard Constructors and Named Constructors + CalibratedCamera(); + CalibratedCamera(const gtsam::Pose3 &pose); + CalibratedCamera(Vector v); + static gtsam::CalibratedCamera Level(const gtsam::Pose2 &pose2, double height); - // Testable - void print(string s) const; - bool equals(const gtsam::CalibratedCamera& camera, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::CalibratedCamera &camera, double tol) const; - // Manifold - static size_t Dim(); - size_t dim() const; - gtsam::CalibratedCamera retract(Vector d) const; - Vector localCoordinates(const gtsam::CalibratedCamera& T2) const; + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::CalibratedCamera retract(Vector d) const; + Vector localCoordinates(const gtsam::CalibratedCamera &T2) const; - // Action on Point3 - gtsam::Point2 project(const gtsam::Point3& point) const; - static gtsam::Point2 Project(const gtsam::Point3& cameraPoint); + // Action on Point3 + gtsam::Point2 project(const gtsam::Point3 &point) const; + static gtsam::Point2 Project(const gtsam::Point3 &cameraPoint); - // Standard Interface - gtsam::Pose3 pose() const; - double range(const gtsam::Point3& p) const; // TODO: Other overloaded range methods + // Standard Interface + gtsam::Pose3 pose() const; + double range(const gtsam::Point3 &p) const; // TODO: Other overloaded range methods - // enabling serialization functionality - void serialize() const; -}; + // enabling serialization functionality + void serialize() const; + }; #include -template -class PinholeCamera { - // Standard Constructors and Named Constructors - PinholeCamera(); - PinholeCamera(const gtsam::Pose3& pose); - PinholeCamera(const gtsam::Pose3& pose, const CALIBRATION& K); - static This Level(const CALIBRATION& K, const gtsam::Pose2& pose, double height); - 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); + template + class PinholeCamera + { + // Standard Constructors and Named Constructors + PinholeCamera(); + PinholeCamera(const gtsam::Pose3 &pose); + PinholeCamera(const gtsam::Pose3 &pose, const CALIBRATION &K); + static This Level(const CALIBRATION &K, const gtsam::Pose2 &pose, double height); + 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) const; - bool equals(const This& camera, double tol) const; + // Testable + void print(string s) const; + bool equals(const This &camera, double tol) const; - // Standard Interface - gtsam::Pose3 pose() const; - CALIBRATION calibration() 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(); + // 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::Point3 backproject(const gtsam::Point2& p, double depth) const; - double range(const gtsam::Point3& point); - double range(const gtsam::Pose3& pose); + // 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::Point3 backproject(const gtsam::Point2 &p, double depth) const; + double range(const gtsam::Point3 &point); + double range(const gtsam::Pose3 &pose); - // enabling serialization functionality - void serialize() const; -}; + // enabling serialization functionality + 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; + // 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; -template -class CameraSet { - CameraSet(); + template + class CameraSet + { + CameraSet(); + // structure specific methods + T at(size_t i) const; + void push_back(const T &cam); + }; - // // common STL methods - // size_t size() const; - // bool empty() const; - // void clear(); - - // structure specific methods - T at(size_t i) const; - void push_back(const T& cam); -}; - -// typedefs added here for shorter type name and to enforce uniformity in naming conventions -//typedef gtsam::CameraSet CameraSetCal3_S2; -//typedef gtsam::CameraSet CameraSetCal3Bundler; + // typedefs added here for shorter type name and to enforce uniformity in naming conventions + //typedef gtsam::CameraSet CameraSetCal3_S2; + //typedef gtsam::CameraSet CameraSetCal3Bundler; #include -class StereoCamera { - // Standard Constructors and Named Constructors - StereoCamera(); - StereoCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2Stereo* K); + class StereoCamera + { + // Standard Constructors and Named Constructors + StereoCamera(); + StereoCamera(const gtsam::Pose3 &pose, const gtsam::Cal3_S2Stereo *K); - // Testable - void print(string s) const; - bool equals(const gtsam::StereoCamera& camera, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::StereoCamera &camera, double tol) const; - // Standard Interface - gtsam::Pose3 pose() const; - double baseline() const; - gtsam::Cal3_S2Stereo calibration() const; + // Standard Interface + gtsam::Pose3 pose() const; + double baseline() const; + gtsam::Cal3_S2Stereo calibration() const; - // Manifold - gtsam::StereoCamera retract(Vector d) const; - Vector localCoordinates(const gtsam::StereoCamera& T2) const; - size_t dim() const; - static size_t Dim(); + // Manifold + gtsam::StereoCamera retract(Vector d) const; + Vector localCoordinates(const gtsam::StereoCamera &T2) const; + size_t dim() const; + static size_t Dim(); - // Transformations and measurement functions - gtsam::StereoPoint2 project(const gtsam::Point3& point); - gtsam::Point3 backproject(const gtsam::StereoPoint2& p) const; + // Transformations and measurement functions + gtsam::StereoPoint2 project(const gtsam::Point3 &point); + gtsam::Point3 backproject(const gtsam::StereoPoint2 &p) const; - // enabling serialization functionality - void serialize() const; -}; + // enabling serialization functionality + void serialize() const; + }; #include -// Templates appear not yet supported for free functions - issue raised at borglab/wrap#14 to add support -gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, - gtsam::Cal3_S2* sharedCal, const gtsam::Point2Vector& measurements, - double rank_tol, bool optimize); -gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, - gtsam::Cal3DS2* sharedCal, const gtsam::Point2Vector& measurements, - double rank_tol, bool optimize); -gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, - gtsam::Cal3Bundler* sharedCal, const gtsam::Point2Vector& measurements, - double rank_tol, bool optimize); -gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3_S2& cameras, - const gtsam::Point2Vector& measurements, double rank_tol, - bool optimize); -gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Bundler& cameras, - const gtsam::Point2Vector& measurements, double rank_tol, - bool optimize); - -//************************************************************************* -// Symbolic -//************************************************************************* + // Templates appear not yet supported for free functions - issue raised at borglab/wrap#14 to add support + gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector &poses, + gtsam::Cal3_S2 *sharedCal, const gtsam::Point2Vector &measurements, + double rank_tol, bool optimize); + gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector &poses, + gtsam::Cal3DS2 *sharedCal, const gtsam::Point2Vector &measurements, + double rank_tol, bool optimize); + gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector &poses, + gtsam::Cal3Bundler *sharedCal, const gtsam::Point2Vector &measurements, + double rank_tol, bool optimize); + gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3_S2 &cameras, + const gtsam::Point2Vector &measurements, double rank_tol, + bool optimize); + gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Bundler &cameras, + const gtsam::Point2Vector &measurements, double rank_tol, + bool optimize); + + //************************************************************************* + // Symbolic + //************************************************************************* #include -virtual class SymbolicFactor { - // Standard Constructors and Named Constructors - SymbolicFactor(const gtsam::SymbolicFactor& f); - SymbolicFactor(); - SymbolicFactor(size_t j); - SymbolicFactor(size_t j1, size_t j2); - SymbolicFactor(size_t j1, size_t j2, size_t j3); - SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4); - SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4, size_t j5); - SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4, size_t j5, size_t j6); - static gtsam::SymbolicFactor FromKeys(const gtsam::KeyVector& js); + virtual class SymbolicFactor + { + // Standard Constructors and Named Constructors + SymbolicFactor(const gtsam::SymbolicFactor &f); + SymbolicFactor(); + SymbolicFactor(size_t j); + SymbolicFactor(size_t j1, size_t j2); + SymbolicFactor(size_t j1, size_t j2, size_t j3); + SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4); + SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4, size_t j5); + SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4, size_t j5, size_t j6); + static gtsam::SymbolicFactor FromKeys(const gtsam::KeyVector &js); - // From Factor - size_t size() const; - void print(string s) const; - bool equals(const gtsam::SymbolicFactor& other, double tol) const; - gtsam::KeyVector keys(); -}; + // From Factor + size_t size() const; + void print(string s) const; + bool equals(const gtsam::SymbolicFactor &other, double tol) const; + gtsam::KeyVector keys(); + }; #include -virtual class SymbolicFactorGraph { - SymbolicFactorGraph(); - SymbolicFactorGraph(const gtsam::SymbolicBayesNet& bayesNet); - SymbolicFactorGraph(const gtsam::SymbolicBayesTree& bayesTree); + virtual class SymbolicFactorGraph + { + SymbolicFactorGraph(); + SymbolicFactorGraph(const gtsam::SymbolicBayesNet &bayesNet); + SymbolicFactorGraph(const gtsam::SymbolicBayesTree &bayesTree); - // From FactorGraph - void push_back(gtsam::SymbolicFactor* factor); - void print(string s) const; - bool equals(const gtsam::SymbolicFactorGraph& rhs, double tol) const; - size_t size() const; - bool exists(size_t idx) const; + // From FactorGraph + void push_back(gtsam::SymbolicFactor *factor); + void print(string s) const; + bool equals(const gtsam::SymbolicFactorGraph &rhs, double tol) const; + size_t size() const; + bool exists(size_t idx) const; - // Standard interface - gtsam::KeySet keys() const; - void push_back(const gtsam::SymbolicFactorGraph& graph); - void push_back(const gtsam::SymbolicBayesNet& bayesNet); - void push_back(const gtsam::SymbolicBayesTree& bayesTree); + // Standard interface + gtsam::KeySet keys() const; + void push_back(const gtsam::SymbolicFactorGraph &graph); + void push_back(const gtsam::SymbolicBayesNet &bayesNet); + void push_back(const gtsam::SymbolicBayesTree &bayesTree); - //Advanced Interface - void push_factor(size_t key); - void push_factor(size_t key1, size_t key2); - void push_factor(size_t key1, size_t key2, size_t key3); - void push_factor(size_t key1, size_t key2, size_t key3, size_t key4); + //Advanced Interface + void push_factor(size_t key); + void push_factor(size_t key1, size_t key2); + void push_factor(size_t key1, size_t key2, size_t key3); + void push_factor(size_t key1, size_t key2, size_t key3, size_t key4); - gtsam::SymbolicBayesNet* eliminateSequential(); - gtsam::SymbolicBayesNet* eliminateSequential(const gtsam::Ordering& ordering); - gtsam::SymbolicBayesTree* eliminateMultifrontal(); - gtsam::SymbolicBayesTree* eliminateMultifrontal(const gtsam::Ordering& ordering); - pair eliminatePartialSequential( - const gtsam::Ordering& ordering); - pair eliminatePartialSequential( - const gtsam::KeyVector& keys); - pair eliminatePartialMultifrontal( - const gtsam::Ordering& ordering); - pair eliminatePartialMultifrontal( - const gtsam::KeyVector& keys); - gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& ordering); - gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& key_vector); - gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& ordering, - const gtsam::Ordering& marginalizedVariableOrdering); - gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& key_vector, - const gtsam::Ordering& marginalizedVariableOrdering); - gtsam::SymbolicFactorGraph* marginal(const gtsam::KeyVector& key_vector); -}; + gtsam::SymbolicBayesNet *eliminateSequential(); + gtsam::SymbolicBayesNet *eliminateSequential(const gtsam::Ordering &ordering); + gtsam::SymbolicBayesTree *eliminateMultifrontal(); + gtsam::SymbolicBayesTree *eliminateMultifrontal(const gtsam::Ordering &ordering); + pair eliminatePartialSequential( + const gtsam::Ordering &ordering); + pair eliminatePartialSequential( + const gtsam::KeyVector &keys); + pair eliminatePartialMultifrontal( + const gtsam::Ordering &ordering); + pair eliminatePartialMultifrontal( + const gtsam::KeyVector &keys); + gtsam::SymbolicBayesNet *marginalMultifrontalBayesNet(const gtsam::Ordering &ordering); + gtsam::SymbolicBayesNet *marginalMultifrontalBayesNet(const gtsam::KeyVector &key_vector); + gtsam::SymbolicBayesNet *marginalMultifrontalBayesNet(const gtsam::Ordering &ordering, + const gtsam::Ordering &marginalizedVariableOrdering); + gtsam::SymbolicBayesNet *marginalMultifrontalBayesNet(const gtsam::KeyVector &key_vector, + const gtsam::Ordering &marginalizedVariableOrdering); + gtsam::SymbolicFactorGraph *marginal(const gtsam::KeyVector &key_vector); + }; #include -virtual class SymbolicConditional : gtsam::SymbolicFactor { - // Standard Constructors and Named Constructors - SymbolicConditional(); - SymbolicConditional(const gtsam::SymbolicConditional& other); - SymbolicConditional(size_t key); - SymbolicConditional(size_t key, size_t parent); - SymbolicConditional(size_t key, size_t parent1, size_t parent2); - SymbolicConditional(size_t key, size_t parent1, size_t parent2, size_t parent3); - static gtsam::SymbolicConditional FromKeys(const gtsam::KeyVector& keys, size_t nrFrontals); + virtual class SymbolicConditional : gtsam::SymbolicFactor + { + // Standard Constructors and Named Constructors + SymbolicConditional(); + SymbolicConditional(const gtsam::SymbolicConditional &other); + SymbolicConditional(size_t key); + SymbolicConditional(size_t key, size_t parent); + SymbolicConditional(size_t key, size_t parent1, size_t parent2); + SymbolicConditional(size_t key, size_t parent1, size_t parent2, size_t parent3); + static gtsam::SymbolicConditional FromKeys(const gtsam::KeyVector &keys, size_t nrFrontals); - // Testable - void print(string s) const; - bool equals(const gtsam::SymbolicConditional& other, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::SymbolicConditional &other, double tol) const; - // Standard interface - size_t nrFrontals() const; - size_t nrParents() const; -}; + // Standard interface + size_t nrFrontals() const; + size_t nrParents() const; + }; #include -class SymbolicBayesNet { - SymbolicBayesNet(); - SymbolicBayesNet(const gtsam::SymbolicBayesNet& other); - // Testable - void print(string s) const; - bool equals(const gtsam::SymbolicBayesNet& other, double tol) const; + class SymbolicBayesNet + { + SymbolicBayesNet(); + SymbolicBayesNet(const gtsam::SymbolicBayesNet &other); + // Testable + void print(string s) const; + bool equals(const gtsam::SymbolicBayesNet &other, double tol) const; - // Standard interface - size_t size() const; - void saveGraph(string s) const; - gtsam::SymbolicConditional* at(size_t idx) const; - gtsam::SymbolicConditional* front() const; - gtsam::SymbolicConditional* back() const; - void push_back(gtsam::SymbolicConditional* conditional); - void push_back(const gtsam::SymbolicBayesNet& bayesNet); -}; + // Standard interface + size_t size() const; + void saveGraph(string s) const; + gtsam::SymbolicConditional *at(size_t idx) const; + gtsam::SymbolicConditional *front() const; + gtsam::SymbolicConditional *back() const; + void push_back(gtsam::SymbolicConditional *conditional); + void push_back(const gtsam::SymbolicBayesNet &bayesNet); + }; #include -class SymbolicBayesTree { + class SymbolicBayesTree + { //Constructors SymbolicBayesTree(); - SymbolicBayesTree(const gtsam::SymbolicBayesTree& other); + SymbolicBayesTree(const gtsam::SymbolicBayesTree &other); // Testable void print(string s); - bool equals(const gtsam::SymbolicBayesTree& other, double tol) const; + bool equals(const gtsam::SymbolicBayesTree &other, double tol) const; //Standard Interface //size_t findParentClique(const gtsam::IndexVector& parents) const; @@ -1261,969 +1297,1019 @@ class SymbolicBayesTree { void deleteCachedShortcuts(); size_t numCachedSeparatorMarginals() const; - gtsam::SymbolicConditional* marginalFactor(size_t key) const; - gtsam::SymbolicFactorGraph* joint(size_t key1, size_t key2) const; - gtsam::SymbolicBayesNet* jointBayesNet(size_t key1, size_t key2) const; -}; + gtsam::SymbolicConditional *marginalFactor(size_t key) const; + gtsam::SymbolicFactorGraph *joint(size_t key1, size_t key2) const; + gtsam::SymbolicBayesNet *jointBayesNet(size_t key1, size_t key2) const; + }; -// class SymbolicBayesTreeClique { -// BayesTreeClique(); -// BayesTreeClique(CONDITIONAL* conditional); -// // BayesTreeClique(const pair& result) : Base(result) {} -// -// bool equals(const This& other, double tol) const; -// void print(string s) const; -// void printTree() const; // Default indent of "" -// void printTree(string indent) const; -// size_t numCachedSeparatorMarginals() const; -// -// CONDITIONAL* conditional() const; -// bool isRoot() const; -// size_t treeSize() const; -// // const std::list& children() const { return children_; } -// // derived_ptr parent() const { return parent_.lock(); } -// -// // TODO: need wrapped versions graphs, BayesNet -// // BayesNet shortcut(derived_ptr root, Eliminate function) const; -// // FactorGraph marginal(derived_ptr root, Eliminate function) const; -// // FactorGraph joint(derived_ptr C2, derived_ptr root, Eliminate function) const; -// -// void deleteCachedShortcuts(); -// }; + // class SymbolicBayesTreeClique { + // BayesTreeClique(); + // BayesTreeClique(CONDITIONAL* conditional); + // // BayesTreeClique(const pair& result) : Base(result) {} + // + // bool equals(const This& other, double tol) const; + // void print(string s) const; + // void printTree() const; // Default indent of "" + // void printTree(string indent) const; + // size_t numCachedSeparatorMarginals() const; + // + // CONDITIONAL* conditional() const; + // bool isRoot() const; + // size_t treeSize() const; + // // const std::list& children() const { return children_; } + // // derived_ptr parent() const { return parent_.lock(); } + // + // // TODO: need wrapped versions graphs, BayesNet + // // BayesNet shortcut(derived_ptr root, Eliminate function) const; + // // FactorGraph marginal(derived_ptr root, Eliminate function) const; + // // FactorGraph joint(derived_ptr C2, derived_ptr root, Eliminate function) const; + // + // void deleteCachedShortcuts(); + // }; #include -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); - VariableIndex(const gtsam::VariableIndex& other); + 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); + VariableIndex(const gtsam::VariableIndex &other); - // Testable - bool equals(const gtsam::VariableIndex& other, double tol) const; - void print(string s) const; + // Testable + bool equals(const gtsam::VariableIndex &other, double tol) const; + void print(string s) const; - // Standard interface - size_t size() const; - size_t nFactors() const; - size_t nEntries() const; -}; + // Standard interface + size_t size() const; + size_t nFactors() const; + size_t nEntries() const; + }; -//************************************************************************* -// linear -//************************************************************************* + //************************************************************************* + // linear + //************************************************************************* -namespace noiseModel { + namespace noiseModel + { #include -virtual class Base { - void print(string s) const; - // Methods below are available for all noise models. However, can't add them - // because wrap (incorrectly) thinks robust classes derive from this Base as well. - // bool isConstrained() const; - // bool isUnit() const; - // size_t dim() const; - // Vector sigmas() const; -}; + virtual class Base + { + void print(string s) const; + // Methods below are available for all noise models. However, can't add them + // because wrap (incorrectly) thinks robust classes derive from this Base as well. + // bool isConstrained() const; + // bool isUnit() const; + // size_t dim() const; + // Vector sigmas() const; + }; -virtual class Gaussian : gtsam::noiseModel::Base { - static gtsam::noiseModel::Gaussian* Information(Matrix R); - static gtsam::noiseModel::Gaussian* SqrtInformation(Matrix R); - static gtsam::noiseModel::Gaussian* Covariance(Matrix R); + virtual class Gaussian : gtsam::noiseModel::Base + { + static gtsam::noiseModel::Gaussian *Information(Matrix R); + static gtsam::noiseModel::Gaussian *SqrtInformation(Matrix R); + static gtsam::noiseModel::Gaussian *Covariance(Matrix R); - bool equals(gtsam::noiseModel::Base& expected, double tol); + bool equals(gtsam::noiseModel::Base &expected, double tol); - // access to noise model - Matrix R() const; - Matrix information() const; - Matrix covariance() const; + // access to noise model + Matrix R() const; + Matrix information() const; + Matrix covariance() const; - // Whitening operations - Vector whiten(Vector v) const; - Vector unwhiten(Vector v) const; - Matrix Whiten(Matrix H) const; + // Whitening operations + Vector whiten(Vector v) const; + Vector unwhiten(Vector v) const; + Matrix Whiten(Matrix H) const; - // enabling serialization functionality - void serializable() const; -}; + // enabling serialization functionality + void serializable() const; + }; -virtual class Diagonal : gtsam::noiseModel::Gaussian { - static gtsam::noiseModel::Diagonal* Sigmas(Vector sigmas); - static gtsam::noiseModel::Diagonal* Variances(Vector variances); - static gtsam::noiseModel::Diagonal* Precisions(Vector precisions); - Matrix R() const; + virtual class Diagonal : gtsam::noiseModel::Gaussian + { + static gtsam::noiseModel::Diagonal *Sigmas(Vector sigmas); + static gtsam::noiseModel::Diagonal *Variances(Vector variances); + static gtsam::noiseModel::Diagonal *Precisions(Vector precisions); + Matrix R() const; - // access to noise model - Vector sigmas() const; - Vector invsigmas() const; - Vector precisions() const; + // access to noise model + Vector sigmas() const; + Vector invsigmas() const; + Vector precisions() const; - // enabling serialization functionality - void serializable() const; -}; + // enabling serialization functionality + void serializable() const; + }; -virtual class Constrained : gtsam::noiseModel::Diagonal { - static gtsam::noiseModel::Constrained* MixedSigmas(Vector mu, Vector sigmas); - static gtsam::noiseModel::Constrained* MixedSigmas(double m, Vector sigmas); - static gtsam::noiseModel::Constrained* MixedVariances(Vector mu, Vector variances); - static gtsam::noiseModel::Constrained* MixedVariances(Vector variances); - static gtsam::noiseModel::Constrained* MixedPrecisions(Vector mu, Vector precisions); - static gtsam::noiseModel::Constrained* MixedPrecisions(Vector precisions); + virtual class Constrained : gtsam::noiseModel::Diagonal + { + static gtsam::noiseModel::Constrained *MixedSigmas(Vector mu, Vector sigmas); + static gtsam::noiseModel::Constrained *MixedSigmas(double m, Vector sigmas); + static gtsam::noiseModel::Constrained *MixedVariances(Vector mu, Vector variances); + static gtsam::noiseModel::Constrained *MixedVariances(Vector variances); + static gtsam::noiseModel::Constrained *MixedPrecisions(Vector mu, Vector precisions); + static gtsam::noiseModel::Constrained *MixedPrecisions(Vector precisions); - static gtsam::noiseModel::Constrained* All(size_t dim); - static gtsam::noiseModel::Constrained* All(size_t dim, double mu); + static gtsam::noiseModel::Constrained *All(size_t dim); + static gtsam::noiseModel::Constrained *All(size_t dim, double mu); - gtsam::noiseModel::Constrained* unit() const; + gtsam::noiseModel::Constrained *unit() const; - // enabling serialization functionality - void serializable() const; -}; + // enabling serialization functionality + void serializable() const; + }; -virtual class Isotropic : gtsam::noiseModel::Diagonal { - static gtsam::noiseModel::Isotropic* Sigma(size_t dim, double sigma); - static gtsam::noiseModel::Isotropic* Variance(size_t dim, double varianace); - static gtsam::noiseModel::Isotropic* Precision(size_t dim, double precision); + virtual class Isotropic : gtsam::noiseModel::Diagonal + { + static gtsam::noiseModel::Isotropic *Sigma(size_t dim, double sigma); + static gtsam::noiseModel::Isotropic *Variance(size_t dim, double varianace); + static gtsam::noiseModel::Isotropic *Precision(size_t dim, double precision); - // access to noise model - double sigma() const; + // access to noise model + double sigma() const; - // enabling serialization functionality - void serializable() const; -}; + // enabling serialization functionality + void serializable() const; + }; -virtual class Unit : gtsam::noiseModel::Isotropic { - static gtsam::noiseModel::Unit* Create(size_t dim); + virtual class Unit : gtsam::noiseModel::Isotropic + { + static gtsam::noiseModel::Unit *Create(size_t dim); - // enabling serialization functionality - void serializable() const; -}; + // enabling serialization functionality + void serializable() const; + }; -namespace mEstimator { -virtual class Base { - void print(string s) const; -}; + namespace mEstimator + { + virtual class Base + { + void print(string s) const; + }; -virtual class Null: gtsam::noiseModel::mEstimator::Base { - Null(); - static gtsam::noiseModel::mEstimator::Null* Create(); + virtual class Null : gtsam::noiseModel::mEstimator::Base + { + Null(); + static gtsam::noiseModel::mEstimator::Null *Create(); - // enabling serialization functionality - void serializable() const; + // enabling serialization functionality + void serializable() const; - double weight(double error) const; - double loss(double error) const; -}; + double weight(double error) const; + double loss(double error) const; + }; -virtual class Fair: gtsam::noiseModel::mEstimator::Base { - Fair(double c); - static gtsam::noiseModel::mEstimator::Fair* Create(double c); + virtual class Fair : gtsam::noiseModel::mEstimator::Base + { + Fair(double c); + static gtsam::noiseModel::mEstimator::Fair *Create(double c); - // enabling serialization functionality - void serializable() const; + // enabling serialization functionality + void serializable() const; - double weight(double error) const; - double loss(double error) const; -}; + double weight(double error) const; + double loss(double error) const; + }; -virtual class Huber: gtsam::noiseModel::mEstimator::Base { - Huber(double k); - static gtsam::noiseModel::mEstimator::Huber* Create(double k); + virtual class Huber : gtsam::noiseModel::mEstimator::Base + { + Huber(double k); + static gtsam::noiseModel::mEstimator::Huber *Create(double k); - // enabling serialization functionality - void serializable() const; + // enabling serialization functionality + void serializable() const; - double weight(double error) const; - double loss(double error) const; -}; + double weight(double error) const; + double loss(double error) const; + }; -virtual class Cauchy: gtsam::noiseModel::mEstimator::Base { - Cauchy(double k); - static gtsam::noiseModel::mEstimator::Cauchy* Create(double k); + virtual class Cauchy : gtsam::noiseModel::mEstimator::Base + { + Cauchy(double k); + static gtsam::noiseModel::mEstimator::Cauchy *Create(double k); - // enabling serialization functionality - void serializable() const; + // enabling serialization functionality + void serializable() const; - double weight(double error) const; - double loss(double error) const; -}; + double weight(double error) const; + double loss(double error) const; + }; -virtual class Tukey: gtsam::noiseModel::mEstimator::Base { - Tukey(double k); - static gtsam::noiseModel::mEstimator::Tukey* Create(double k); + virtual class Tukey : gtsam::noiseModel::mEstimator::Base + { + Tukey(double k); + static gtsam::noiseModel::mEstimator::Tukey *Create(double k); - // enabling serialization functionality - void serializable() const; + // enabling serialization functionality + void serializable() const; - double weight(double error) const; - double loss(double error) const; -}; + double weight(double error) const; + double loss(double error) const; + }; -virtual class Welsch: gtsam::noiseModel::mEstimator::Base { - Welsch(double k); - static gtsam::noiseModel::mEstimator::Welsch* Create(double k); + virtual class Welsch : gtsam::noiseModel::mEstimator::Base + { + Welsch(double k); + static gtsam::noiseModel::mEstimator::Welsch *Create(double k); - // enabling serialization functionality - void serializable() const; + // enabling serialization functionality + void serializable() const; - double weight(double error) const; - double loss(double error) const; -}; + double weight(double error) const; + double loss(double error) const; + }; -virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base { - GemanMcClure(double c); - static gtsam::noiseModel::mEstimator::GemanMcClure* Create(double c); + virtual class GemanMcClure : gtsam::noiseModel::mEstimator::Base + { + GemanMcClure(double c); + static gtsam::noiseModel::mEstimator::GemanMcClure *Create(double c); - // enabling serialization functionality - void serializable() const; + // enabling serialization functionality + void serializable() const; - double weight(double error) const; - double loss(double error) const; -}; + double weight(double error) const; + double loss(double error) const; + }; -virtual class DCS: gtsam::noiseModel::mEstimator::Base { - DCS(double c); - static gtsam::noiseModel::mEstimator::DCS* Create(double c); + virtual class DCS : gtsam::noiseModel::mEstimator::Base + { + DCS(double c); + static gtsam::noiseModel::mEstimator::DCS *Create(double c); - // enabling serialization functionality - void serializable() const; + // enabling serialization functionality + void serializable() const; - double weight(double error) const; - double loss(double error) const; -}; + double weight(double error) const; + double loss(double error) const; + }; -virtual class L2WithDeadZone: gtsam::noiseModel::mEstimator::Base { - L2WithDeadZone(double k); - static gtsam::noiseModel::mEstimator::L2WithDeadZone* Create(double k); + virtual class L2WithDeadZone : gtsam::noiseModel::mEstimator::Base + { + L2WithDeadZone(double k); + static gtsam::noiseModel::mEstimator::L2WithDeadZone *Create(double k); - // enabling serialization functionality - void serializable() const; + // enabling serialization functionality + void serializable() const; - double weight(double error) const; - double loss(double error) const; -}; + double weight(double error) const; + double loss(double error) const; + }; -}///\namespace mEstimator + } // namespace mEstimator -virtual class Robust : gtsam::noiseModel::Base { - Robust(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise); - static gtsam::noiseModel::Robust* Create(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise); + virtual class Robust : gtsam::noiseModel::Base + { + Robust(const gtsam::noiseModel::mEstimator::Base *robust, const gtsam::noiseModel::Base *noise); + static gtsam::noiseModel::Robust *Create(const gtsam::noiseModel::mEstimator::Base *robust, const gtsam::noiseModel::Base *noise); - // enabling serialization functionality - void serializable() const; -}; + // enabling serialization functionality + void serializable() const; + }; -}///\namespace noiseModel + } // namespace noiseModel #include -class Sampler { - // Constructors - Sampler(gtsam::noiseModel::Diagonal* model, int seed); - Sampler(Vector sigmas, int seed); + class Sampler + { + // Constructors + Sampler(gtsam::noiseModel::Diagonal *model, int seed); + Sampler(Vector sigmas, int seed); - // Standard Interface - size_t dim() const; - Vector sigmas() const; - gtsam::noiseModel::Diagonal* model() const; - Vector sample(); -}; + // Standard Interface + size_t dim() const; + Vector sigmas() const; + gtsam::noiseModel::Diagonal *model() const; + Vector sample(); + }; #include -class VectorValues { + class VectorValues + { //Constructors - VectorValues(); - VectorValues(const gtsam::VectorValues& other); + VectorValues(); + VectorValues(const gtsam::VectorValues &other); - //Named Constructors - static gtsam::VectorValues Zero(const gtsam::VectorValues& model); + //Named Constructors + static gtsam::VectorValues Zero(const gtsam::VectorValues &model); - //Standard Interface - size_t size() const; - size_t dim(size_t j) const; - bool exists(size_t j) const; - void print(string s) const; - bool equals(const gtsam::VectorValues& expected, double tol) const; - void insert(size_t j, Vector value); - Vector vector() const; - Vector at(size_t j) const; - void update(const gtsam::VectorValues& values); + //Standard Interface + size_t size() const; + size_t dim(size_t j) const; + bool exists(size_t j) const; + void print(string s) const; + bool equals(const gtsam::VectorValues &expected, double tol) const; + void insert(size_t j, Vector value); + Vector vector() const; + Vector at(size_t j) const; + void update(const gtsam::VectorValues &values); - //Advanced Interface - void setZero(); + //Advanced Interface + void setZero(); - gtsam::VectorValues add(const gtsam::VectorValues& c) const; - void addInPlace(const gtsam::VectorValues& c); - gtsam::VectorValues subtract(const gtsam::VectorValues& c) const; - gtsam::VectorValues scale(double a) const; - void scaleInPlace(double a); + gtsam::VectorValues add(const gtsam::VectorValues &c) const; + void addInPlace(const gtsam::VectorValues &c); + gtsam::VectorValues subtract(const gtsam::VectorValues &c) const; + gtsam::VectorValues scale(double a) const; + void scaleInPlace(double a); - bool hasSameStructure(const gtsam::VectorValues& other) const; - double dot(const gtsam::VectorValues& V) const; - double norm() const; - double squaredNorm() const; + bool hasSameStructure(const gtsam::VectorValues &other) const; + double dot(const gtsam::VectorValues &V) const; + double norm() const; + double squaredNorm() const; - // enabling serialization functionality - void serialize() const; -}; + // enabling serialization functionality + void serialize() const; + }; #include -virtual class GaussianFactor { - gtsam::KeyVector keys() const; - void print(string s) const; - bool equals(const gtsam::GaussianFactor& lf, double tol) const; - double error(const gtsam::VectorValues& c) const; - gtsam::GaussianFactor* clone() const; - gtsam::GaussianFactor* negate() const; - Matrix augmentedInformation() const; - Matrix information() const; - Matrix augmentedJacobian() const; - pair jacobian() const; - size_t size() const; - bool empty() const; -}; + virtual class GaussianFactor + { + gtsam::KeyVector keys() const; + void print(string s) const; + bool equals(const gtsam::GaussianFactor &lf, double tol) const; + double error(const gtsam::VectorValues &c) const; + gtsam::GaussianFactor *clone() const; + gtsam::GaussianFactor *negate() const; + Matrix augmentedInformation() const; + Matrix information() const; + Matrix augmentedJacobian() const; + pair jacobian() const; + size_t size() const; + bool empty() const; + }; #include -virtual class JacobianFactor : gtsam::GaussianFactor { - //Constructors - JacobianFactor(); - JacobianFactor(const gtsam::GaussianFactor& factor); - JacobianFactor(Vector b_in); - JacobianFactor(size_t i1, Matrix A1, Vector b, - const gtsam::noiseModel::Diagonal* model); - JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, Vector b, - const gtsam::noiseModel::Diagonal* model); - JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, size_t i3, Matrix A3, - Vector b, const gtsam::noiseModel::Diagonal* model); - JacobianFactor(const gtsam::GaussianFactorGraph& graph); + virtual class JacobianFactor : gtsam::GaussianFactor + { + //Constructors + JacobianFactor(); + JacobianFactor(const gtsam::GaussianFactor &factor); + JacobianFactor(Vector b_in); + JacobianFactor(size_t i1, Matrix A1, Vector b, + const gtsam::noiseModel::Diagonal *model); + JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, Vector b, + const gtsam::noiseModel::Diagonal *model); + JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, size_t i3, Matrix A3, + Vector b, const gtsam::noiseModel::Diagonal *model); + JacobianFactor(const gtsam::GaussianFactorGraph &graph); - //Testable - void print(string s) const; - void printKeys(string s) const; - bool equals(const gtsam::GaussianFactor& lf, double tol) const; - size_t size() const; - Vector unweighted_error(const gtsam::VectorValues& c) const; - Vector error_vector(const gtsam::VectorValues& c) const; - double error(const gtsam::VectorValues& c) const; + //Testable + void print(string s) const; + void printKeys(string s) const; + bool equals(const gtsam::GaussianFactor &lf, double tol) const; + size_t size() const; + Vector unweighted_error(const gtsam::VectorValues &c) const; + Vector error_vector(const gtsam::VectorValues &c) const; + double error(const gtsam::VectorValues &c) const; - //Standard Interface - Matrix getA() const; - Vector getb() const; - size_t rows() const; - size_t cols() const; - bool isConstrained() const; - pair jacobianUnweighted() const; - Matrix augmentedJacobianUnweighted() const; + //Standard Interface + Matrix getA() const; + Vector getb() const; + size_t rows() const; + size_t cols() const; + bool isConstrained() const; + pair jacobianUnweighted() const; + Matrix augmentedJacobianUnweighted() const; - void transposeMultiplyAdd(double alpha, Vector e, gtsam::VectorValues& x) const; - gtsam::JacobianFactor whiten() const; + void transposeMultiplyAdd(double alpha, Vector e, gtsam::VectorValues &x) const; + gtsam::JacobianFactor whiten() const; - pair eliminate(const gtsam::Ordering& keys) const; + pair eliminate(const gtsam::Ordering &keys) const; - void setModel(bool anyConstrained, Vector sigmas); + void setModel(bool anyConstrained, Vector sigmas); - gtsam::noiseModel::Diagonal* get_model() const; + gtsam::noiseModel::Diagonal *get_model() const; - // enabling serialization functionality - void serialize() const; -}; + // enabling serialization functionality + void serialize() const; + }; #include -virtual class HessianFactor : gtsam::GaussianFactor { - //Constructors - HessianFactor(); - HessianFactor(const gtsam::GaussianFactor& factor); - HessianFactor(size_t j, Matrix G, Vector g, double f); - HessianFactor(size_t j, Vector mu, Matrix Sigma); - HessianFactor(size_t j1, size_t j2, Matrix G11, Matrix G12, Vector g1, Matrix G22, - Vector g2, double f); - HessianFactor(size_t j1, size_t j2, size_t j3, Matrix G11, Matrix G12, Matrix G13, - Vector g1, Matrix G22, Matrix G23, Vector g2, Matrix G33, Vector g3, - double f); - HessianFactor(const gtsam::GaussianFactorGraph& factors); + virtual class HessianFactor : gtsam::GaussianFactor + { + //Constructors + HessianFactor(); + HessianFactor(const gtsam::GaussianFactor &factor); + HessianFactor(size_t j, Matrix G, Vector g, double f); + HessianFactor(size_t j, Vector mu, Matrix Sigma); + HessianFactor(size_t j1, size_t j2, Matrix G11, Matrix G12, Vector g1, Matrix G22, + Vector g2, double f); + HessianFactor(size_t j1, size_t j2, size_t j3, Matrix G11, Matrix G12, Matrix G13, + Vector g1, Matrix G22, Matrix G23, Vector g2, Matrix G33, Vector g3, + double f); + HessianFactor(const gtsam::GaussianFactorGraph &factors); - //Testable - size_t size() const; - void print(string s) const; - void printKeys(string s) const; - bool equals(const gtsam::GaussianFactor& lf, double tol) const; - double error(const gtsam::VectorValues& c) const; + //Testable + size_t size() const; + void print(string s) const; + void printKeys(string s) const; + bool equals(const gtsam::GaussianFactor &lf, double tol) const; + double error(const gtsam::VectorValues &c) const; - //Standard Interface - size_t rows() const; - Matrix information() const; - double constantTerm() const; - Vector linearTerm() const; + //Standard Interface + size_t rows() const; + Matrix information() const; + double constantTerm() const; + Vector linearTerm() const; - // enabling serialization functionality - void serialize() const; -}; + // enabling serialization functionality + void serialize() const; + }; #include -class GaussianFactorGraph { - GaussianFactorGraph(); - GaussianFactorGraph(const gtsam::GaussianBayesNet& bayesNet); - GaussianFactorGraph(const gtsam::GaussianBayesTree& bayesTree); + class GaussianFactorGraph + { + GaussianFactorGraph(); + GaussianFactorGraph(const gtsam::GaussianBayesNet &bayesNet); + GaussianFactorGraph(const gtsam::GaussianBayesTree &bayesTree); - // From FactorGraph - void print(string s) const; - bool equals(const gtsam::GaussianFactorGraph& lfgraph, double tol) const; - size_t size() const; - gtsam::GaussianFactor* at(size_t idx) const; - gtsam::KeySet keys() const; - gtsam::KeyVector keyVector() const; - bool exists(size_t idx) const; + // From FactorGraph + void print(string s) const; + bool equals(const gtsam::GaussianFactorGraph &lfgraph, double tol) const; + size_t size() const; + gtsam::GaussianFactor *at(size_t idx) const; + gtsam::KeySet keys() const; + gtsam::KeyVector keyVector() const; + bool exists(size_t idx) const; - // Building the graph - void push_back(const gtsam::GaussianFactor* factor); - void push_back(const gtsam::GaussianConditional* conditional); - void push_back(const gtsam::GaussianFactorGraph& graph); - void push_back(const gtsam::GaussianBayesNet& bayesNet); - void push_back(const gtsam::GaussianBayesTree& bayesTree); - void add(const gtsam::GaussianFactor& factor); - void add(Vector b); - void add(size_t key1, Matrix A1, Vector b, const gtsam::noiseModel::Diagonal* model); - void add(size_t key1, Matrix A1, size_t key2, Matrix A2, Vector b, - const gtsam::noiseModel::Diagonal* model); - void add(size_t key1, Matrix A1, size_t key2, Matrix A2, size_t key3, Matrix A3, - Vector b, const gtsam::noiseModel::Diagonal* model); + // Building the graph + void push_back(const gtsam::GaussianFactor *factor); + void push_back(const gtsam::GaussianConditional *conditional); + void push_back(const gtsam::GaussianFactorGraph &graph); + void push_back(const gtsam::GaussianBayesNet &bayesNet); + void push_back(const gtsam::GaussianBayesTree &bayesTree); + void add(const gtsam::GaussianFactor &factor); + void add(Vector b); + void add(size_t key1, Matrix A1, Vector b, const gtsam::noiseModel::Diagonal *model); + void add(size_t key1, Matrix A1, size_t key2, Matrix A2, Vector b, + const gtsam::noiseModel::Diagonal *model); + void add(size_t key1, Matrix A1, size_t key2, Matrix A2, size_t key3, Matrix A3, + Vector b, const gtsam::noiseModel::Diagonal *model); - // error and probability - double error(const gtsam::VectorValues& c) const; - double probPrime(const gtsam::VectorValues& c) const; + // error and probability + double error(const gtsam::VectorValues &c) const; + double probPrime(const gtsam::VectorValues &c) const; - gtsam::GaussianFactorGraph clone() const; - gtsam::GaussianFactorGraph negate() const; + gtsam::GaussianFactorGraph clone() const; + gtsam::GaussianFactorGraph negate() const; - // Optimizing and linear algebra - gtsam::VectorValues optimize() const; - gtsam::VectorValues optimize(const gtsam::Ordering& ordering) const; - gtsam::VectorValues optimizeGradientSearch() const; - gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const; - gtsam::VectorValues gradientAtZero() const; + // Optimizing and linear algebra + gtsam::VectorValues optimize() const; + gtsam::VectorValues optimize(const gtsam::Ordering &ordering) const; + gtsam::VectorValues optimizeGradientSearch() const; + gtsam::VectorValues gradient(const gtsam::VectorValues &x0) const; + gtsam::VectorValues gradientAtZero() const; - // Elimination and marginals - gtsam::GaussianBayesNet* eliminateSequential(); - gtsam::GaussianBayesNet* eliminateSequential(const gtsam::Ordering& ordering); - gtsam::GaussianBayesTree* eliminateMultifrontal(); - gtsam::GaussianBayesTree* eliminateMultifrontal(const gtsam::Ordering& ordering); - pair eliminatePartialSequential( - const gtsam::Ordering& ordering); - pair eliminatePartialSequential( - const gtsam::KeyVector& keys); - pair eliminatePartialMultifrontal( - const gtsam::Ordering& ordering); - pair eliminatePartialMultifrontal( - const gtsam::KeyVector& keys); - gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& ordering); - gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& key_vector); - gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& ordering, - const gtsam::Ordering& marginalizedVariableOrdering); - gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& key_vector, - const gtsam::Ordering& marginalizedVariableOrdering); - gtsam::GaussianFactorGraph* marginal(const gtsam::KeyVector& key_vector); + // Elimination and marginals + gtsam::GaussianBayesNet *eliminateSequential(); + gtsam::GaussianBayesNet *eliminateSequential(const gtsam::Ordering &ordering); + gtsam::GaussianBayesTree *eliminateMultifrontal(); + gtsam::GaussianBayesTree *eliminateMultifrontal(const gtsam::Ordering &ordering); + pair eliminatePartialSequential( + const gtsam::Ordering &ordering); + pair eliminatePartialSequential( + const gtsam::KeyVector &keys); + pair eliminatePartialMultifrontal( + const gtsam::Ordering &ordering); + pair eliminatePartialMultifrontal( + const gtsam::KeyVector &keys); + gtsam::GaussianBayesNet *marginalMultifrontalBayesNet(const gtsam::Ordering &ordering); + gtsam::GaussianBayesNet *marginalMultifrontalBayesNet(const gtsam::KeyVector &key_vector); + gtsam::GaussianBayesNet *marginalMultifrontalBayesNet(const gtsam::Ordering &ordering, + const gtsam::Ordering &marginalizedVariableOrdering); + gtsam::GaussianBayesNet *marginalMultifrontalBayesNet(const gtsam::KeyVector &key_vector, + const gtsam::Ordering &marginalizedVariableOrdering); + gtsam::GaussianFactorGraph *marginal(const gtsam::KeyVector &key_vector); - // Conversion to matrices - Matrix sparseJacobian_() const; - Matrix augmentedJacobian() const; - Matrix augmentedJacobian(const gtsam::Ordering& ordering) const; - pair jacobian() const; - pair jacobian(const gtsam::Ordering& ordering) const; - Matrix augmentedHessian() const; - Matrix augmentedHessian(const gtsam::Ordering& ordering) const; - pair hessian() const; - pair hessian(const gtsam::Ordering& ordering) const; + // Conversion to matrices + Matrix sparseJacobian_() const; + Matrix augmentedJacobian() const; + Matrix augmentedJacobian(const gtsam::Ordering &ordering) const; + pair jacobian() const; + pair jacobian(const gtsam::Ordering &ordering) const; + Matrix augmentedHessian() const; + Matrix augmentedHessian(const gtsam::Ordering &ordering) const; + pair hessian() const; + pair hessian(const gtsam::Ordering &ordering) const; - // enabling serialization functionality - void serialize() const; -}; + // enabling serialization functionality + void serialize() const; + }; #include -virtual class GaussianConditional : gtsam::GaussianFactor { - //Constructors - GaussianConditional(size_t key, Vector d, Matrix R, const gtsam::noiseModel::Diagonal* sigmas); - GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, - const gtsam::noiseModel::Diagonal* sigmas); - GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, - size_t name2, Matrix T, const gtsam::noiseModel::Diagonal* sigmas); + virtual class GaussianConditional : gtsam::GaussianFactor + { + //Constructors + GaussianConditional(size_t key, Vector d, Matrix R, const gtsam::noiseModel::Diagonal *sigmas); + GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, + const gtsam::noiseModel::Diagonal *sigmas); + GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, + size_t name2, Matrix T, const gtsam::noiseModel::Diagonal *sigmas); - //Constructors with no noise model - GaussianConditional(size_t key, Vector d, Matrix R); + //Constructors with no noise model + GaussianConditional(size_t key, Vector d, Matrix R); GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S); GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, - size_t name2, Matrix T); + size_t name2, Matrix T); - //Standard Interface - void print(string s) const; - bool equals(const gtsam::GaussianConditional &cg, double tol) const; + //Standard Interface + void print(string s) const; + bool equals(const gtsam::GaussianConditional &cg, double tol) const; - //Advanced Interface - gtsam::VectorValues solve(const gtsam::VectorValues& parents) const; - gtsam::VectorValues solveOtherRHS(const gtsam::VectorValues& parents, const gtsam::VectorValues& rhs) const; - void solveTransposeInPlace(gtsam::VectorValues& gy) const; - void scaleFrontalsBySigma(gtsam::VectorValues& gy) const; - Matrix R() const; - Matrix S() const; - Vector d() const; + //Advanced Interface + gtsam::VectorValues solve(const gtsam::VectorValues &parents) const; + gtsam::VectorValues solveOtherRHS(const gtsam::VectorValues &parents, const gtsam::VectorValues &rhs) const; + void solveTransposeInPlace(gtsam::VectorValues &gy) const; + void scaleFrontalsBySigma(gtsam::VectorValues &gy) const; + Matrix R() const; + Matrix S() const; + Vector d() const; - // enabling serialization functionality - void serialize() const; -}; + // enabling serialization functionality + void serialize() const; + }; #include -virtual class GaussianDensity : gtsam::GaussianConditional { + virtual class GaussianDensity : gtsam::GaussianConditional + { //Constructors - GaussianDensity(size_t key, Vector d, Matrix R, const gtsam::noiseModel::Diagonal* sigmas); + GaussianDensity(size_t key, Vector d, Matrix R, const gtsam::noiseModel::Diagonal *sigmas); - //Standard Interface - void print(string s) const; - bool equals(const gtsam::GaussianDensity &cg, double tol) const; - Vector mean() const; - Matrix covariance() const; -}; + //Standard Interface + void print(string s) const; + bool equals(const gtsam::GaussianDensity &cg, double tol) const; + Vector mean() const; + Matrix covariance() const; + }; #include -virtual class GaussianBayesNet { + virtual class GaussianBayesNet + { //Constructors - GaussianBayesNet(); - GaussianBayesNet(const gtsam::GaussianConditional* conditional); + GaussianBayesNet(); + GaussianBayesNet(const gtsam::GaussianConditional *conditional); - // Testable - void print(string s) const; - bool equals(const gtsam::GaussianBayesNet& other, double tol) const; - size_t size() const; + // Testable + void print(string s) const; + bool equals(const gtsam::GaussianBayesNet &other, double tol) const; + size_t size() const; - // FactorGraph derived interface - // size_t size() const; - gtsam::GaussianConditional* at(size_t idx) const; - gtsam::KeySet keys() const; - bool exists(size_t idx) const; + // FactorGraph derived interface + // size_t size() const; + gtsam::GaussianConditional *at(size_t idx) const; + gtsam::KeySet keys() const; + bool exists(size_t idx) const; - gtsam::GaussianConditional* front() const; - gtsam::GaussianConditional* back() const; - void push_back(gtsam::GaussianConditional* conditional); - void push_back(const gtsam::GaussianBayesNet& bayesNet); + gtsam::GaussianConditional *front() const; + gtsam::GaussianConditional *back() const; + void push_back(gtsam::GaussianConditional *conditional); + void push_back(const gtsam::GaussianBayesNet &bayesNet); - gtsam::VectorValues optimize() const; - gtsam::VectorValues optimize(gtsam::VectorValues& solutionForMissing) const; - gtsam::VectorValues optimizeGradientSearch() const; - gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const; - gtsam::VectorValues gradientAtZero() const; - double error(const gtsam::VectorValues& x) const; - double determinant() const; - double logDeterminant() const; - gtsam::VectorValues backSubstitute(const gtsam::VectorValues& gx) const; - gtsam::VectorValues backSubstituteTranspose(const gtsam::VectorValues& gx) const; -}; + gtsam::VectorValues optimize() const; + gtsam::VectorValues optimize(gtsam::VectorValues &solutionForMissing) const; + gtsam::VectorValues optimizeGradientSearch() const; + gtsam::VectorValues gradient(const gtsam::VectorValues &x0) const; + gtsam::VectorValues gradientAtZero() const; + double error(const gtsam::VectorValues &x) const; + double determinant() const; + double logDeterminant() const; + gtsam::VectorValues backSubstitute(const gtsam::VectorValues &gx) const; + gtsam::VectorValues backSubstituteTranspose(const gtsam::VectorValues &gx) const; + }; #include -virtual class GaussianBayesTree { - // Standard Constructors and Named Constructors - GaussianBayesTree(); - GaussianBayesTree(const gtsam::GaussianBayesTree& other); - bool equals(const gtsam::GaussianBayesTree& other, double tol) const; - void print(string s); - size_t size() const; - bool empty() const; - size_t numCachedSeparatorMarginals() const; - void saveGraph(string s) const; + virtual class GaussianBayesTree + { + // Standard Constructors and Named Constructors + GaussianBayesTree(); + GaussianBayesTree(const gtsam::GaussianBayesTree &other); + bool equals(const gtsam::GaussianBayesTree &other, double tol) const; + void print(string s); + size_t size() const; + bool empty() const; + size_t numCachedSeparatorMarginals() const; + void saveGraph(string s) const; - gtsam::VectorValues optimize() const; - gtsam::VectorValues optimizeGradientSearch() const; - gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const; - gtsam::VectorValues gradientAtZero() const; - double error(const gtsam::VectorValues& x) const; - double determinant() const; - double logDeterminant() const; - Matrix marginalCovariance(size_t key) const; - gtsam::GaussianConditional* marginalFactor(size_t key) const; - gtsam::GaussianFactorGraph* joint(size_t key1, size_t key2) const; - gtsam::GaussianBayesNet* jointBayesNet(size_t key1, size_t key2) const; -}; + gtsam::VectorValues optimize() const; + gtsam::VectorValues optimizeGradientSearch() const; + gtsam::VectorValues gradient(const gtsam::VectorValues &x0) const; + gtsam::VectorValues gradientAtZero() const; + double error(const gtsam::VectorValues &x) const; + double determinant() const; + double logDeterminant() const; + Matrix marginalCovariance(size_t key) const; + gtsam::GaussianConditional *marginalFactor(size_t key) const; + gtsam::GaussianFactorGraph *joint(size_t key1, size_t key2) const; + gtsam::GaussianBayesNet *jointBayesNet(size_t key1, size_t key2) const; + }; #include -class Errors { + class Errors + { //Constructors Errors(); - Errors(const gtsam::VectorValues& V); + Errors(const gtsam::VectorValues &V); //Testable void print(string s); - bool equals(const gtsam::Errors& expected, double tol) const; -}; + bool equals(const gtsam::Errors &expected, double tol) const; + }; #include -class GaussianISAM { - //Constructor - GaussianISAM(); + class GaussianISAM + { + //Constructor + GaussianISAM(); - //Standard Interface - void update(const gtsam::GaussianFactorGraph& newFactors); - void saveGraph(string s) const; - void clear(); -}; + //Standard Interface + void update(const gtsam::GaussianFactorGraph &newFactors); + void saveGraph(string s) const; + void clear(); + }; #include -virtual class IterativeOptimizationParameters { - string getVerbosity() const; - void setVerbosity(string s) ; - void print() const; -}; + virtual class IterativeOptimizationParameters + { + string getVerbosity() const; + void setVerbosity(string s); + void print() const; + }; -//virtual class IterativeSolver { -// IterativeSolver(); -// gtsam::VectorValues optimize (); -//}; + //virtual class IterativeSolver { + // IterativeSolver(); + // gtsam::VectorValues optimize (); + //}; #include -virtual class ConjugateGradientParameters : gtsam::IterativeOptimizationParameters { - ConjugateGradientParameters(); - int getMinIterations() const ; - int getMaxIterations() const ; - int getReset() const; - double getEpsilon_rel() const; - double getEpsilon_abs() const; + virtual class ConjugateGradientParameters : gtsam::IterativeOptimizationParameters + { + ConjugateGradientParameters(); + int getMinIterations() const; + int getMaxIterations() const; + int getReset() const; + double getEpsilon_rel() const; + double getEpsilon_abs() const; - void setMinIterations(int value); - void setMaxIterations(int value); - void setReset(int value); - void setEpsilon_rel(double value); - void setEpsilon_abs(double value); - void print() const; -}; + void setMinIterations(int value); + void setMaxIterations(int value); + void setReset(int value); + void setEpsilon_rel(double value); + void setEpsilon_abs(double value); + void print() const; + }; #include -virtual class PreconditionerParameters { - PreconditionerParameters(); -}; + virtual class PreconditionerParameters + { + PreconditionerParameters(); + }; -virtual class DummyPreconditionerParameters : gtsam::PreconditionerParameters { - DummyPreconditionerParameters(); -}; + virtual class DummyPreconditionerParameters : gtsam::PreconditionerParameters + { + DummyPreconditionerParameters(); + }; #include -virtual class PCGSolverParameters : gtsam::ConjugateGradientParameters { - PCGSolverParameters(); - void print(string s); - void setPreconditionerParams(gtsam::PreconditionerParameters* preconditioner); -}; + virtual class PCGSolverParameters : gtsam::ConjugateGradientParameters + { + PCGSolverParameters(); + void print(string s); + void setPreconditionerParams(gtsam::PreconditionerParameters *preconditioner); + }; #include -virtual class SubgraphSolverParameters : gtsam::ConjugateGradientParameters { - SubgraphSolverParameters(); - void print() const; -}; + virtual class SubgraphSolverParameters : gtsam::ConjugateGradientParameters + { + SubgraphSolverParameters(); + void print() const; + }; -virtual class SubgraphSolver { - SubgraphSolver(const gtsam::GaussianFactorGraph &A, const gtsam::SubgraphSolverParameters ¶meters, const gtsam::Ordering& ordering); - SubgraphSolver(const gtsam::GaussianFactorGraph &Ab1, const gtsam::GaussianFactorGraph* Ab2, const gtsam::SubgraphSolverParameters ¶meters, const gtsam::Ordering& ordering); - gtsam::VectorValues optimize() const; -}; + virtual class SubgraphSolver + { + SubgraphSolver(const gtsam::GaussianFactorGraph &A, const gtsam::SubgraphSolverParameters ¶meters, const gtsam::Ordering &ordering); + SubgraphSolver(const gtsam::GaussianFactorGraph &Ab1, const gtsam::GaussianFactorGraph *Ab2, const gtsam::SubgraphSolverParameters ¶meters, const gtsam::Ordering &ordering); + gtsam::VectorValues optimize() const; + }; #include -class KalmanFilter { - KalmanFilter(size_t n); - // gtsam::GaussianDensity* init(Vector x0, const gtsam::SharedDiagonal& P0); - gtsam::GaussianDensity* init(Vector x0, Matrix P0); - void print(string s) const; - static size_t step(gtsam::GaussianDensity* p); - gtsam::GaussianDensity* predict(gtsam::GaussianDensity* p, Matrix F, - Matrix B, Vector u, const gtsam::noiseModel::Diagonal* modelQ); - gtsam::GaussianDensity* predictQ(gtsam::GaussianDensity* p, Matrix F, - Matrix B, Vector u, Matrix Q); - gtsam::GaussianDensity* predict2(gtsam::GaussianDensity* p, Matrix A0, - Matrix A1, Vector b, const gtsam::noiseModel::Diagonal* model); - gtsam::GaussianDensity* update(gtsam::GaussianDensity* p, Matrix H, - Vector z, const gtsam::noiseModel::Diagonal* model); - gtsam::GaussianDensity* updateQ(gtsam::GaussianDensity* p, Matrix H, - Vector z, Matrix Q); -}; + class KalmanFilter + { + KalmanFilter(size_t n); + // gtsam::GaussianDensity* init(Vector x0, const gtsam::SharedDiagonal& P0); + gtsam::GaussianDensity *init(Vector x0, Matrix P0); + void print(string s) const; + static size_t step(gtsam::GaussianDensity *p); + gtsam::GaussianDensity *predict(gtsam::GaussianDensity *p, Matrix F, + Matrix B, Vector u, const gtsam::noiseModel::Diagonal *modelQ); + gtsam::GaussianDensity *predictQ(gtsam::GaussianDensity *p, Matrix F, + Matrix B, Vector u, Matrix Q); + gtsam::GaussianDensity *predict2(gtsam::GaussianDensity *p, Matrix A0, + Matrix A1, Vector b, const gtsam::noiseModel::Diagonal *model); + gtsam::GaussianDensity *update(gtsam::GaussianDensity *p, Matrix H, + Vector z, const gtsam::noiseModel::Diagonal *model); + gtsam::GaussianDensity *updateQ(gtsam::GaussianDensity *p, Matrix H, + Vector z, Matrix Q); + }; -//************************************************************************* -// nonlinear -//************************************************************************* + //************************************************************************* + // nonlinear + //************************************************************************* #include -size_t symbol(char chr, size_t index); -char symbolChr(size_t key); -size_t symbolIndex(size_t key); + size_t symbol(char chr, size_t index); + char symbolChr(size_t key); + size_t symbolIndex(size_t key); -namespace symbol_shorthand { - size_t A(size_t j); - size_t B(size_t j); - size_t C(size_t j); - size_t D(size_t j); - size_t E(size_t j); - size_t F(size_t j); - size_t G(size_t j); - size_t H(size_t j); - size_t I(size_t j); - size_t J(size_t j); - size_t K(size_t j); - size_t L(size_t j); - size_t M(size_t j); - size_t N(size_t j); - size_t O(size_t j); - size_t P(size_t j); - size_t Q(size_t j); - size_t R(size_t j); - size_t S(size_t j); - size_t T(size_t j); - size_t U(size_t j); - size_t V(size_t j); - size_t W(size_t j); - size_t X(size_t j); - size_t Y(size_t j); - size_t Z(size_t j); -}///\namespace symbol + namespace symbol_shorthand + { + size_t A(size_t j); + size_t B(size_t j); + size_t C(size_t j); + size_t D(size_t j); + size_t E(size_t j); + size_t F(size_t j); + size_t G(size_t j); + size_t H(size_t j); + size_t I(size_t j); + size_t J(size_t j); + size_t K(size_t j); + size_t L(size_t j); + size_t M(size_t j); + size_t N(size_t j); + size_t O(size_t j); + size_t P(size_t j); + size_t Q(size_t j); + size_t R(size_t j); + size_t S(size_t j); + size_t T(size_t j); + size_t U(size_t j); + size_t V(size_t j); + size_t W(size_t j); + size_t X(size_t j); + size_t Y(size_t j); + size_t Z(size_t j); + } // namespace symbol_shorthand -// Default keyformatter -void PrintKeyList (const gtsam::KeyList& keys); -void PrintKeyList (const gtsam::KeyList& keys, string s); -void PrintKeyVector(const gtsam::KeyVector& keys); -void PrintKeyVector(const gtsam::KeyVector& keys, string s); -void PrintKeySet (const gtsam::KeySet& keys); -void PrintKeySet (const gtsam::KeySet& keys, string s); + // Default keyformatter + void PrintKeyList(const gtsam::KeyList &keys); + void PrintKeyList(const gtsam::KeyList &keys, string s); + void PrintKeyVector(const gtsam::KeyVector &keys); + void PrintKeyVector(const gtsam::KeyVector &keys, string s); + void PrintKeySet(const gtsam::KeySet &keys); + void PrintKeySet(const gtsam::KeySet &keys, string s); #include -class LabeledSymbol { - LabeledSymbol(size_t full_key); - LabeledSymbol(const gtsam::LabeledSymbol& key); - LabeledSymbol(unsigned char valType, unsigned char label, size_t j); + class LabeledSymbol + { + LabeledSymbol(size_t full_key); + LabeledSymbol(const gtsam::LabeledSymbol &key); + LabeledSymbol(unsigned char valType, unsigned char label, size_t j); - size_t key() const; - unsigned char label() const; - unsigned char chr() const; - size_t index() const; + size_t key() const; + unsigned char label() const; + unsigned char chr() const; + size_t index() const; - gtsam::LabeledSymbol upper() const; - gtsam::LabeledSymbol lower() const; - gtsam::LabeledSymbol newChr(unsigned char c) const; - gtsam::LabeledSymbol newLabel(unsigned char label) const; + gtsam::LabeledSymbol upper() const; + gtsam::LabeledSymbol lower() const; + gtsam::LabeledSymbol newChr(unsigned char c) const; + gtsam::LabeledSymbol newLabel(unsigned char label) const; - void print(string s) const; -}; + void print(string s) const; + }; -size_t mrsymbol(unsigned char c, unsigned char label, size_t j); -unsigned char mrsymbolChr(size_t key); -unsigned char mrsymbolLabel(size_t key); -size_t mrsymbolIndex(size_t key); + size_t mrsymbol(unsigned char c, unsigned char label, size_t j); + unsigned char mrsymbolChr(size_t key); + unsigned char mrsymbolLabel(size_t key); + size_t mrsymbolIndex(size_t key); #include -class Ordering { - // Standard Constructors and Named Constructors - Ordering(); - Ordering(const gtsam::Ordering& other); + class Ordering + { + // Standard Constructors and Named Constructors + Ordering(); + Ordering(const gtsam::Ordering &other); - // Testable - void print(string s) const; - bool equals(const gtsam::Ordering& ord, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::Ordering &ord, double tol) const; - // Standard interface - size_t size() const; - size_t at(size_t key) const; - void push_back(size_t key); + // Standard interface + size_t size() const; + size_t at(size_t key) const; + void push_back(size_t key); - // enabling serialization functionality - void serialize() const; -}; + // enabling serialization functionality + void serialize() const; + }; #include -class NonlinearFactorGraph { - NonlinearFactorGraph(); - NonlinearFactorGraph(const gtsam::NonlinearFactorGraph& graph); + class NonlinearFactorGraph + { + NonlinearFactorGraph(); + NonlinearFactorGraph(const gtsam::NonlinearFactorGraph &graph); - // FactorGraph - void print(string s) const; - bool equals(const gtsam::NonlinearFactorGraph& fg, double tol) const; - size_t size() const; - bool empty() const; - void remove(size_t i); - void replace(size_t i, gtsam::NonlinearFactor* factors); - void resize(size_t size); - size_t nrFactors() const; - gtsam::NonlinearFactor* at(size_t idx) const; - void push_back(const gtsam::NonlinearFactorGraph& factors); - void push_back(gtsam::NonlinearFactor* factor); - void add(gtsam::NonlinearFactor* factor); - bool exists(size_t idx) const; - gtsam::KeySet keys() const; - gtsam::KeyVector keyVector() const; + // FactorGraph + void print(string s) const; + bool equals(const gtsam::NonlinearFactorGraph &fg, double tol) const; + size_t size() const; + bool empty() const; + void remove(size_t i); + void replace(size_t i, gtsam::NonlinearFactor *factors); + void resize(size_t size); + size_t nrFactors() const; + gtsam::NonlinearFactor *at(size_t idx) const; + void push_back(const gtsam::NonlinearFactorGraph &factors); + void push_back(gtsam::NonlinearFactor *factor); + void add(gtsam::NonlinearFactor *factor); + bool exists(size_t idx) const; + gtsam::KeySet keys() const; + gtsam::KeyVector keyVector() const; - template, gtsam::imuBias::ConstantBias}> - void addPrior(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); + template , gtsam::imuBias::ConstantBias}> + void addPrior(size_t key, const T &prior, const gtsam::noiseModel::Base *noiseModel); - // NonlinearFactorGraph - void printErrors(const gtsam::Values& values) const; - double error(const gtsam::Values& values) const; - double probPrime(const gtsam::Values& values) const; - gtsam::Ordering orderingCOLAMD() const; - // Ordering* orderingCOLAMDConstrained(const gtsam::Values& c, const std::map& constraints) const; - gtsam::GaussianFactorGraph* linearize(const gtsam::Values& values) const; - gtsam::NonlinearFactorGraph clone() const; + // NonlinearFactorGraph + void printErrors(const gtsam::Values &values) const; + double error(const gtsam::Values &values) const; + double probPrime(const gtsam::Values &values) const; + gtsam::Ordering orderingCOLAMD() const; + // Ordering* orderingCOLAMDConstrained(const gtsam::Values& c, const std::map& constraints) const; + gtsam::GaussianFactorGraph *linearize(const gtsam::Values &values) const; + gtsam::NonlinearFactorGraph clone() const; - // enabling serialization functionality - void serialize() const; -}; + // enabling serialization functionality + void serialize() const; + }; #include -virtual class NonlinearFactor { - // Factor base class - size_t size() const; - gtsam::KeyVector keys() const; - void print(string s) const; - void printKeys(string s) const; - // NonlinearFactor - bool equals(const gtsam::NonlinearFactor& other, double tol) const; - double error(const gtsam::Values& c) const; - size_t dim() const; - bool active(const gtsam::Values& c) const; - gtsam::GaussianFactor* linearize(const gtsam::Values& c) const; - gtsam::NonlinearFactor* clone() const; - // gtsam::NonlinearFactor* rekey(const gtsam::KeyVector& newKeys) const; //TODO: Conversion from KeyVector to std::vector does not happen -}; + virtual class NonlinearFactor + { + // Factor base class + size_t size() const; + gtsam::KeyVector keys() const; + void print(string s) const; + void printKeys(string s) const; + // NonlinearFactor + bool equals(const gtsam::NonlinearFactor &other, double tol) const; + double error(const gtsam::Values &c) const; + size_t dim() const; + bool active(const gtsam::Values &c) const; + gtsam::GaussianFactor *linearize(const gtsam::Values &c) const; + gtsam::NonlinearFactor *clone() const; + // gtsam::NonlinearFactor* rekey(const gtsam::KeyVector& newKeys) const; //TODO: Conversion from KeyVector to std::vector does not happen + }; #include -virtual class NoiseModelFactor: gtsam::NonlinearFactor { - bool equals(const gtsam::NoiseModelFactor& other, double tol) const; - gtsam::noiseModel::Base* noiseModel() const; - Vector unwhitenedError(const gtsam::Values& x) const; - Vector whitenedError(const gtsam::Values& x) const; -}; + virtual class NoiseModelFactor : gtsam::NonlinearFactor + { + bool equals(const gtsam::NoiseModelFactor &other, double tol) const; + gtsam::noiseModel::Base *noiseModel() const; + Vector unwhitenedError(const gtsam::Values &x) const; + Vector whitenedError(const gtsam::Values &x) const; + }; #include -class Values { - Values(); - Values(const gtsam::Values& other); + class Values + { + Values(); + Values(const gtsam::Values &other); - size_t size() const; - bool empty() const; - void clear(); - size_t dim() const; + size_t size() const; + bool empty() const; + void clear(); + size_t dim() const; - void print(string s) const; - bool equals(const gtsam::Values& other, double tol) const; + void print(string s) const; + bool equals(const gtsam::Values &other, double tol) const; - void insert(const gtsam::Values& values); - void update(const gtsam::Values& values); - void erase(size_t j); - void swap(gtsam::Values& values); + void insert(const gtsam::Values &values); + void update(const gtsam::Values &values); + void erase(size_t j); + void swap(gtsam::Values &values); - bool exists(size_t j) const; - gtsam::KeyVector keys() const; + bool exists(size_t j) const; + gtsam::KeyVector keys() const; - gtsam::VectorValues zeroVectors() const; + gtsam::VectorValues zeroVectors() const; - gtsam::Values retract(const gtsam::VectorValues& delta) const; - gtsam::VectorValues localCoordinates(const gtsam::Values& cp) const; + gtsam::Values retract(const gtsam::VectorValues &delta) const; + gtsam::VectorValues localCoordinates(const gtsam::Values &cp) const; - // enabling serialization functionality - void serialize() const; + // enabling serialization functionality + void serialize() const; - // New in 4.0, we have to specialize every insert/update/at to generate wrappers - // Instead of the old: - // void insert(size_t j, const gtsam::Value& value); - // void update(size_t j, const gtsam::Value& val); - // gtsam::Value at(size_t j) const; - - // The order is important: Vector has to precede Point2/Point3 so `atVector` - // can work for those fixed-size vectors. - void insert(size_t j, Vector vector); - void insert(size_t j, Matrix matrix); - void insert(size_t j, const gtsam::Point2& point2); - void insert(size_t j, const gtsam::Point3& point3); - void insert(size_t j, const gtsam::Rot2& rot2); - void insert(size_t j, const gtsam::Pose2& pose2); - void insert(size_t j, const gtsam::SO3& R); - void insert(size_t j, const gtsam::SO4& Q); - void insert(size_t j, const gtsam::SOn& P); - void insert(size_t j, const gtsam::Rot3& rot3); - void insert(size_t j, const gtsam::Pose3& pose3); - void insert(size_t j, const gtsam::Unit3& unit3); - void insert(size_t j, const gtsam::Cal3_S2& cal3_s2); - void insert(size_t j, const gtsam::Cal3DS2& cal3ds2); - void insert(size_t j, const gtsam::Cal3Bundler& cal3bundler); - void insert(size_t j, const gtsam::EssentialMatrix& essential_matrix); - void insert(size_t j, const gtsam::PinholeCameraCal3_S2& simple_camera); - void insert(size_t j, const gtsam::PinholeCamera& camera); - void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); - void insert(size_t j, const gtsam::NavState& nav_state); + // New in 4.0, we have to specialize every insert/update/at to generate wrappers + // Instead of the old: + // void insert(size_t j, const gtsam::Value& value); + // void update(size_t j, const gtsam::Value& val); + // gtsam::Value at(size_t j) const; - void update(size_t j, const gtsam::Point2& point2); - void update(size_t j, const gtsam::Point3& point3); - void update(size_t j, const gtsam::Rot2& rot2); - void update(size_t j, const gtsam::Pose2& pose2); - void update(size_t j, const gtsam::SO3& R); - void update(size_t j, const gtsam::SO4& Q); - void update(size_t j, const gtsam::SOn& P); - void update(size_t j, const gtsam::Rot3& rot3); - void update(size_t j, const gtsam::Pose3& pose3); - void update(size_t j, const gtsam::Unit3& unit3); - void update(size_t j, const gtsam::Cal3_S2& cal3_s2); - void update(size_t j, const gtsam::Cal3DS2& cal3ds2); - void update(size_t j, const gtsam::Cal3Bundler& cal3bundler); - void update(size_t j, const gtsam::EssentialMatrix& essential_matrix); - void update(size_t j, const gtsam::PinholeCameraCal3_S2& simple_camera); - void update(size_t j, const gtsam::PinholeCamera& 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); - void update(size_t j, Matrix matrix); + // The order is important: Vector has to precede Point2/Point3 so `atVector` + // can work for those fixed-size vectors. + void insert(size_t j, Vector vector); + void insert(size_t j, Matrix matrix); + void insert(size_t j, const gtsam::Point2 &point2); + void insert(size_t j, const gtsam::Point3 &point3); + void insert(size_t j, const gtsam::Rot2 &rot2); + void insert(size_t j, const gtsam::Pose2 &pose2); + void insert(size_t j, const gtsam::SO3 &R); + void insert(size_t j, const gtsam::SO4 &Q); + void insert(size_t j, const gtsam::SOn &P); + void insert(size_t j, const gtsam::Rot3 &rot3); + void insert(size_t j, const gtsam::Pose3 &pose3); + void insert(size_t j, const gtsam::Unit3 &unit3); + void insert(size_t j, const gtsam::Cal3_S2 &cal3_s2); + void insert(size_t j, const gtsam::Cal3DS2 &cal3ds2); + void insert(size_t j, const gtsam::Cal3Bundler &cal3bundler); + void insert(size_t j, const gtsam::EssentialMatrix &essential_matrix); + void insert(size_t j, const gtsam::PinholeCameraCal3_S2 &simple_camera); + void insert(size_t j, const gtsam::PinholeCamera &camera); + void insert(size_t j, const gtsam::imuBias::ConstantBias &constant_bias); + void insert(size_t j, const gtsam::NavState &nav_state); - template, gtsam::imuBias::ConstantBias, gtsam::NavState, Vector, Matrix}> - T at(size_t j); + void update(size_t j, const gtsam::Point2 &point2); + void update(size_t j, const gtsam::Point3 &point3); + void update(size_t j, const gtsam::Rot2 &rot2); + void update(size_t j, const gtsam::Pose2 &pose2); + void update(size_t j, const gtsam::SO3 &R); + void update(size_t j, const gtsam::SO4 &Q); + void update(size_t j, const gtsam::SOn &P); + void update(size_t j, const gtsam::Rot3 &rot3); + void update(size_t j, const gtsam::Pose3 &pose3); + void update(size_t j, const gtsam::Unit3 &unit3); + void update(size_t j, const gtsam::Cal3_S2 &cal3_s2); + void update(size_t j, const gtsam::Cal3DS2 &cal3ds2); + void update(size_t j, const gtsam::Cal3Bundler &cal3bundler); + void update(size_t j, const gtsam::EssentialMatrix &essential_matrix); + void update(size_t j, const gtsam::PinholeCameraCal3_S2 &simple_camera); + void update(size_t j, const gtsam::PinholeCamera &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); + void update(size_t j, Matrix matrix); - /// version for double - void insertDouble(size_t j, double c); - double atDouble(size_t j) const; -}; + template , gtsam::imuBias::ConstantBias, gtsam::NavState, Vector, Matrix}> + T at(size_t j); + + /// version for double + void insertDouble(size_t j, double c); + double atDouble(size_t j) const; + }; #include -class Marginals { - Marginals(const gtsam::NonlinearFactorGraph& graph, - const gtsam::Values& solution); - Marginals(const gtsam::GaussianFactorGraph& gfgraph, - const gtsam::Values& solution); - Marginals(const gtsam::GaussianFactorGraph& gfgraph, - const gtsam::VectorValues& solutionvec); + class Marginals + { + Marginals(const gtsam::NonlinearFactorGraph &graph, + const gtsam::Values &solution); + Marginals(const gtsam::GaussianFactorGraph &gfgraph, + const gtsam::Values &solution); + Marginals(const gtsam::GaussianFactorGraph &gfgraph, + const gtsam::VectorValues &solutionvec); - void print(string s) const; - Matrix marginalCovariance(size_t variable) const; - Matrix marginalInformation(size_t variable) const; - gtsam::JointMarginal jointMarginalCovariance(const gtsam::KeyVector& variables) const; - gtsam::JointMarginal jointMarginalInformation(const gtsam::KeyVector& variables) const; -}; + void print(string s) const; + Matrix marginalCovariance(size_t variable) const; + Matrix marginalInformation(size_t variable) const; + gtsam::JointMarginal jointMarginalCovariance(const gtsam::KeyVector &variables) const; + gtsam::JointMarginal jointMarginalInformation(const gtsam::KeyVector &variables) const; + }; -class JointMarginal { - Matrix at(size_t iVariable, size_t jVariable) const; - Matrix fullMatrix() const; - void print(string s) const; - void print() const; -}; + class JointMarginal + { + Matrix at(size_t iVariable, size_t jVariable) const; + Matrix fullMatrix() const; + void print(string s) const; + void print() const; + }; #include -virtual class LinearContainerFactor : gtsam::NonlinearFactor { + virtual class LinearContainerFactor : gtsam::NonlinearFactor + { - LinearContainerFactor(gtsam::GaussianFactor* factor, const gtsam::Values& linearizationPoint); - LinearContainerFactor(gtsam::GaussianFactor* factor); + LinearContainerFactor(gtsam::GaussianFactor *factor, const gtsam::Values &linearizationPoint); + LinearContainerFactor(gtsam::GaussianFactor *factor); - gtsam::GaussianFactor* factor() const; -// const boost::optional& linearizationPoint() const; + gtsam::GaussianFactor *factor() const; + // const boost::optional& linearizationPoint() const; - bool isJacobian() const; - gtsam::JacobianFactor* toJacobian() const; - gtsam::HessianFactor* toHessian() const; + bool isJacobian() const; + gtsam::JacobianFactor *toJacobian() const; + gtsam::HessianFactor *toHessian() const; - static gtsam::NonlinearFactorGraph ConvertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph, - const gtsam::Values& linearizationPoint); + static gtsam::NonlinearFactorGraph ConvertLinearGraph(const gtsam::GaussianFactorGraph &linear_graph, + const gtsam::Values &linearizationPoint); - static gtsam::NonlinearFactorGraph ConvertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph); + static gtsam::NonlinearFactorGraph ConvertLinearGraph(const gtsam::GaussianFactorGraph &linear_graph); - // enabling serialization functionality - void serializable() const; -}; // \class LinearContainerFactor + // enabling serialization functionality + void serializable() const; + }; // \class LinearContainerFactor // Summarization functionality //#include @@ -2241,196 +2327,210 @@ virtual class LinearContainerFactor : gtsam::NonlinearFactor { // Nonlinear optimizers //************************************************************************* #include -virtual class NonlinearOptimizerParams { - NonlinearOptimizerParams(); - void print(string s) const; + virtual class NonlinearOptimizerParams + { + NonlinearOptimizerParams(); + void print(string s) const; - int getMaxIterations() const; - double getRelativeErrorTol() const; - double getAbsoluteErrorTol() const; - double getErrorTol() const; - string getVerbosity() const; + int getMaxIterations() const; + double getRelativeErrorTol() const; + double getAbsoluteErrorTol() const; + double getErrorTol() const; + string getVerbosity() const; - void setMaxIterations(int value); - void setRelativeErrorTol(double value); - void setAbsoluteErrorTol(double value); - void setErrorTol(double value); - void setVerbosity(string s); + void setMaxIterations(int value); + void setRelativeErrorTol(double value); + void setAbsoluteErrorTol(double value); + void setErrorTol(double value); + void setVerbosity(string s); - string getLinearSolverType() const; - void setLinearSolverType(string solver); + string getLinearSolverType() const; + void setLinearSolverType(string solver); - void setIterativeParams(gtsam::IterativeOptimizationParameters* params); - void setOrdering(const gtsam::Ordering& ordering); - string getOrderingType() const; - void setOrderingType(string ordering); + void setIterativeParams(gtsam::IterativeOptimizationParameters *params); + void setOrdering(const gtsam::Ordering &ordering); + string getOrderingType() const; + void setOrderingType(string ordering); - bool isMultifrontal() const; - bool isSequential() const; - bool isCholmod() const; - bool isIterative() const; -}; + bool isMultifrontal() const; + bool isSequential() const; + bool isCholmod() const; + bool isIterative() const; + }; -bool checkConvergence(double relativeErrorTreshold, - double absoluteErrorTreshold, double errorThreshold, - double currentError, double newError); -bool checkConvergence(const gtsam::NonlinearOptimizerParams& params, - double currentError, double newError); + bool checkConvergence(double relativeErrorTreshold, + double absoluteErrorTreshold, double errorThreshold, + double currentError, double newError); + bool checkConvergence(const gtsam::NonlinearOptimizerParams ¶ms, + double currentError, double newError); #include -virtual class GaussNewtonParams : gtsam::NonlinearOptimizerParams { - GaussNewtonParams(); -}; + virtual class GaussNewtonParams : gtsam::NonlinearOptimizerParams + { + GaussNewtonParams(); + }; #include -virtual class LevenbergMarquardtParams : gtsam::NonlinearOptimizerParams { - LevenbergMarquardtParams(); + virtual class LevenbergMarquardtParams : gtsam::NonlinearOptimizerParams + { + LevenbergMarquardtParams(); - bool getDiagonalDamping() const; - double getlambdaFactor() const; - double getlambdaInitial() const; - double getlambdaLowerBound() const; - double getlambdaUpperBound() const; - bool getUseFixedLambdaFactor(); - string getLogFile() const; - string getVerbosityLM() const; + bool getDiagonalDamping() const; + double getlambdaFactor() const; + double getlambdaInitial() const; + double getlambdaLowerBound() const; + double getlambdaUpperBound() const; + bool getUseFixedLambdaFactor(); + string getLogFile() const; + string getVerbosityLM() const; - void setDiagonalDamping(bool flag); - void setlambdaFactor(double value); - void setlambdaInitial(double value); - void setlambdaLowerBound(double value); - void setlambdaUpperBound(double value); - void setUseFixedLambdaFactor(bool flag); - void setLogFile(string s); - void setVerbosityLM(string s); + void setDiagonalDamping(bool flag); + void setlambdaFactor(double value); + void setlambdaInitial(double value); + void setlambdaLowerBound(double value); + void setlambdaUpperBound(double value); + void setUseFixedLambdaFactor(bool flag); + void setLogFile(string s); + void setVerbosityLM(string s); - static gtsam::LevenbergMarquardtParams LegacyDefaults(); - static gtsam::LevenbergMarquardtParams CeresDefaults(); + static gtsam::LevenbergMarquardtParams LegacyDefaults(); + static gtsam::LevenbergMarquardtParams CeresDefaults(); - static gtsam::LevenbergMarquardtParams EnsureHasOrdering( - gtsam::LevenbergMarquardtParams params, - const gtsam::NonlinearFactorGraph& graph); - static gtsam::LevenbergMarquardtParams ReplaceOrdering( - gtsam::LevenbergMarquardtParams params, const gtsam::Ordering& ordering); -}; + static gtsam::LevenbergMarquardtParams EnsureHasOrdering( + gtsam::LevenbergMarquardtParams params, + const gtsam::NonlinearFactorGraph &graph); + static gtsam::LevenbergMarquardtParams ReplaceOrdering( + gtsam::LevenbergMarquardtParams params, const gtsam::Ordering &ordering); + }; #include -virtual class DoglegParams : gtsam::NonlinearOptimizerParams { - DoglegParams(); + virtual class DoglegParams : gtsam::NonlinearOptimizerParams + { + DoglegParams(); - double getDeltaInitial() const; - string getVerbosityDL() const; + double getDeltaInitial() const; + string getVerbosityDL() const; - void setDeltaInitial(double deltaInitial) const; - void setVerbosityDL(string verbosityDL) const; -}; + void setDeltaInitial(double deltaInitial) const; + void setVerbosityDL(string verbosityDL) const; + }; #include -virtual class NonlinearOptimizer { - gtsam::Values optimize(); - gtsam::Values optimizeSafely(); - double error() const; - int iterations() const; - gtsam::Values values() const; - gtsam::NonlinearFactorGraph graph() const; - gtsam::GaussianFactorGraph* iterate() const; -}; + virtual class NonlinearOptimizer + { + gtsam::Values optimize(); + gtsam::Values optimizeSafely(); + double error() const; + int iterations() const; + gtsam::Values values() const; + gtsam::NonlinearFactorGraph graph() const; + gtsam::GaussianFactorGraph *iterate() const; + }; #include -virtual class GaussNewtonOptimizer : gtsam::NonlinearOptimizer { - GaussNewtonOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues); - GaussNewtonOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const gtsam::GaussNewtonParams& params); -}; + virtual class GaussNewtonOptimizer : gtsam::NonlinearOptimizer + { + GaussNewtonOptimizer(const gtsam::NonlinearFactorGraph &graph, const gtsam::Values &initialValues); + GaussNewtonOptimizer(const gtsam::NonlinearFactorGraph &graph, const gtsam::Values &initialValues, const gtsam::GaussNewtonParams ¶ms); + }; #include -virtual class DoglegOptimizer : gtsam::NonlinearOptimizer { - DoglegOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues); - DoglegOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const gtsam::DoglegParams& params); - double getDelta() const; -}; + virtual class DoglegOptimizer : gtsam::NonlinearOptimizer + { + DoglegOptimizer(const gtsam::NonlinearFactorGraph &graph, const gtsam::Values &initialValues); + DoglegOptimizer(const gtsam::NonlinearFactorGraph &graph, const gtsam::Values &initialValues, const gtsam::DoglegParams ¶ms); + double getDelta() const; + }; #include -virtual class LevenbergMarquardtOptimizer : gtsam::NonlinearOptimizer { - LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues); - LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const gtsam::LevenbergMarquardtParams& params); - double lambda() const; - void print(string str) const; -}; + virtual class LevenbergMarquardtOptimizer : gtsam::NonlinearOptimizer + { + LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph &graph, const gtsam::Values &initialValues); + LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph &graph, const gtsam::Values &initialValues, const gtsam::LevenbergMarquardtParams ¶ms); + double lambda() const; + void print(string str) const; + }; #include -class ISAM2GaussNewtonParams { - ISAM2GaussNewtonParams(); + class ISAM2GaussNewtonParams + { + ISAM2GaussNewtonParams(); - void print(string str) const; + void print(string str) const; - /** Getters and Setters for all properties */ - double getWildfireThreshold() const; - void setWildfireThreshold(double wildfireThreshold); -}; + /** Getters and Setters for all properties */ + double getWildfireThreshold() const; + void setWildfireThreshold(double wildfireThreshold); + }; -class ISAM2DoglegParams { - ISAM2DoglegParams(); + class ISAM2DoglegParams + { + ISAM2DoglegParams(); - void print(string str) const; + void print(string str) const; - /** Getters and Setters for all properties */ - double getWildfireThreshold() const; - void setWildfireThreshold(double wildfireThreshold); - double getInitialDelta() const; - void setInitialDelta(double initialDelta); - string getAdaptationMode() const; - void setAdaptationMode(string adaptationMode); - bool isVerbose() const; - void setVerbose(bool verbose); -}; + /** Getters and Setters for all properties */ + double getWildfireThreshold() const; + void setWildfireThreshold(double wildfireThreshold); + double getInitialDelta() const; + void setInitialDelta(double initialDelta); + string getAdaptationMode() const; + void setAdaptationMode(string adaptationMode); + bool isVerbose() const; + void setVerbose(bool verbose); + }; -class ISAM2ThresholdMapValue { - ISAM2ThresholdMapValue(char c, Vector thresholds); - ISAM2ThresholdMapValue(const gtsam::ISAM2ThresholdMapValue& other); -}; + class ISAM2ThresholdMapValue + { + ISAM2ThresholdMapValue(char c, Vector thresholds); + ISAM2ThresholdMapValue(const gtsam::ISAM2ThresholdMapValue &other); + }; -class ISAM2ThresholdMap { - ISAM2ThresholdMap(); - ISAM2ThresholdMap(const gtsam::ISAM2ThresholdMap& other); + class ISAM2ThresholdMap + { + ISAM2ThresholdMap(); + ISAM2ThresholdMap(const gtsam::ISAM2ThresholdMap &other); - // Note: no print function + // Note: no print function - // common STL methods - size_t size() const; - bool empty() const; - void clear(); + // common STL methods + size_t size() const; + bool empty() const; + void clear(); - // structure specific methods - void insert(const gtsam::ISAM2ThresholdMapValue& value) const; -}; + // structure specific methods + void insert(const gtsam::ISAM2ThresholdMapValue &value) const; + }; -class ISAM2Params { - ISAM2Params(); + class ISAM2Params + { + ISAM2Params(); - void print(string str) const; + void print(string str) const; - /** Getters and Setters for all properties */ - void setOptimizationParams(const gtsam::ISAM2GaussNewtonParams& gauss_newton__params); - void setOptimizationParams(const gtsam::ISAM2DoglegParams& dogleg_params); - void setRelinearizeThreshold(double threshold); - void setRelinearizeThreshold(const gtsam::ISAM2ThresholdMap& threshold_map); - int getRelinearizeSkip() const; - void setRelinearizeSkip(int relinearizeSkip); - bool isEnableRelinearization() const; - void setEnableRelinearization(bool enableRelinearization); - bool isEvaluateNonlinearError() const; - void setEvaluateNonlinearError(bool evaluateNonlinearError); - string getFactorization() const; - void setFactorization(string factorization); - bool isCacheLinearizedFactors() const; - void setCacheLinearizedFactors(bool cacheLinearizedFactors); - bool isEnableDetailedResults() const; - void setEnableDetailedResults(bool enableDetailedResults); - bool isEnablePartialRelinearizationCheck() const; - void setEnablePartialRelinearizationCheck(bool enablePartialRelinearizationCheck); -}; + /** Getters and Setters for all properties */ + void setOptimizationParams(const gtsam::ISAM2GaussNewtonParams &gauss_newton__params); + void setOptimizationParams(const gtsam::ISAM2DoglegParams &dogleg_params); + void setRelinearizeThreshold(double threshold); + void setRelinearizeThreshold(const gtsam::ISAM2ThresholdMap &threshold_map); + int getRelinearizeSkip() const; + void setRelinearizeSkip(int relinearizeSkip); + bool isEnableRelinearization() const; + void setEnableRelinearization(bool enableRelinearization); + bool isEvaluateNonlinearError() const; + void setEvaluateNonlinearError(bool evaluateNonlinearError); + string getFactorization() const; + void setFactorization(string factorization); + bool isCacheLinearizedFactors() const; + void setCacheLinearizedFactors(bool cacheLinearizedFactors); + bool isEnableDetailedResults() const; + void setEnableDetailedResults(bool enableDetailedResults); + bool isEnablePartialRelinearizationCheck() const; + void setEnablePartialRelinearizationCheck(bool enablePartialRelinearizationCheck); + }; -class ISAM2Clique { + class ISAM2Clique + { //Constructors ISAM2Clique(); @@ -2438,74 +2538,77 @@ class ISAM2Clique { //Standard Interface Vector gradientContribution() const; void print(string s); -}; + }; -class ISAM2Result { - ISAM2Result(); + class ISAM2Result + { + ISAM2Result(); - void print(string str) const; + void print(string str) const; - /** Getters and Setters for all properties */ - size_t getVariablesRelinearized() const; - size_t getVariablesReeliminated() const; - size_t getCliques() const; - double getErrorBefore() const; - double getErrorAfter() const; -}; + /** Getters and Setters for all properties */ + size_t getVariablesRelinearized() const; + size_t getVariablesReeliminated() const; + size_t getCliques() const; + double getErrorBefore() const; + double getErrorAfter() const; + }; -class ISAM2 { - ISAM2(); - ISAM2(const gtsam::ISAM2Params& params); - ISAM2(const gtsam::ISAM2& other); + class ISAM2 + { + ISAM2(); + ISAM2(const gtsam::ISAM2Params ¶ms); + ISAM2(const gtsam::ISAM2 &other); - bool equals(const gtsam::ISAM2& other, double tol) const; - void print(string s) const; - void printStats() const; - void saveGraph(string s) const; + bool equals(const gtsam::ISAM2 &other, double tol) const; + void print(string s) const; + void printStats() const; + void saveGraph(string s) const; - gtsam::ISAM2Result update(); - gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta); - gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices); - gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices, const gtsam::KeyGroupMap& constrainedKeys); - gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices, gtsam::KeyGroupMap& constrainedKeys, const gtsam::KeyList& noRelinKeys); - gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices, gtsam::KeyGroupMap& constrainedKeys, const gtsam::KeyList& noRelinKeys, const gtsam::KeyList& extraReelimKeys); - gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices, gtsam::KeyGroupMap& constrainedKeys, const gtsam::KeyList& noRelinKeys, const gtsam::KeyList& extraReelimKeys, bool force_relinearize); + gtsam::ISAM2Result update(); + gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph &newFactors, const gtsam::Values &newTheta); + gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph &newFactors, const gtsam::Values &newTheta, const gtsam::FactorIndices &removeFactorIndices); + gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph &newFactors, const gtsam::Values &newTheta, const gtsam::FactorIndices &removeFactorIndices, const gtsam::KeyGroupMap &constrainedKeys); + gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph &newFactors, const gtsam::Values &newTheta, const gtsam::FactorIndices &removeFactorIndices, gtsam::KeyGroupMap &constrainedKeys, const gtsam::KeyList &noRelinKeys); + gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph &newFactors, const gtsam::Values &newTheta, const gtsam::FactorIndices &removeFactorIndices, gtsam::KeyGroupMap &constrainedKeys, const gtsam::KeyList &noRelinKeys, const gtsam::KeyList &extraReelimKeys); + gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph &newFactors, const gtsam::Values &newTheta, const gtsam::FactorIndices &removeFactorIndices, gtsam::KeyGroupMap &constrainedKeys, const gtsam::KeyList &noRelinKeys, const gtsam::KeyList &extraReelimKeys, bool force_relinearize); - gtsam::Values getLinearizationPoint() const; - gtsam::Values calculateEstimate() const; - template , - Vector, Matrix}> - VALUE calculateEstimate(size_t key) const; - gtsam::Values calculateBestEstimate() const; - Matrix marginalCovariance(size_t key) const; - gtsam::VectorValues getDelta() const; - gtsam::NonlinearFactorGraph getFactorsUnsafe() const; - gtsam::VariableIndex getVariableIndex() const; - gtsam::ISAM2Params params() const; -}; + gtsam::Values getLinearizationPoint() const; + gtsam::Values calculateEstimate() const; + template , + Vector, Matrix}> + VALUE calculateEstimate(size_t key) const; + gtsam::Values calculateBestEstimate() const; + Matrix marginalCovariance(size_t key) const; + gtsam::VectorValues getDelta() const; + gtsam::NonlinearFactorGraph getFactorsUnsafe() const; + gtsam::VariableIndex getVariableIndex() const; + gtsam::ISAM2Params params() const; + }; #include -class NonlinearISAM { - NonlinearISAM(); - NonlinearISAM(int reorderInterval); - void print(string s) const; - void printStats() const; - void saveGraph(string s) const; - gtsam::Values estimate() const; - Matrix marginalCovariance(size_t key) const; - int reorderInterval() const; - int reorderCounter() const; - void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& initialValues); - void reorder_relinearize(); + class NonlinearISAM + { + NonlinearISAM(); + NonlinearISAM(int reorderInterval); + void print(string s) const; + void printStats() const; + void saveGraph(string s) const; + gtsam::Values estimate() const; + Matrix marginalCovariance(size_t key) const; + int reorderInterval() const; + int reorderCounter() const; + void update(const gtsam::NonlinearFactorGraph &newFactors, const gtsam::Values &initialValues); + void reorder_relinearize(); - // These might be expensive as instead of a reference the wrapper will make a copy - gtsam::GaussianISAM bayesTree() const; - gtsam::Values getLinearizationPoint() const; - gtsam::NonlinearFactorGraph getFactorsUnsafe() const; -}; + // These might be expensive as instead of a reference the wrapper will make a copy + gtsam::GaussianISAM bayesTree() const; + gtsam::Values getLinearizationPoint() const; + gtsam::NonlinearFactorGraph getFactorsUnsafe() const; + }; //************************************************************************* // Nonlinear factor types @@ -2514,917 +2617,966 @@ class NonlinearISAM { #include #include -template}> -virtual class PriorFactor : gtsam::NoiseModelFactor { - PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); - T prior() const; - - // enabling serialization functionality - void serialize() const; -}; + template }> + virtual class PriorFactor : gtsam::NoiseModelFactor + { + PriorFactor(size_t key, const T &prior, const gtsam::noiseModel::Base *noiseModel); + T prior() const; + // enabling serialization functionality + void serialize() const; + }; #include -template -virtual class BetweenFactor : gtsam::NoiseModelFactor { - BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel); - T measured() const; + template + virtual class BetweenFactor : gtsam::NoiseModelFactor + { + BetweenFactor(size_t key1, size_t key2, const T &relativePose, const gtsam::noiseModel::Base *noiseModel); + T measured() const; - // enabling serialization functionality - void serialize() const; -}; + // enabling serialization functionality + void serialize() const; + }; #include -template -virtual class NonlinearEquality : gtsam::NoiseModelFactor { - // Constructor - forces exact evaluation - NonlinearEquality(size_t j, const T& feasible); - // Constructor - allows inexact evaluation - NonlinearEquality(size_t j, const T& feasible, double error_gain); + template + virtual class NonlinearEquality : gtsam::NoiseModelFactor + { + // Constructor - forces exact evaluation + NonlinearEquality(size_t j, const T &feasible); + // Constructor - allows inexact evaluation + NonlinearEquality(size_t j, const T &feasible, double error_gain); - // enabling serialization functionality - void serialize() const; -}; + // enabling serialization functionality + void serialize() const; + }; #include -template -virtual class RangeFactor : gtsam::NoiseModelFactor { - RangeFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel); + template + virtual class RangeFactor : gtsam::NoiseModelFactor + { + RangeFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base *noiseModel); - // enabling serialization functionality - void serialize() const; -}; - -typedef gtsam::RangeFactor RangeFactor2D; -typedef gtsam::RangeFactor RangeFactor3D; -typedef gtsam::RangeFactor RangeFactorPose2; -typedef gtsam::RangeFactor RangeFactorPose3; -typedef gtsam::RangeFactor RangeFactorCalibratedCameraPoint; -typedef gtsam::RangeFactor RangeFactorSimpleCameraPoint; -typedef gtsam::RangeFactor RangeFactorCalibratedCamera; -typedef gtsam::RangeFactor RangeFactorSimpleCamera; + // enabling serialization functionality + void serialize() const; + }; + typedef gtsam::RangeFactor RangeFactor2D; + typedef gtsam::RangeFactor RangeFactor3D; + typedef gtsam::RangeFactor RangeFactorPose2; + typedef gtsam::RangeFactor RangeFactorPose3; + typedef gtsam::RangeFactor RangeFactorCalibratedCameraPoint; + typedef gtsam::RangeFactor RangeFactorSimpleCameraPoint; + typedef gtsam::RangeFactor RangeFactorCalibratedCamera; + typedef gtsam::RangeFactor RangeFactorSimpleCamera; #include -template -virtual class RangeFactorWithTransform : gtsam::NoiseModelFactor { - RangeFactorWithTransform(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel, const POSE& body_T_sensor); + template + virtual class RangeFactorWithTransform : gtsam::NoiseModelFactor + { + RangeFactorWithTransform(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base *noiseModel, const POSE &body_T_sensor); - // enabling serialization functionality - void serialize() const; -}; + // enabling serialization functionality + void serialize() const; + }; -typedef gtsam::RangeFactorWithTransform RangeFactorWithTransform2D; -typedef gtsam::RangeFactorWithTransform RangeFactorWithTransform3D; -typedef gtsam::RangeFactorWithTransform RangeFactorWithTransformPose2; -typedef gtsam::RangeFactorWithTransform RangeFactorWithTransformPose3; + typedef gtsam::RangeFactorWithTransform RangeFactorWithTransform2D; + typedef gtsam::RangeFactorWithTransform RangeFactorWithTransform3D; + typedef gtsam::RangeFactorWithTransform RangeFactorWithTransformPose2; + typedef gtsam::RangeFactorWithTransform RangeFactorWithTransformPose3; #include -template -virtual class BearingFactor : gtsam::NoiseModelFactor { - BearingFactor(size_t key1, size_t key2, const BEARING& measured, const gtsam::noiseModel::Base* noiseModel); + template + virtual class BearingFactor : gtsam::NoiseModelFactor + { + BearingFactor(size_t key1, size_t key2, const BEARING &measured, const gtsam::noiseModel::Base *noiseModel); - // enabling serialization functionality - void serialize() const; -}; + // enabling serialization functionality + void serialize() const; + }; -typedef gtsam::BearingFactor BearingFactor2D; -typedef gtsam::BearingFactor BearingFactorPose2; + typedef gtsam::BearingFactor BearingFactor2D; + typedef gtsam::BearingFactor BearingFactorPose2; #include -template -class BearingRange { - BearingRange(const BEARING& b, const RANGE& r); - BEARING bearing() const; - RANGE range() const; - static This Measure(const POSE& pose, const POINT& point); - static BEARING MeasureBearing(const POSE& pose, const POINT& point); - static RANGE MeasureRange(const POSE& pose, const POINT& point); - void print(string s) const; -}; + template + class BearingRange + { + BearingRange(const BEARING &b, const RANGE &r); + BEARING bearing() const; + RANGE range() const; + static This Measure(const POSE &pose, const POINT &point); + static BEARING MeasureBearing(const POSE &pose, const POINT &point); + static RANGE MeasureRange(const POSE &pose, const POINT &point); + void print(string s) const; + }; -typedef gtsam::BearingRange BearingRange2D; + typedef gtsam::BearingRange BearingRange2D; #include -template -virtual class BearingRangeFactor : gtsam::NoiseModelFactor { - BearingRangeFactor(size_t poseKey, size_t pointKey, - const BEARING& measuredBearing, const RANGE& measuredRange, - const gtsam::noiseModel::Base* noiseModel); + template + virtual class BearingRangeFactor : gtsam::NoiseModelFactor + { + BearingRangeFactor(size_t poseKey, size_t pointKey, + const BEARING &measuredBearing, const RANGE &measuredRange, + const gtsam::noiseModel::Base *noiseModel); - // enabling serialization functionality - void serialize() const; -}; - -typedef gtsam::BearingRangeFactor BearingRangeFactor2D; -typedef gtsam::BearingRangeFactor BearingRangeFactorPose2; + // enabling serialization functionality + void serialize() const; + }; + typedef gtsam::BearingRangeFactor BearingRangeFactor2D; + typedef gtsam::BearingRangeFactor BearingRangeFactorPose2; #include -template -virtual class GenericProjectionFactor : gtsam::NoiseModelFactor { - GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, - size_t poseKey, size_t pointKey, const CALIBRATION* k); - GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, - size_t poseKey, size_t pointKey, const CALIBRATION* k, const POSE& body_P_sensor); + template + virtual class GenericProjectionFactor : gtsam::NoiseModelFactor + { + GenericProjectionFactor(const gtsam::Point2 &measured, const gtsam::noiseModel::Base *noiseModel, + size_t poseKey, size_t pointKey, const CALIBRATION *k); + GenericProjectionFactor(const gtsam::Point2 &measured, const gtsam::noiseModel::Base *noiseModel, + size_t poseKey, size_t pointKey, const CALIBRATION *k, const POSE &body_P_sensor); - GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, - size_t poseKey, size_t pointKey, const CALIBRATION* k, bool throwCheirality, bool verboseCheirality); - GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, - size_t poseKey, size_t pointKey, const CALIBRATION* k, bool throwCheirality, bool verboseCheirality, - const POSE& body_P_sensor); + GenericProjectionFactor(const gtsam::Point2 &measured, const gtsam::noiseModel::Base *noiseModel, + size_t poseKey, size_t pointKey, const CALIBRATION *k, bool throwCheirality, bool verboseCheirality); + GenericProjectionFactor(const gtsam::Point2 &measured, const gtsam::noiseModel::Base *noiseModel, + size_t poseKey, size_t pointKey, const CALIBRATION *k, bool throwCheirality, bool verboseCheirality, + const POSE &body_P_sensor); - gtsam::Point2 measured() const; - CALIBRATION* calibration() const; - bool verboseCheirality() const; - bool throwCheirality() const; - - // enabling serialization functionality - void serialize() const; -}; -typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3_S2; -typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3DS2; + gtsam::Point2 measured() const; + CALIBRATION *calibration() const; + bool verboseCheirality() const; + bool throwCheirality() const; + // enabling serialization functionality + void serialize() const; + }; + typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3_S2; + typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3DS2; #include -template -virtual class GeneralSFMFactor : gtsam::NoiseModelFactor { - GeneralSFMFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model, size_t cameraKey, size_t landmarkKey); - gtsam::Point2 measured() const; -}; -typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3_S2; -typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3DS2; -typedef gtsam::GeneralSFMFactor, gtsam::Point3> GeneralSFMFactorCal3Bundler; + template + virtual class GeneralSFMFactor : gtsam::NoiseModelFactor + { + GeneralSFMFactor(const gtsam::Point2 &measured, const gtsam::noiseModel::Base *model, size_t cameraKey, size_t landmarkKey); + gtsam::Point2 measured() const; + }; + typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3_S2; + typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3DS2; + typedef gtsam::GeneralSFMFactor, gtsam::Point3> GeneralSFMFactorCal3Bundler; -template -virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor { - GeneralSFMFactor2(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model, size_t poseKey, size_t landmarkKey, size_t calibKey); - gtsam::Point2 measured() const; + template + virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor + { + GeneralSFMFactor2(const gtsam::Point2 &measured, const gtsam::noiseModel::Base *model, size_t poseKey, size_t landmarkKey, size_t calibKey); + gtsam::Point2 measured() const; - // enabling serialization functionality - void serialize() const; -}; + // enabling serialization functionality + void serialize() const; + }; #include -class SmartProjectionParams { - SmartProjectionParams(); - // TODO(frank): make these work: - // void setLinearizationMode(LinearizationMode linMode); - // void setDegeneracyMode(DegeneracyMode degMode); - void setRankTolerance(double rankTol); - void setEnableEPI(bool enableEPI); - void setLandmarkDistanceThreshold(bool landmarkDistanceThreshold); - void setDynamicOutlierRejectionThreshold(bool dynOutRejectionThreshold); -}; + class SmartProjectionParams + { + SmartProjectionParams(); + // TODO(frank): make these work: + // void setLinearizationMode(LinearizationMode linMode); + // void setDegeneracyMode(DegeneracyMode degMode); + void setRankTolerance(double rankTol); + void setEnableEPI(bool enableEPI); + void setLandmarkDistanceThreshold(bool landmarkDistanceThreshold); + void setDynamicOutlierRejectionThreshold(bool dynOutRejectionThreshold); + }; #include -template -virtual class SmartProjectionPoseFactor: gtsam::NonlinearFactor { + template + virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor + { - SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, - const CALIBRATION* K); - SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, - const CALIBRATION* K, - const gtsam::Pose3& body_P_sensor); - SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, - const CALIBRATION* K, - const gtsam::SmartProjectionParams& params); - SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, - const CALIBRATION* K, - const gtsam::Pose3& body_P_sensor, - const gtsam::SmartProjectionParams& params); + SmartProjectionPoseFactor(const gtsam::noiseModel::Base *noise, + const CALIBRATION *K); + SmartProjectionPoseFactor(const gtsam::noiseModel::Base *noise, + const CALIBRATION *K, + const gtsam::Pose3 &body_P_sensor); + SmartProjectionPoseFactor(const gtsam::noiseModel::Base *noise, + const CALIBRATION *K, + const gtsam::SmartProjectionParams ¶ms); + SmartProjectionPoseFactor(const gtsam::noiseModel::Base *noise, + const CALIBRATION *K, + const gtsam::Pose3 &body_P_sensor, + const gtsam::SmartProjectionParams ¶ms); - void add(const gtsam::Point2& measured_i, size_t poseKey_i); + void add(const gtsam::Point2 &measured_i, size_t poseKey_i); - // enabling serialization functionality - //void serialize() const; -}; - -typedef gtsam::SmartProjectionPoseFactor SmartProjectionPose3Factor; + // enabling serialization functionality + //void serialize() const; + }; + typedef gtsam::SmartProjectionPoseFactor SmartProjectionPose3Factor; #include -template -virtual class GenericStereoFactor : gtsam::NoiseModelFactor { - GenericStereoFactor(const gtsam::StereoPoint2& measured, const gtsam::noiseModel::Base* noiseModel, - size_t poseKey, size_t landmarkKey, const gtsam::Cal3_S2Stereo* K); - gtsam::StereoPoint2 measured() const; - gtsam::Cal3_S2Stereo* calibration() const; + template + virtual class GenericStereoFactor : gtsam::NoiseModelFactor + { + GenericStereoFactor(const gtsam::StereoPoint2 &measured, const gtsam::noiseModel::Base *noiseModel, + size_t poseKey, size_t landmarkKey, const gtsam::Cal3_S2Stereo *K); + gtsam::StereoPoint2 measured() const; + gtsam::Cal3_S2Stereo *calibration() const; - // enabling serialization functionality - void serialize() const; -}; -typedef gtsam::GenericStereoFactor GenericStereoFactor3D; + // enabling serialization functionality + void serialize() const; + }; + typedef gtsam::GenericStereoFactor GenericStereoFactor3D; #include -template -virtual class PoseTranslationPrior : gtsam::NoiseModelFactor { - PoseTranslationPrior(size_t key, const POSE& pose_z, const gtsam::noiseModel::Base* noiseModel); -}; + template + virtual class PoseTranslationPrior : gtsam::NoiseModelFactor + { + PoseTranslationPrior(size_t key, const POSE &pose_z, const gtsam::noiseModel::Base *noiseModel); + }; -typedef gtsam::PoseTranslationPrior PoseTranslationPrior2D; -typedef gtsam::PoseTranslationPrior PoseTranslationPrior3D; + typedef gtsam::PoseTranslationPrior PoseTranslationPrior2D; + typedef gtsam::PoseTranslationPrior PoseTranslationPrior3D; #include -template -virtual class PoseRotationPrior : gtsam::NoiseModelFactor { - PoseRotationPrior(size_t key, const POSE& pose_z, const gtsam::noiseModel::Base* noiseModel); -}; + template + virtual class PoseRotationPrior : gtsam::NoiseModelFactor + { + PoseRotationPrior(size_t key, const POSE &pose_z, const gtsam::noiseModel::Base *noiseModel); + }; -typedef gtsam::PoseRotationPrior PoseRotationPrior2D; -typedef gtsam::PoseRotationPrior PoseRotationPrior3D; + typedef gtsam::PoseRotationPrior PoseRotationPrior2D; + typedef gtsam::PoseRotationPrior PoseRotationPrior3D; #include -virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor { - EssentialMatrixFactor(size_t key, const gtsam::Point2& pA, const gtsam::Point2& pB, - const gtsam::noiseModel::Base* noiseModel); -}; + virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor + { + EssentialMatrixFactor(size_t key, const gtsam::Point2 &pA, const gtsam::Point2 &pB, + const gtsam::noiseModel::Base *noiseModel); + }; #include -class SfmTrack { - SfmTrack(); - SfmTrack(const gtsam::Point3& pt); - const Point3& point3() const; + class SfmTrack + { + SfmTrack(); + SfmTrack(const gtsam::Point3 &pt); + const Point3 &point3() const; - double r; - double g; - double b; - // TODO Need to close wrap#10 to allow this to work. - // std::vector> measurements; + double r; + double g; + double b; + // TODO Need to close wrap#10 to allow this to work. + // std::vector> measurements; - size_t number_measurements() const; - pair measurement(size_t idx) const; - pair siftIndex(size_t idx) const; - void add_measurement(size_t idx, const gtsam::Point2& m); -}; + size_t number_measurements() const; + pair measurement(size_t idx) const; + pair siftIndex(size_t idx) const; + void add_measurement(size_t idx, const gtsam::Point2 &m); + }; -class SfmData { - SfmData(); - size_t number_cameras() const; - size_t number_tracks() const; - gtsam::PinholeCamera camera(size_t idx) const; - gtsam::SfmTrack track(size_t idx) const; - void add_track(const gtsam::SfmTrack& t) ; - void add_camera(const gtsam::SfmCamera& cam); -}; + class SfmData + { + SfmData(); + size_t number_cameras() const; + size_t number_tracks() const; + gtsam::PinholeCamera camera(size_t idx) const; + gtsam::SfmTrack track(size_t idx) const; + void add_track(const gtsam::SfmTrack &t); + void add_camera(const gtsam::SfmCamera &cam); + }; -gtsam::SfmData readBal(string filename); -bool writeBAL(string filename, gtsam::SfmData& data); -gtsam::Values initialCamerasEstimate(const gtsam::SfmData& db); -gtsam::Values initialCamerasAndPointsEstimate(const gtsam::SfmData& db); + gtsam::SfmData readBal(string filename); + bool writeBAL(string filename, gtsam::SfmData &data); + gtsam::Values initialCamerasEstimate(const gtsam::SfmData &db); + gtsam::Values initialCamerasAndPointsEstimate(const gtsam::SfmData &db); -pair load2D(string filename, - gtsam::noiseModel::Diagonal* model, int maxIndex, bool addNoise, bool smart); -pair load2D(string filename, - gtsam::noiseModel::Diagonal* model, int maxIndex, bool addNoise); -pair load2D(string filename, - gtsam::noiseModel::Diagonal* model, int maxIndex); -pair load2D(string filename, - gtsam::noiseModel::Diagonal* model); -pair load2D(string filename); -pair load2D_robust(string filename, - gtsam::noiseModel::Base* model, int maxIndex); -void save2D(const gtsam::NonlinearFactorGraph& graph, - const gtsam::Values& config, gtsam::noiseModel::Diagonal* model, - string filename); + pair load2D(string filename, + gtsam::noiseModel::Diagonal *model, int maxIndex, bool addNoise, bool smart); + pair load2D(string filename, + gtsam::noiseModel::Diagonal *model, int maxIndex, bool addNoise); + pair load2D(string filename, + gtsam::noiseModel::Diagonal *model, int maxIndex); + pair load2D(string filename, + gtsam::noiseModel::Diagonal *model); + pair load2D(string filename); + pair load2D_robust(string filename, + gtsam::noiseModel::Base *model, int maxIndex); + 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[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); + // 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); + pair load3D(string filename); -pair readG2o(string filename); -pair readG2o(string filename, bool is3D); -void writeG2o(const gtsam::NonlinearFactorGraph& graph, - const gtsam::Values& estimate, string filename); + pair readG2o(string filename); + pair readG2o(string filename, bool is3D); + void writeG2o(const gtsam::NonlinearFactorGraph &graph, + const gtsam::Values &estimate, string filename); #include -class InitializePose3 { - static gtsam::Values computeOrientationsChordal( - const gtsam::NonlinearFactorGraph& pose3Graph); - static gtsam::Values computeOrientationsGradient( - const gtsam::NonlinearFactorGraph& pose3Graph, - const gtsam::Values& givenGuess, size_t maxIter, const bool setRefFrame); - static gtsam::Values computeOrientationsGradient( - const gtsam::NonlinearFactorGraph& pose3Graph, - const gtsam::Values& givenGuess); - static gtsam::NonlinearFactorGraph buildPose3graph( - const gtsam::NonlinearFactorGraph& graph); - static gtsam::Values initializeOrientations( - const gtsam::NonlinearFactorGraph& graph); - static gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, - const gtsam::Values& givenGuess, - bool useGradient); - static gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph); -}; + class InitializePose3 + { + static gtsam::Values computeOrientationsChordal( + const gtsam::NonlinearFactorGraph &pose3Graph); + static gtsam::Values computeOrientationsGradient( + const gtsam::NonlinearFactorGraph &pose3Graph, + const gtsam::Values &givenGuess, size_t maxIter, const bool setRefFrame); + static gtsam::Values computeOrientationsGradient( + const gtsam::NonlinearFactorGraph &pose3Graph, + const gtsam::Values &givenGuess); + static gtsam::NonlinearFactorGraph buildPose3graph( + const gtsam::NonlinearFactorGraph &graph); + static gtsam::Values initializeOrientations( + const gtsam::NonlinearFactorGraph &graph); + static gtsam::Values initialize(const gtsam::NonlinearFactorGraph &graph, + const gtsam::Values &givenGuess, + bool useGradient); + static gtsam::Values initialize(const gtsam::NonlinearFactorGraph &graph); + }; #include -template -virtual class KarcherMeanFactor : gtsam::NonlinearFactor { - KarcherMeanFactor(const gtsam::KeyVector& keys); -}; + template + virtual class KarcherMeanFactor : gtsam::NonlinearFactor + { + KarcherMeanFactor(const gtsam::KeyVector &keys); + }; #include -gtsam::noiseModel::Isotropic* ConvertNoiseModel( - gtsam::noiseModel::Base* model, size_t d); + gtsam::noiseModel::Isotropic *ConvertNoiseModel( + gtsam::noiseModel::Base *model, size_t d); -template -virtual class FrobeniusFactor : gtsam::NoiseModelFactor { - FrobeniusFactor(size_t key1, size_t key2); - FrobeniusFactor(size_t key1, size_t key2, gtsam::noiseModel::Base* model); + template + virtual class FrobeniusFactor : gtsam::NoiseModelFactor + { + FrobeniusFactor(size_t key1, size_t key2); + FrobeniusFactor(size_t key1, size_t key2, gtsam::noiseModel::Base *model); - Vector evaluateError(const T& R1, const T& R2); -}; + Vector evaluateError(const T &R1, const T &R2); + }; -template -virtual class FrobeniusBetweenFactor : gtsam::NoiseModelFactor { - FrobeniusBetweenFactor(size_t key1, size_t key2, const T& R12); - FrobeniusBetweenFactor(size_t key1, size_t key2, const T& R12, gtsam::noiseModel::Base* model); + template + virtual class FrobeniusBetweenFactor : gtsam::NoiseModelFactor + { + FrobeniusBetweenFactor(size_t key1, size_t key2, const T &R12); + FrobeniusBetweenFactor(size_t key1, size_t key2, const T &R12, gtsam::noiseModel::Base *model); - Vector evaluateError(const T& R1, const T& R2); -}; + Vector evaluateError(const T &R1, const T &R2); + }; #include -virtual class ShonanFactor3 : gtsam::NoiseModelFactor { - ShonanFactor3(size_t key1, size_t key2, const gtsam::Rot3 &R12, - size_t p); - ShonanFactor3(size_t key1, size_t key2, const gtsam::Rot3 &R12, - size_t p, gtsam::noiseModel::Base *model); - Vector evaluateError(const gtsam::SOn &Q1, const gtsam::SOn &Q2); -}; + virtual class ShonanFactor3 : gtsam::NoiseModelFactor + { + ShonanFactor3(size_t key1, size_t key2, const gtsam::Rot3 &R12, + size_t p); + ShonanFactor3(size_t key1, size_t key2, const gtsam::Rot3 &R12, + size_t p, gtsam::noiseModel::Base *model); + Vector evaluateError(const gtsam::SOn &Q1, const gtsam::SOn &Q2); + }; #include -template -class BinaryMeasurement { - BinaryMeasurement(size_t key1, size_t key2, const T& measured, - const gtsam::noiseModel::Base* model); - size_t key1() const; - size_t key2() const; - T measured() const; - gtsam::noiseModel::Base* noiseModel() const; -}; + template + class BinaryMeasurement + { + BinaryMeasurement(size_t key1, size_t key2, const T &measured, + const gtsam::noiseModel::Base *model); + size_t key1() const; + size_t key2() const; + T measured() const; + gtsam::noiseModel::Base *noiseModel() const; + }; -typedef gtsam::BinaryMeasurement BinaryMeasurementUnit3; -typedef gtsam::BinaryMeasurement BinaryMeasurementRot3; + 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); -}; + 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 wrap! + // TODO(frank): copy/pasta below until we have integer template paremeters in wrap! -class ShonanAveragingParameters2 { - ShonanAveragingParameters2(const gtsam::LevenbergMarquardtParams& lm); - ShonanAveragingParameters2(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(); - void setGaugesWeight(double value); - double getGaugesWeight(); -}; + class ShonanAveragingParameters2 + { + ShonanAveragingParameters2(const gtsam::LevenbergMarquardtParams &lm); + ShonanAveragingParameters2(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(); + void setGaugesWeight(double value); + double getGaugesWeight(); + }; -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 setAnchorWeight(double value); - double getAnchorWeight() const; - void setKarcherWeight(double value); - double getKarcherWeight(); - void setGaugesWeight(double value); - double getGaugesWeight(); -}; + 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 setAnchorWeight(double value); + double getAnchorWeight() const; + void setKarcherWeight(double value); + double getKarcherWeight(); + void setGaugesWeight(double value); + double getGaugesWeight(); + }; -class ShonanAveraging2 { - ShonanAveraging2(string g2oFile); - ShonanAveraging2(string g2oFile, - const gtsam::ShonanAveragingParameters2 ¶meters); + class ShonanAveraging2 + { + ShonanAveraging2(string g2oFile); + ShonanAveraging2(string g2oFile, + const gtsam::ShonanAveragingParameters2 ¶meters); - // Query properties - size_t nrUnknowns() const; - size_t nrMeasurements() const; - gtsam::Rot2 measured(size_t i); - gtsam::KeyVector keys(size_t i); + // Query properties + size_t nrUnknowns() const; + size_t nrMeasurements() const; + gtsam::Rot2 measured(size_t i); + gtsam::KeyVector keys(size_t i); - // Matrix API (advanced use, debugging) - Matrix denseD() const; - Matrix denseQ() const; - Matrix denseL() const; - // Matrix computeLambda_(Matrix S) const; - Matrix computeLambda_(const gtsam::Values& values) const; - Matrix computeA_(const gtsam::Values& values) const; - double computeMinEigenValue(const gtsam::Values& values) const; - gtsam::Values initializeWithDescent(size_t p, const gtsam::Values& values, - const Vector& minEigenVector, double minEigenValue) const; + // Matrix API (advanced use, debugging) + Matrix denseD() const; + Matrix denseQ() const; + Matrix denseL() const; + // Matrix computeLambda_(Matrix S) const; + Matrix computeLambda_(const gtsam::Values &values) const; + Matrix computeA_(const gtsam::Values &values) const; + double computeMinEigenValue(const gtsam::Values &values) const; + gtsam::Values initializeWithDescent(size_t p, const gtsam::Values &values, + const Vector &minEigenVector, double minEigenValue) const; - // Advanced API - gtsam::NonlinearFactorGraph buildGraphAt(size_t p) const; - gtsam::Values initializeRandomlyAt(size_t p) const; - double costAt(size_t p, const gtsam::Values& values) const; - pair computeMinEigenVector(const gtsam::Values& values) const; - bool checkOptimality(const gtsam::Values& values) const; - gtsam::LevenbergMarquardtOptimizer* createOptimizerAt(size_t p, const gtsam::Values& initial); - // gtsam::Values tryOptimizingAt(size_t p) const; - gtsam::Values tryOptimizingAt(size_t p, const gtsam::Values& initial) const; - gtsam::Values projectFrom(size_t p, const gtsam::Values& values) const; - gtsam::Values roundSolution(const gtsam::Values& values) const; + // Advanced API + gtsam::NonlinearFactorGraph buildGraphAt(size_t p) const; + gtsam::Values initializeRandomlyAt(size_t p) const; + double costAt(size_t p, const gtsam::Values &values) const; + pair computeMinEigenVector(const gtsam::Values &values) const; + bool checkOptimality(const gtsam::Values &values) const; + gtsam::LevenbergMarquardtOptimizer *createOptimizerAt(size_t p, const gtsam::Values &initial); + // gtsam::Values tryOptimizingAt(size_t p) const; + gtsam::Values tryOptimizingAt(size_t p, const gtsam::Values &initial) const; + gtsam::Values projectFrom(size_t p, const gtsam::Values &values) const; + gtsam::Values roundSolution(const gtsam::Values &values) const; - // Basic API - double cost(const gtsam::Values& values) const; - gtsam::Values initializeRandomly() const; - pair run(const gtsam::Values& initial, size_t min_p, size_t max_p) const; -}; + // Basic API + double cost(const gtsam::Values &values) const; + gtsam::Values initializeRandomly() const; + pair run(const gtsam::Values &initial, size_t min_p, size_t max_p) const; + }; -class ShonanAveraging3 { - ShonanAveraging3(string g2oFile); - ShonanAveraging3(string g2oFile, - const gtsam::ShonanAveragingParameters3 ¶meters); - - // TODO(frank): deprecate once we land pybind wrapper - ShonanAveraging3(const gtsam::BetweenFactorPose3s &factors); - ShonanAveraging3(const gtsam::BetweenFactorPose3s &factors, - const gtsam::ShonanAveragingParameters3 ¶meters); + class ShonanAveraging3 + { + ShonanAveraging3(string g2oFile); + ShonanAveraging3(string g2oFile, + const gtsam::ShonanAveragingParameters3 ¶meters); - // Query properties - size_t nrUnknowns() const; - size_t nrMeasurements() const; - gtsam::Rot3 measured(size_t i); - gtsam::KeyVector keys(size_t i); + // TODO(frank): deprecate once we land pybind wrapper + ShonanAveraging3(const gtsam::BetweenFactorPose3s &factors); + ShonanAveraging3(const gtsam::BetweenFactorPose3s &factors, + const gtsam::ShonanAveragingParameters3 ¶meters); - // Matrix API (advanced use, debugging) - Matrix denseD() const; - Matrix denseQ() const; - Matrix denseL() const; - // Matrix computeLambda_(Matrix S) const; - Matrix computeLambda_(const gtsam::Values& values) const; - Matrix computeA_(const gtsam::Values& values) const; - double computeMinEigenValue(const gtsam::Values& values) const; - gtsam::Values initializeWithDescent(size_t p, const gtsam::Values& values, - const Vector& minEigenVector, double minEigenValue) const; + // Query properties + size_t nrUnknowns() const; + size_t nrMeasurements() const; + gtsam::Rot3 measured(size_t i); + gtsam::KeyVector keys(size_t i); - // Advanced API - gtsam::NonlinearFactorGraph buildGraphAt(size_t p) const; - gtsam::Values initializeRandomlyAt(size_t p) const; - double costAt(size_t p, const gtsam::Values& values) const; - pair computeMinEigenVector(const gtsam::Values& values) const; - bool checkOptimality(const gtsam::Values& values) const; - gtsam::LevenbergMarquardtOptimizer* createOptimizerAt(size_t p, const gtsam::Values& initial); - // gtsam::Values tryOptimizingAt(size_t p) const; - gtsam::Values tryOptimizingAt(size_t p, const gtsam::Values& initial) const; - gtsam::Values projectFrom(size_t p, const gtsam::Values& values) const; - gtsam::Values roundSolution(const gtsam::Values& values) const; + // Matrix API (advanced use, debugging) + Matrix denseD() const; + Matrix denseQ() const; + Matrix denseL() const; + // Matrix computeLambda_(Matrix S) const; + Matrix computeLambda_(const gtsam::Values &values) const; + Matrix computeA_(const gtsam::Values &values) const; + double computeMinEigenValue(const gtsam::Values &values) const; + gtsam::Values initializeWithDescent(size_t p, const gtsam::Values &values, + const Vector &minEigenVector, double minEigenValue) const; - // Basic API - double cost(const gtsam::Values& values) const; - gtsam::Values initializeRandomly() const; - pair run(const gtsam::Values& initial, size_t min_p, size_t max_p) const; -}; + // Advanced API + gtsam::NonlinearFactorGraph buildGraphAt(size_t p) const; + gtsam::Values initializeRandomlyAt(size_t p) const; + double costAt(size_t p, const gtsam::Values &values) const; + pair computeMinEigenVector(const gtsam::Values &values) const; + bool checkOptimality(const gtsam::Values &values) const; + gtsam::LevenbergMarquardtOptimizer *createOptimizerAt(size_t p, const gtsam::Values &initial); + // gtsam::Values tryOptimizingAt(size_t p) const; + gtsam::Values tryOptimizingAt(size_t p, const gtsam::Values &initial) const; + gtsam::Values projectFrom(size_t p, const gtsam::Values &values) const; + gtsam::Values roundSolution(const gtsam::Values &values) const; + + // Basic API + double cost(const gtsam::Values &values) const; + gtsam::Values initializeRandomly() const; + pair run(const gtsam::Values &initial, size_t min_p, size_t max_p) const; + }; #include -class KeyPairDoubleMap { - KeyPairDoubleMap(); - KeyPairDoubleMap(const gtsam::KeyPairDoubleMap& other); + class KeyPairDoubleMap + { + KeyPairDoubleMap(); + KeyPairDoubleMap(const gtsam::KeyPairDoubleMap &other); - size_t size() const; - bool empty() const; - void clear(); - size_t at(const pair& keypair) const; -}; + size_t size() const; + bool empty() const; + void clear(); + size_t at(const pair &keypair) const; + }; -class MFAS { - MFAS(const gtsam::BinaryMeasurementsUnit3& relativeTranslations, - const gtsam::Unit3& projectionDirection); + class MFAS + { + MFAS(const gtsam::BinaryMeasurementsUnit3 &relativeTranslations, + const gtsam::Unit3 &projectionDirection); - gtsam::KeyPairDoubleMap computeOutlierWeights() const; - gtsam::KeyVector computeOrdering() const; -}; + gtsam::KeyPairDoubleMap computeOutlierWeights() const; + gtsam::KeyVector computeOrdering() const; + }; #include -class TranslationRecovery { - TranslationRecovery(const gtsam::BinaryMeasurementsUnit3 &relativeTranslations, - const gtsam::LevenbergMarquardtParams &lmParams); - TranslationRecovery( - const gtsam::BinaryMeasurementsUnit3 & relativeTranslations); // default LevenbergMarquardtParams - gtsam::Values run(const double scale) const; - gtsam::Values run() const; // default scale = 1.0 -}; + class TranslationRecovery + { + TranslationRecovery(const gtsam::BinaryMeasurementsUnit3 &relativeTranslations, + const gtsam::LevenbergMarquardtParams &lmParams); + TranslationRecovery( + const gtsam::BinaryMeasurementsUnit3 &relativeTranslations); // default LevenbergMarquardtParams + gtsam::Values run(const double scale) const; + gtsam::Values run() const; // default scale = 1.0 + }; -//************************************************************************* -// Navigation -//************************************************************************* -namespace imuBias { + //************************************************************************* + // Navigation + //************************************************************************* + namespace imuBias + { #include -class ConstantBias { - // Constructors - ConstantBias(); - ConstantBias(Vector biasAcc, Vector biasGyro); + class ConstantBias + { + // Constructors + ConstantBias(); + ConstantBias(Vector biasAcc, Vector biasGyro); - // Testable - void print(string s) const; - bool equals(const gtsam::imuBias::ConstantBias& expected, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::imuBias::ConstantBias &expected, double tol) const; - // Group - static gtsam::imuBias::ConstantBias identity(); - gtsam::imuBias::ConstantBias inverse() const; - gtsam::imuBias::ConstantBias compose(const gtsam::imuBias::ConstantBias& b) const; - gtsam::imuBias::ConstantBias between(const gtsam::imuBias::ConstantBias& b) const; + // Group + static gtsam::imuBias::ConstantBias identity(); + gtsam::imuBias::ConstantBias inverse() const; + gtsam::imuBias::ConstantBias compose(const gtsam::imuBias::ConstantBias &b) const; + gtsam::imuBias::ConstantBias between(const gtsam::imuBias::ConstantBias &b) const; - // Manifold - gtsam::imuBias::ConstantBias retract(Vector v) const; - Vector localCoordinates(const gtsam::imuBias::ConstantBias& b) const; + // Manifold + gtsam::imuBias::ConstantBias retract(Vector v) const; + Vector localCoordinates(const gtsam::imuBias::ConstantBias &b) const; - // Lie Group - static gtsam::imuBias::ConstantBias Expmap(Vector v); - static Vector Logmap(const gtsam::imuBias::ConstantBias& b); + // Lie Group + static gtsam::imuBias::ConstantBias Expmap(Vector v); + static Vector Logmap(const gtsam::imuBias::ConstantBias &b); - // Standard Interface - Vector vector() const; - Vector accelerometer() const; - Vector gyroscope() const; - Vector correctAccelerometer(Vector measurement) const; - Vector correctGyroscope(Vector measurement) const; -}; + // Standard Interface + Vector vector() const; + Vector accelerometer() const; + Vector gyroscope() const; + Vector correctAccelerometer(Vector measurement) const; + Vector correctGyroscope(Vector measurement) const; + }; -}///\namespace imuBias + } // namespace imuBias #include -class NavState { - // Constructors - NavState(); - NavState(const gtsam::Rot3& R, const gtsam::Point3& t, Vector v); - NavState(const gtsam::Pose3& pose, Vector v); + class NavState + { + // Constructors + NavState(); + NavState(const gtsam::Rot3 &R, const gtsam::Point3 &t, Vector v); + NavState(const gtsam::Pose3 &pose, Vector v); - // Testable - void print(string s) const; - bool equals(const gtsam::NavState& expected, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::NavState &expected, double tol) const; - // Access - gtsam::Rot3 attitude() const; - gtsam::Point3 position() const; - Vector velocity() const; - gtsam::Pose3 pose() const; -}; + // Access + gtsam::Rot3 attitude() const; + gtsam::Point3 position() const; + Vector velocity() const; + gtsam::Pose3 pose() const; + }; #include -virtual class PreintegratedRotationParams { - PreintegratedRotationParams(); + virtual class PreintegratedRotationParams + { + PreintegratedRotationParams(); - // Testable - void print(string s) const; - bool equals(const gtsam::PreintegratedRotationParams& expected, double tol); + // Testable + void print(string s) const; + bool equals(const gtsam::PreintegratedRotationParams &expected, double tol); - void setGyroscopeCovariance(Matrix cov); - void setOmegaCoriolis(Vector omega); - void setBodyPSensor(const gtsam::Pose3& pose); + void setGyroscopeCovariance(Matrix cov); + void setOmegaCoriolis(Vector omega); + void setBodyPSensor(const gtsam::Pose3 &pose); - Matrix getGyroscopeCovariance() const; + Matrix getGyroscopeCovariance() const; - // TODO(frank): allow optional - // boost::optional getOmegaCoriolis() const; - // boost::optional getBodyPSensor() const; -}; + // TODO(frank): allow optional + // boost::optional getOmegaCoriolis() const; + // boost::optional getBodyPSensor() const; + }; #include -virtual class PreintegrationParams : gtsam::PreintegratedRotationParams { - PreintegrationParams(Vector n_gravity); + virtual class PreintegrationParams : gtsam::PreintegratedRotationParams + { + PreintegrationParams(Vector n_gravity); - static gtsam::PreintegrationParams* MakeSharedD(double g); - static gtsam::PreintegrationParams* MakeSharedU(double g); - static gtsam::PreintegrationParams* MakeSharedD(); // default g = 9.81 - static gtsam::PreintegrationParams* MakeSharedU(); // default g = 9.81 + static gtsam::PreintegrationParams *MakeSharedD(double g); + static gtsam::PreintegrationParams *MakeSharedU(double g); + static gtsam::PreintegrationParams *MakeSharedD(); // default g = 9.81 + static gtsam::PreintegrationParams *MakeSharedU(); // default g = 9.81 - // Testable - void print(string s) const; - bool equals(const gtsam::PreintegrationParams& expected, double tol); + // Testable + void print(string s) const; + bool equals(const gtsam::PreintegrationParams &expected, double tol); - void setAccelerometerCovariance(Matrix cov); - void setIntegrationCovariance(Matrix cov); - void setUse2ndOrderCoriolis(bool flag); + void setAccelerometerCovariance(Matrix cov); + void setIntegrationCovariance(Matrix cov); + void setUse2ndOrderCoriolis(bool flag); - Matrix getAccelerometerCovariance() const; - Matrix getIntegrationCovariance() const; - bool getUse2ndOrderCoriolis() const; -}; + Matrix getAccelerometerCovariance() const; + Matrix getIntegrationCovariance() const; + bool getUse2ndOrderCoriolis() const; + }; #include -class PreintegratedImuMeasurements { - // Constructors - PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params); - PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params, - const gtsam::imuBias::ConstantBias& bias); + class PreintegratedImuMeasurements + { + // Constructors + PreintegratedImuMeasurements(const gtsam::PreintegrationParams *params); + PreintegratedImuMeasurements(const gtsam::PreintegrationParams *params, + const gtsam::imuBias::ConstantBias &bias); - // Testable - void print(string s) const; - bool equals(const gtsam::PreintegratedImuMeasurements& expected, double tol); + // Testable + void print(string s) const; + bool equals(const gtsam::PreintegratedImuMeasurements &expected, double tol); - // Standard Interface - void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, - double deltaT); - void resetIntegration(); - void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias& biasHat); + // Standard Interface + void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, + double deltaT); + void resetIntegration(); + void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias &biasHat); - Matrix preintMeasCov() const; - Vector preintegrated() const; - double deltaTij() const; - gtsam::Rot3 deltaRij() const; - Vector deltaPij() const; - Vector deltaVij() const; - gtsam::imuBias::ConstantBias biasHat() const; - Vector biasHatVector() const; - gtsam::NavState predict(const gtsam::NavState& state_i, - const gtsam::imuBias::ConstantBias& bias) const; -}; + Matrix preintMeasCov() const; + Vector preintegrated() const; + double deltaTij() const; + gtsam::Rot3 deltaRij() const; + Vector deltaPij() const; + Vector deltaVij() const; + gtsam::imuBias::ConstantBias biasHat() const; + Vector biasHatVector() const; + gtsam::NavState predict(const gtsam::NavState &state_i, + const gtsam::imuBias::ConstantBias &bias) const; + }; -virtual class ImuFactor: gtsam::NonlinearFactor { - ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, - size_t bias, - const gtsam::PreintegratedImuMeasurements& preintegratedMeasurements); + virtual class ImuFactor : gtsam::NonlinearFactor + { + ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, + size_t bias, + const gtsam::PreintegratedImuMeasurements &preintegratedMeasurements); - // Standard Interface - gtsam::PreintegratedImuMeasurements preintegratedMeasurements() const; - Vector evaluateError(const gtsam::Pose3& pose_i, Vector vel_i, - const gtsam::Pose3& pose_j, Vector vel_j, - const gtsam::imuBias::ConstantBias& bias); -}; + // Standard Interface + gtsam::PreintegratedImuMeasurements preintegratedMeasurements() const; + Vector evaluateError(const gtsam::Pose3 &pose_i, Vector vel_i, + const gtsam::Pose3 &pose_j, Vector vel_j, + const gtsam::imuBias::ConstantBias &bias); + }; #include -virtual class PreintegrationCombinedParams : gtsam::PreintegrationParams { - PreintegrationCombinedParams(Vector n_gravity); + virtual class PreintegrationCombinedParams : gtsam::PreintegrationParams + { + PreintegrationCombinedParams(Vector n_gravity); - static gtsam::PreintegrationCombinedParams* MakeSharedD(double g); - static gtsam::PreintegrationCombinedParams* MakeSharedU(double g); - static gtsam::PreintegrationCombinedParams* MakeSharedD(); // default g = 9.81 - static gtsam::PreintegrationCombinedParams* MakeSharedU(); // default g = 9.81 + static gtsam::PreintegrationCombinedParams *MakeSharedD(double g); + static gtsam::PreintegrationCombinedParams *MakeSharedU(double g); + static gtsam::PreintegrationCombinedParams *MakeSharedD(); // default g = 9.81 + static gtsam::PreintegrationCombinedParams *MakeSharedU(); // default g = 9.81 - // Testable - void print(string s) const; - bool equals(const gtsam::PreintegrationCombinedParams& expected, double tol); + // Testable + void print(string s) const; + bool equals(const gtsam::PreintegrationCombinedParams &expected, double tol); - void setBiasAccCovariance(Matrix cov); - void setBiasOmegaCovariance(Matrix cov); - void setBiasAccOmegaInt(Matrix cov); - - Matrix getBiasAccCovariance() const ; - Matrix getBiasOmegaCovariance() const ; - Matrix getBiasAccOmegaInt() const; - -}; + void setBiasAccCovariance(Matrix cov); + void setBiasOmegaCovariance(Matrix cov); + void setBiasAccOmegaInt(Matrix cov); -class PreintegratedCombinedMeasurements { -// Constructors - PreintegratedCombinedMeasurements(const gtsam::PreintegrationCombinedParams* params); - PreintegratedCombinedMeasurements(const gtsam::PreintegrationCombinedParams* params, - const gtsam::imuBias::ConstantBias& bias); - // Testable - void print(string s) const; - bool equals(const gtsam::PreintegratedCombinedMeasurements& expected, - double tol); + Matrix getBiasAccCovariance() const; + Matrix getBiasOmegaCovariance() const; + Matrix getBiasAccOmegaInt() const; + }; - // Standard Interface - void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, - double deltaT); - void resetIntegration(); - void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias& biasHat); + class PreintegratedCombinedMeasurements + { + // Constructors + PreintegratedCombinedMeasurements(const gtsam::PreintegrationCombinedParams *params); + PreintegratedCombinedMeasurements(const gtsam::PreintegrationCombinedParams *params, + const gtsam::imuBias::ConstantBias &bias); + // Testable + void print(string s) const; + bool equals(const gtsam::PreintegratedCombinedMeasurements &expected, + double tol); - Matrix preintMeasCov() const; - double deltaTij() const; - gtsam::Rot3 deltaRij() const; - Vector deltaPij() const; - Vector deltaVij() const; - gtsam::imuBias::ConstantBias biasHat() const; - Vector biasHatVector() const; - gtsam::NavState predict(const gtsam::NavState& state_i, - const gtsam::imuBias::ConstantBias& bias) const; -}; + // Standard Interface + void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, + double deltaT); + void resetIntegration(); + void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias &biasHat); -virtual class CombinedImuFactor: gtsam::NonlinearFactor { - CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, - size_t bias_i, size_t bias_j, - const gtsam::PreintegratedCombinedMeasurements& CombinedPreintegratedMeasurements); + Matrix preintMeasCov() const; + double deltaTij() const; + gtsam::Rot3 deltaRij() const; + Vector deltaPij() const; + Vector deltaVij() const; + gtsam::imuBias::ConstantBias biasHat() const; + Vector biasHatVector() const; + gtsam::NavState predict(const gtsam::NavState &state_i, + const gtsam::imuBias::ConstantBias &bias) const; + }; - // Standard Interface - gtsam::PreintegratedCombinedMeasurements preintegratedMeasurements() const; - Vector evaluateError(const gtsam::Pose3& pose_i, Vector vel_i, - const gtsam::Pose3& pose_j, Vector vel_j, - const gtsam::imuBias::ConstantBias& bias_i, - const gtsam::imuBias::ConstantBias& bias_j); -}; + virtual class CombinedImuFactor : gtsam::NonlinearFactor + { + CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, + size_t bias_i, size_t bias_j, + const gtsam::PreintegratedCombinedMeasurements &CombinedPreintegratedMeasurements); + + // Standard Interface + gtsam::PreintegratedCombinedMeasurements preintegratedMeasurements() const; + Vector evaluateError(const gtsam::Pose3 &pose_i, Vector vel_i, + const gtsam::Pose3 &pose_j, Vector vel_j, + const gtsam::imuBias::ConstantBias &bias_i, + const gtsam::imuBias::ConstantBias &bias_j); + }; #include -class PreintegratedAhrsMeasurements { - // Standard Constructor - PreintegratedAhrsMeasurements(Vector bias, Matrix measuredOmegaCovariance); - PreintegratedAhrsMeasurements(const gtsam::PreintegratedAhrsMeasurements& rhs); + class PreintegratedAhrsMeasurements + { + // Standard Constructor + PreintegratedAhrsMeasurements(Vector bias, Matrix measuredOmegaCovariance); + PreintegratedAhrsMeasurements(const gtsam::PreintegratedAhrsMeasurements &rhs); - // Testable - void print(string s) const; - bool equals(const gtsam::PreintegratedAhrsMeasurements& expected, double tol); + // Testable + void print(string s) const; + bool equals(const gtsam::PreintegratedAhrsMeasurements &expected, double tol); - // get Data - gtsam::Rot3 deltaRij() const; - double deltaTij() const; - Vector biasHat() const; + // get Data + gtsam::Rot3 deltaRij() const; + double deltaTij() const; + Vector biasHat() const; - // Standard Interface - void integrateMeasurement(Vector measuredOmega, double deltaT); - void resetIntegration() ; -}; + // Standard Interface + void integrateMeasurement(Vector measuredOmega, double deltaT); + void resetIntegration(); + }; -virtual class AHRSFactor : gtsam::NonlinearFactor { - AHRSFactor(size_t rot_i, size_t rot_j,size_t bias, - const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, Vector omegaCoriolis); - AHRSFactor(size_t rot_i, size_t rot_j, size_t bias, - const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, Vector omegaCoriolis, - const gtsam::Pose3& body_P_sensor); + virtual class AHRSFactor : gtsam::NonlinearFactor + { + AHRSFactor(size_t rot_i, size_t rot_j, size_t bias, + const gtsam::PreintegratedAhrsMeasurements &preintegratedMeasurements, Vector omegaCoriolis); + AHRSFactor(size_t rot_i, size_t rot_j, size_t bias, + const gtsam::PreintegratedAhrsMeasurements &preintegratedMeasurements, Vector omegaCoriolis, + const gtsam::Pose3 &body_P_sensor); - // Standard Interface - gtsam::PreintegratedAhrsMeasurements preintegratedMeasurements() const; - Vector evaluateError(const gtsam::Rot3& rot_i, const gtsam::Rot3& rot_j, - Vector bias) const; - gtsam::Rot3 predict(const gtsam::Rot3& rot_i, Vector bias, - const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, - Vector omegaCoriolis) const; -}; + // Standard Interface + gtsam::PreintegratedAhrsMeasurements preintegratedMeasurements() const; + Vector evaluateError(const gtsam::Rot3 &rot_i, const gtsam::Rot3 &rot_j, + Vector bias) const; + gtsam::Rot3 predict(const gtsam::Rot3 &rot_i, Vector bias, + const gtsam::PreintegratedAhrsMeasurements &preintegratedMeasurements, + Vector omegaCoriolis) const; + }; #include -//virtual class AttitudeFactor : gtsam::NonlinearFactor { -// AttitudeFactor(const Unit3& nZ, const Unit3& bRef); -// AttitudeFactor(); -//}; -virtual class Rot3AttitudeFactor : gtsam::NonlinearFactor{ - Rot3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model, - const gtsam::Unit3& bRef); - Rot3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model); - Rot3AttitudeFactor(); - void print(string s) const; - bool equals(const gtsam::NonlinearFactor& expected, double tol) const; - gtsam::Unit3 nZ() const; - gtsam::Unit3 bRef() const; -}; + //virtual class AttitudeFactor : gtsam::NonlinearFactor { + // AttitudeFactor(const Unit3& nZ, const Unit3& bRef); + // AttitudeFactor(); + //}; + virtual class Rot3AttitudeFactor : gtsam::NonlinearFactor + { + Rot3AttitudeFactor(size_t key, const gtsam::Unit3 &nZ, const gtsam::noiseModel::Diagonal *model, + const gtsam::Unit3 &bRef); + Rot3AttitudeFactor(size_t key, const gtsam::Unit3 &nZ, const gtsam::noiseModel::Diagonal *model); + Rot3AttitudeFactor(); + void print(string s) const; + bool equals(const gtsam::NonlinearFactor &expected, double tol) const; + gtsam::Unit3 nZ() const; + gtsam::Unit3 bRef() const; + }; -virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor { - Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, - const gtsam::noiseModel::Diagonal* model, - const gtsam::Unit3& bRef); - Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, - const gtsam::noiseModel::Diagonal* model); - Pose3AttitudeFactor(); - void print(string s) const; - bool equals(const gtsam::NonlinearFactor& expected, double tol) const; - gtsam::Unit3 nZ() const; - gtsam::Unit3 bRef() const; -}; + virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor + { + Pose3AttitudeFactor(size_t key, const gtsam::Unit3 &nZ, + const gtsam::noiseModel::Diagonal *model, + const gtsam::Unit3 &bRef); + Pose3AttitudeFactor(size_t key, const gtsam::Unit3 &nZ, + const gtsam::noiseModel::Diagonal *model); + Pose3AttitudeFactor(); + void print(string s) const; + bool equals(const gtsam::NonlinearFactor &expected, double tol) const; + gtsam::Unit3 nZ() const; + gtsam::Unit3 bRef() const; + }; #include -virtual class GPSFactor : gtsam::NonlinearFactor{ - GPSFactor(size_t key, const gtsam::Point3& gpsIn, - const gtsam::noiseModel::Base* model); + virtual class GPSFactor : gtsam::NonlinearFactor + { + GPSFactor(size_t key, const gtsam::Point3 &gpsIn, + const gtsam::noiseModel::Base *model); - // Testable - void print(string s) const; - bool equals(const gtsam::GPSFactor& expected, double tol); + // Testable + void print(string s) const; + bool equals(const gtsam::GPSFactor &expected, double tol); - // Standard Interface - gtsam::Point3 measurementIn() const; -}; + // Standard Interface + gtsam::Point3 measurementIn() const; + }; -virtual class GPSFactor2 : gtsam::NonlinearFactor { - GPSFactor2(size_t key, const gtsam::Point3& gpsIn, - const gtsam::noiseModel::Base* model); + virtual class GPSFactor2 : gtsam::NonlinearFactor + { + GPSFactor2(size_t key, const gtsam::Point3 &gpsIn, + const gtsam::noiseModel::Base *model); - // Testable - void print(string s) const; - bool equals(const gtsam::GPSFactor2& expected, double tol); + // Testable + void print(string s) const; + bool equals(const gtsam::GPSFactor2 &expected, double tol); - // Standard Interface - gtsam::Point3 measurementIn() const; -}; + // Standard Interface + gtsam::Point3 measurementIn() const; + }; #include -virtual class Scenario { - gtsam::Pose3 pose(double t) const; - Vector omega_b(double t) const; - Vector velocity_n(double t) const; - Vector acceleration_n(double t) const; - gtsam::Rot3 rotation(double t) const; - gtsam::NavState navState(double t) const; - Vector velocity_b(double t) const; - Vector acceleration_b(double t) const; -}; + virtual class Scenario + { + gtsam::Pose3 pose(double t) const; + Vector omega_b(double t) const; + Vector velocity_n(double t) const; + Vector acceleration_n(double t) const; + gtsam::Rot3 rotation(double t) const; + gtsam::NavState navState(double t) const; + Vector velocity_b(double t) const; + Vector acceleration_b(double t) const; + }; -virtual class ConstantTwistScenario : gtsam::Scenario { - ConstantTwistScenario(Vector w, Vector v); - ConstantTwistScenario(Vector w, Vector v, - const gtsam::Pose3& nTb0); -}; + virtual class ConstantTwistScenario : gtsam::Scenario + { + ConstantTwistScenario(Vector w, Vector v); + ConstantTwistScenario(Vector w, Vector v, + const gtsam::Pose3 &nTb0); + }; -virtual class AcceleratingScenario : gtsam::Scenario { - AcceleratingScenario(const gtsam::Rot3& nRb, const gtsam::Point3& p0, - Vector v0, Vector a_n, - Vector omega_b); -}; + virtual class AcceleratingScenario : gtsam::Scenario + { + AcceleratingScenario(const gtsam::Rot3 &nRb, const gtsam::Point3 &p0, + Vector v0, Vector a_n, + Vector omega_b); + }; #include -class ScenarioRunner { - ScenarioRunner(const gtsam::Scenario& scenario, - const gtsam::PreintegrationParams* p, - double imuSampleTime, - const gtsam::imuBias::ConstantBias& bias); - Vector gravity_n() const; - Vector actualAngularVelocity(double t) const; - Vector actualSpecificForce(double t) const; - Vector measuredAngularVelocity(double t) const; - Vector measuredSpecificForce(double t) const; - double imuSampleTime() const; - gtsam::PreintegratedImuMeasurements integrate( - double T, const gtsam::imuBias::ConstantBias& estimatedBias, - bool corrupted) const; - gtsam::NavState predict( - const gtsam::PreintegratedImuMeasurements& pim, - const gtsam::imuBias::ConstantBias& estimatedBias) const; - Matrix estimateCovariance( - double T, size_t N, - const gtsam::imuBias::ConstantBias& estimatedBias) const; - Matrix estimateNoiseCovariance(size_t N) const; -}; + class ScenarioRunner + { + ScenarioRunner(const gtsam::Scenario &scenario, + const gtsam::PreintegrationParams *p, + double imuSampleTime, + const gtsam::imuBias::ConstantBias &bias); + Vector gravity_n() const; + Vector actualAngularVelocity(double t) const; + Vector actualSpecificForce(double t) const; + Vector measuredAngularVelocity(double t) const; + Vector measuredSpecificForce(double t) const; + double imuSampleTime() const; + gtsam::PreintegratedImuMeasurements integrate( + double T, const gtsam::imuBias::ConstantBias &estimatedBias, + bool corrupted) const; + gtsam::NavState predict( + const gtsam::PreintegratedImuMeasurements &pim, + const gtsam::imuBias::ConstantBias &estimatedBias) const; + Matrix estimateCovariance( + double T, size_t N, + const gtsam::imuBias::ConstantBias &estimatedBias) const; + Matrix estimateNoiseCovariance(size_t N) const; + }; -//************************************************************************* -// Utilities -//************************************************************************* + //************************************************************************* + // Utilities + //************************************************************************* -namespace utilities { - - #include - gtsam::KeyList createKeyList(Vector I); - gtsam::KeyList createKeyList(string s, Vector I); - gtsam::KeyVector createKeyVector(Vector I); - gtsam::KeyVector createKeyVector(string s, Vector I); - gtsam::KeySet createKeySet(Vector I); - gtsam::KeySet createKeySet(string s, Vector I); - Matrix extractPoint2(const gtsam::Values& values); - Matrix extractPoint3(const gtsam::Values& values); - gtsam::Values allPose2s(gtsam::Values& values); - Matrix extractPose2(const gtsam::Values& values); - gtsam::Values allPose3s(gtsam::Values& values); - Matrix extractPose3(const gtsam::Values& values); - void perturbPoint2(gtsam::Values& values, double sigma, int seed); - void perturbPose2 (gtsam::Values& values, double sigmaT, double sigmaR, int seed); - void perturbPoint3(gtsam::Values& values, double sigma, int seed); - void insertBackprojections(gtsam::Values& values, const gtsam::PinholeCameraCal3_S2& c, Vector J, Matrix Z, double depth); - void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K); - void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K, const gtsam::Pose3& body_P_sensor); - Matrix reprojectionErrors(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values); - gtsam::Values localToWorld(const gtsam::Values& local, const gtsam::Pose2& base); - gtsam::Values localToWorld(const gtsam::Values& local, const gtsam::Pose2& base, const gtsam::KeyVector& keys); - -} //\namespace utilities + namespace utilities + { #include -class RedirectCout { - RedirectCout(); - string str(); -}; + gtsam::KeyList createKeyList(Vector I); + gtsam::KeyList createKeyList(string s, Vector I); + gtsam::KeyVector createKeyVector(Vector I); + gtsam::KeyVector createKeyVector(string s, Vector I); + gtsam::KeySet createKeySet(Vector I); + gtsam::KeySet createKeySet(string s, Vector I); + Matrix extractPoint2(const gtsam::Values &values); + Matrix extractPoint3(const gtsam::Values &values); + gtsam::Values allPose2s(gtsam::Values &values); + Matrix extractPose2(const gtsam::Values &values); + gtsam::Values allPose3s(gtsam::Values &values); + Matrix extractPose3(const gtsam::Values &values); + void perturbPoint2(gtsam::Values &values, double sigma, int seed); + void perturbPose2(gtsam::Values &values, double sigmaT, double sigmaR, int seed); + void perturbPoint3(gtsam::Values &values, double sigma, int seed); + void insertBackprojections(gtsam::Values &values, const gtsam::PinholeCameraCal3_S2 &c, Vector J, Matrix Z, double depth); + void insertProjectionFactors(gtsam::NonlinearFactorGraph &graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base *model, const gtsam::Cal3_S2 *K); + void insertProjectionFactors(gtsam::NonlinearFactorGraph &graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base *model, const gtsam::Cal3_S2 *K, const gtsam::Pose3 &body_P_sensor); + Matrix reprojectionErrors(const gtsam::NonlinearFactorGraph &graph, const gtsam::Values &values); + gtsam::Values localToWorld(const gtsam::Values &local, const gtsam::Pose2 &base); + gtsam::Values localToWorld(const gtsam::Values &local, const gtsam::Pose2 &base, const gtsam::KeyVector &keys); -} + } // namespace utilities + +#include + class RedirectCout + { + RedirectCout(); + string str(); + }; + +} // namespace gtsam diff --git a/python/gtsam/preamble.h b/python/gtsam/preamble.h index c8a577431..fa98cd171 100644 --- a/python/gtsam/preamble.h +++ b/python/gtsam/preamble.h @@ -5,10 +5,10 @@ PYBIND11_MAKE_OPAQUE(std::vector>); #else PYBIND11_MAKE_OPAQUE(std::vector); #endif -PYBIND11_MAKE_OPAQUE(std::vector >); +PYBIND11_MAKE_OPAQUE(std::vector>); PYBIND11_MAKE_OPAQUE(std::vector); -PYBIND11_MAKE_OPAQUE(std::vector > >); -PYBIND11_MAKE_OPAQUE(std::vector > >); +PYBIND11_MAKE_OPAQUE(std::vector>>); +PYBIND11_MAKE_OPAQUE(std::vector>>); PYBIND11_MAKE_OPAQUE(std::vector); -PYBIND11_MAKE_OPAQUE(gtsam::CameraSet >); -PYBIND11_MAKE_OPAQUE(gtsam::CameraSet >); \ No newline at end of file +PYBIND11_MAKE_OPAQUE(gtsam::CameraSet>); +PYBIND11_MAKE_OPAQUE(gtsam::CameraSet>); \ No newline at end of file diff --git a/python/gtsam/specializations.h b/python/gtsam/specializations.h index 431697aac..63694f6f4 100644 --- a/python/gtsam/specializations.h +++ b/python/gtsam/specializations.h @@ -1,17 +1,17 @@ // Please refer to: https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html // These are required to save one copy operation on Python calls #ifdef GTSAM_ALLOCATOR_TBB -py::bind_vector > >(m_, "KeyVector"); +py::bind_vector>>(m_, "KeyVector"); #else -py::bind_vector >(m_, "KeyVector"); +py::bind_vector>(m_, "KeyVector"); #endif -py::bind_vector > >(m_, "Point2Vector"); -py::bind_vector >(m_, "Pose3Vector"); -py::bind_vector > > >(m_, "BetweenFactorPose3s"); -py::bind_vector > > >(m_, "BetweenFactorPose2s"); -py::bind_vector > >(m_, "BinaryMeasurementsUnit3"); +py::bind_vector>>(m_, "Point2Vector"); +py::bind_vector>(m_, "Pose3Vector"); +py::bind_vector>>>(m_, "BetweenFactorPose3s"); +py::bind_vector>>>(m_, "BetweenFactorPose2s"); +py::bind_vector>>(m_, "BinaryMeasurementsUnit3"); py::bind_map(m_, "IndexPairSetMap"); py::bind_vector(m_, "IndexPairVector"); py::bind_map(m_, "KeyPairDoubleMap"); -py::bind_vector > >(m_, "CameraSetCal3_S2"); -py::bind_vector > >(m_, "CameraSetCal3Bundler"); +py::bind_vector>>(m_, "CameraSetCal3_S2"); +py::bind_vector>>(m_, "CameraSetCal3Bundler"); diff --git a/python/gtsam/tests/test_Triangulation.py b/python/gtsam/tests/test_Triangulation.py index c04766804..6d7751356 100644 --- a/python/gtsam/tests/test_Triangulation.py +++ b/python/gtsam/tests/test_Triangulation.py @@ -14,11 +14,10 @@ import numpy as np import gtsam as g from gtsam.utils.test_case import GtsamTestCase -from gtsam import Cal3_S2, Cal3Bundler, Rot3, Pose3, \ - PinholeCameraCal3_S2, PinholeCameraCal3Bundler, Point3, \ - Point2Vector, Pose3Vector, triangulatePoint3, \ - CameraSetCal3_S2, CameraSetCal3Bundler -from numpy.core.records import array +from gtsam import Cal3_S2, Cal3Bundler, CameraSetCal3_S2,\ + CameraSetCal3Bundler, PinholeCameraCal3_S2, PinholeCameraCal3Bundler, \ + Point3, Pose3, Point2Vector, Pose3Vector, Rot3, triangulatePoint3 + class TestVisualISAMExample(GtsamTestCase): """ Tests for triangulation with shared and individual calibrations """ @@ -48,7 +47,6 @@ class TestVisualISAMExample(GtsamTestCase): Returns: vector of measurements and cameras """ - cameras = [] measurements = Point2Vector() for k, pose in zip(cal_params, self.poses): @@ -85,6 +83,8 @@ class TestVisualISAMExample(GtsamTestCase): K2 = (1600, 1300, 0, 650, 440) measurements, camera_list = self.generate_measurements(Cal3_S2, PinholeCameraCal3_S2, K1, K2) + + # convert list to CameraSet object cameras = CameraSetCal3_S2() for camera in camera_list: cameras.append(camera) @@ -99,6 +99,8 @@ class TestVisualISAMExample(GtsamTestCase): K2 = (1600, 0, 0, 650, 440) measurements, camera_list = self.generate_measurements(Cal3Bundler, PinholeCameraCal3Bundler, K1, K2) + + # convert list to CameraSet object cameras = CameraSetCal3Bundler() for camera in camera_list: cameras.append(camera) From 44d1d69274886b7dc6885e1ada8159fa89e7c4c2 Mon Sep 17 00:00:00 2001 From: Sushmita Date: Sat, 5 Dec 2020 12:07:39 -0500 Subject: [PATCH 12/17] removed typedef and formatted code --- gtsam/gtsam.i | 4 ---- python/gtsam/tests/test_Triangulation.py | 18 ++++++++++-------- 2 files changed, 10 insertions(+), 12 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 1ab2425ec..0d739c138 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -1112,10 +1112,6 @@ namespace gtsam void push_back(const T &cam); }; - // typedefs added here for shorter type name and to enforce uniformity in naming conventions - //typedef gtsam::CameraSet CameraSetCal3_S2; - //typedef gtsam::CameraSet CameraSetCal3Bundler; - #include class StereoCamera { diff --git a/python/gtsam/tests/test_Triangulation.py b/python/gtsam/tests/test_Triangulation.py index 6d7751356..0dff861f1 100644 --- a/python/gtsam/tests/test_Triangulation.py +++ b/python/gtsam/tests/test_Triangulation.py @@ -45,10 +45,11 @@ class TestVisualISAMExample(GtsamTestCase): camera_model: Camera model e.g. PinholeCameraCal3_S2 cal_params: (list of) camera parameters e.g. K1, K2 Returns: - vector of measurements and cameras + list of measurements and cameras """ cameras = [] - measurements = Point2Vector() + measurements = Point2Vector() + for k, pose in zip(cal_params, self.poses): K = calibration(*k) camera = camera_model(pose, K) @@ -63,22 +64,23 @@ class TestVisualISAMExample(GtsamTestCase): """ Tests triangulation with shared Cal3_S2 calibration""" # Some common constants sharedCal = (1500, 1200, 0, 640, 480) + measurements, _ = self.generate_measurements(Cal3_S2, PinholeCameraCal3_S2, sharedCal, sharedCal) - triangulated_landmark = triangulatePoint3(self.poses,Cal3_S2(sharedCal), measurements, rank_tol=1e-9, optimize=True) + triangulated_landmark = triangulatePoint3(self.poses, Cal3_S2(sharedCal), measurements, rank_tol=1e-9, optimize=True) self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9) - # 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) + # Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) measurements_noisy = Point2Vector() measurements_noisy.append(measurements[0] - np.array([0.1, 0.5])) measurements_noisy.append(measurements[1] - np.array([-0.2, 0.3])) - triangulated_landmark = triangulatePoint3(self.poses,Cal3_S2(sharedCal), measurements_noisy, rank_tol=1e-9, optimize=True) - self.gtsamAssertEquals(self.landmark, triangulated_landmark,1e-2) + triangulated_landmark = triangulatePoint3(self.poses, Cal3_S2(sharedCal), measurements_noisy, rank_tol=1e-9, optimize=True) + self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-2) def test_distinct_Ks(self): """ Tests triangulation with individual Cal3_S2 calibrations """ - # two cameras + # two camera parameters K1 = (1500, 1200, 0, 640, 480) K2 = (1600, 1300, 0, 650, 440) @@ -94,7 +96,7 @@ class TestVisualISAMExample(GtsamTestCase): def test_distinct_Ks_Bundler(self): """ Tests triangulation with individual Cal3Bundler calibrations""" - # two cameras + # two camera parameters K1 = (1500, 0, 0, 640, 480) K2 = (1600, 0, 0, 650, 440) From 858884f1e7a74f65a357a2f800141ce30ec0b493 Mon Sep 17 00:00:00 2001 From: Sushmita Date: Sat, 5 Dec 2020 13:16:13 -0500 Subject: [PATCH 13/17] moved camera_set to generate_measurements --- python/gtsam/tests/test_Triangulation.py | 28 ++++++++++-------------- 1 file changed, 11 insertions(+), 17 deletions(-) diff --git a/python/gtsam/tests/test_Triangulation.py b/python/gtsam/tests/test_Triangulation.py index 0dff861f1..901aad0b9 100644 --- a/python/gtsam/tests/test_Triangulation.py +++ b/python/gtsam/tests/test_Triangulation.py @@ -38,18 +38,22 @@ class TestVisualISAMExample(GtsamTestCase): # landmark ~5 meters infront of camera self.landmark = Point3(5, 0.5, 1.2) - def generate_measurements(self, calibration, camera_model, *cal_params): + def generate_measurements(self, calibration, camera_model, camera_set, *cal_params): """ Generate vector of measurements for given calibration and camera model Args: calibration: Camera calibration e.g. Cal3_S2 camera_model: Camera model e.g. PinholeCameraCal3_S2 cal_params: (list of) camera parameters e.g. K1, K2 + camera_set: Cameraset object (for individual calibrations) Returns: - list of measurements and cameras + list of measurements and list/CameraSet object for cameras """ - cameras = [] + if camera_set is not None: + cameras = camera_set() + else: + cameras = [] measurements = Point2Vector() - + for k, pose in zip(cal_params, self.poses): K = calibration(*k) camera = camera_model(pose, K) @@ -65,7 +69,7 @@ class TestVisualISAMExample(GtsamTestCase): # Some common constants sharedCal = (1500, 1200, 0, 640, 480) - measurements, _ = self.generate_measurements(Cal3_S2, PinholeCameraCal3_S2, sharedCal, sharedCal) + measurements, _ = self.generate_measurements(Cal3_S2, PinholeCameraCal3_S2, None, sharedCal, sharedCal) triangulated_landmark = triangulatePoint3(self.poses, Cal3_S2(sharedCal), measurements, rank_tol=1e-9, optimize=True) self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9) @@ -84,12 +88,7 @@ class TestVisualISAMExample(GtsamTestCase): K1 = (1500, 1200, 0, 640, 480) K2 = (1600, 1300, 0, 650, 440) - measurements, camera_list = self.generate_measurements(Cal3_S2, PinholeCameraCal3_S2, K1, K2) - - # convert list to CameraSet object - cameras = CameraSetCal3_S2() - for camera in camera_list: - cameras.append(camera) + measurements, cameras = self.generate_measurements(Cal3_S2, PinholeCameraCal3_S2, CameraSetCal3_S2, K1, K2) triangulated_landmark = triangulatePoint3(cameras, measurements, rank_tol=1e-9, optimize=True) self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9) @@ -100,12 +99,7 @@ class TestVisualISAMExample(GtsamTestCase): K1 = (1500, 0, 0, 640, 480) K2 = (1600, 0, 0, 650, 440) - measurements, camera_list = self.generate_measurements(Cal3Bundler, PinholeCameraCal3Bundler, K1, K2) - - # convert list to CameraSet object - cameras = CameraSetCal3Bundler() - for camera in camera_list: - cameras.append(camera) + measurements, cameras = self.generate_measurements(Cal3Bundler, PinholeCameraCal3Bundler, CameraSetCal3Bundler, K1, K2) triangulated_landmark = triangulatePoint3(cameras, measurements, rank_tol=1e-9, optimize=True) self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9) From b24f943c360b6be3c594fb9e80578b08d8ba28fe Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 5 Dec 2020 18:08:45 -0500 Subject: [PATCH 14/17] Revert "code formatted" This reverts commit 8be6890b206c761bb1f334f4f7ab6fcfa7ccbff0. --- gtsam/geometry/triangulation.h | 726 ++- gtsam/gtsam.i | 5664 +++++++++++----------- python/gtsam/preamble.h | 10 +- python/gtsam/specializations.h | 18 +- python/gtsam/tests/test_Triangulation.py | 9 +- 5 files changed, 3116 insertions(+), 3311 deletions(-) diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index e22b35e56..1df9efd22 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -27,52 +27,49 @@ #include #include -namespace gtsam -{ +namespace gtsam { - /// Exception thrown by triangulateDLT when SVD returns rank < 3 - class GTSAM_EXPORT TriangulationUnderconstrainedException : public std::runtime_error - { - public: - TriangulationUnderconstrainedException() : std::runtime_error("Triangulation Underconstrained Exception.") - { - } - }; +/// Exception thrown by triangulateDLT when SVD returns rank < 3 +class GTSAM_EXPORT TriangulationUnderconstrainedException: public std::runtime_error { +public: + TriangulationUnderconstrainedException() : + std::runtime_error("Triangulation Underconstrained Exception.") { + } +}; - /// Exception thrown by triangulateDLT when landmark is behind one or more of the cameras - class GTSAM_EXPORT TriangulationCheiralityException : public std::runtime_error - { - public: - TriangulationCheiralityException() : std::runtime_error( - "Triangulation Cheirality Exception: The resulting landmark is behind one or more cameras.") - { - } - }; +/// Exception thrown by triangulateDLT when landmark is behind one or more of the cameras +class GTSAM_EXPORT TriangulationCheiralityException: public std::runtime_error { +public: + TriangulationCheiralityException() : + std::runtime_error( + "Triangulation Cheirality Exception: The resulting landmark is behind one or more cameras.") { + } +}; - /** +/** * DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312 * @param projection_matrices Projection matrices (K*P^-1) * @param measurements 2D measurements * @param rank_tol SVD rank tolerance * @return Triangulated point, in homogeneous coordinates */ - GTSAM_EXPORT Vector4 triangulateHomogeneousDLT( - const std::vector> &projection_matrices, - const Point2Vector &measurements, double rank_tol = 1e-9); +GTSAM_EXPORT Vector4 triangulateHomogeneousDLT( + const std::vector>& projection_matrices, + const Point2Vector& measurements, double rank_tol = 1e-9); - /** +/** * DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312 * @param projection_matrices Projection matrices (K*P^-1) * @param measurements 2D measurements * @param rank_tol SVD rank tolerance * @return Triangulated Point3 */ - GTSAM_EXPORT Point3 triangulateDLT( - const std::vector> &projection_matrices, - const Point2Vector &measurements, - double rank_tol = 1e-9); +GTSAM_EXPORT Point3 triangulateDLT( + const std::vector>& projection_matrices, + const Point2Vector& measurements, + double rank_tol = 1e-9); - /** +/** * Create a factor graph with projection factors from poses and one calibration * @param poses Camera poses * @param sharedCal shared pointer to single calibration object (monocular only!) @@ -81,29 +78,27 @@ namespace gtsam * @param initialEstimate * @return graph and initial values */ - template - std::pair triangulationGraph( - const std::vector &poses, boost::shared_ptr sharedCal, - const Point2Vector &measurements, Key landmarkKey, - const Point3 &initialEstimate) - { - Values values; - values.insert(landmarkKey, initialEstimate); // Initial landmark value - NonlinearFactorGraph graph; - static SharedNoiseModel unit2(noiseModel::Unit::Create(2)); - static SharedNoiseModel prior_model(noiseModel::Isotropic::Sigma(6, 1e-6)); - for (size_t i = 0; i < measurements.size(); i++) - { - const Pose3 &pose_i = poses[i]; - typedef PinholePose Camera; - Camera camera_i(pose_i, sharedCal); - graph.emplace_shared> // - (camera_i, measurements[i], unit2, landmarkKey); - } - return std::make_pair(graph, values); +template +std::pair triangulationGraph( + const std::vector& poses, boost::shared_ptr sharedCal, + const Point2Vector& measurements, Key landmarkKey, + const Point3& initialEstimate) { + Values values; + values.insert(landmarkKey, initialEstimate); // Initial landmark value + NonlinearFactorGraph graph; + static SharedNoiseModel unit2(noiseModel::Unit::Create(2)); + static SharedNoiseModel prior_model(noiseModel::Isotropic::Sigma(6, 1e-6)); + for (size_t i = 0; i < measurements.size(); i++) { + const Pose3& pose_i = poses[i]; + typedef PinholePose Camera; + Camera camera_i(pose_i, sharedCal); + graph.emplace_shared > // + (camera_i, measurements[i], unit2, landmarkKey); } + return std::make_pair(graph, values); +} - /** +/** * Create a factor graph with projection factors from pinhole cameras * (each camera has a pose and calibration) * @param cameras pinhole cameras (monocular or stereo) @@ -112,37 +107,35 @@ namespace gtsam * @param initialEstimate * @return graph and initial values */ - template - std::pair triangulationGraph( - const CameraSet &cameras, - const typename CAMERA::MeasurementVector &measurements, Key landmarkKey, - const Point3 &initialEstimate) - { - Values values; - values.insert(landmarkKey, initialEstimate); // Initial landmark value - NonlinearFactorGraph graph; - static SharedNoiseModel unit(noiseModel::Unit::Create( - traits::dimension)); - for (size_t i = 0; i < measurements.size(); i++) - { - const CAMERA &camera_i = cameras[i]; - graph.emplace_shared> // - (camera_i, measurements[i], unit, landmarkKey); - } - return std::make_pair(graph, values); +template +std::pair triangulationGraph( + const CameraSet& cameras, + const typename CAMERA::MeasurementVector& measurements, Key landmarkKey, + const Point3& initialEstimate) { + Values values; + values.insert(landmarkKey, initialEstimate); // Initial landmark value + NonlinearFactorGraph graph; + static SharedNoiseModel unit(noiseModel::Unit::Create( + traits::dimension)); + for (size_t i = 0; i < measurements.size(); i++) { + const CAMERA& camera_i = cameras[i]; + graph.emplace_shared > // + (camera_i, measurements[i], unit, landmarkKey); } + return std::make_pair(graph, values); +} - /** +/** * Optimize for triangulation * @param graph nonlinear factors for projection * @param values initial values * @param landmarkKey to refer to landmark * @return refined Point3 */ - GTSAM_EXPORT Point3 optimize(const NonlinearFactorGraph &graph, - const Values &values, Key landmarkKey); +GTSAM_EXPORT Point3 optimize(const NonlinearFactorGraph& graph, + const Values& values, Key landmarkKey); - /** +/** * Given an initial estimate , refine a point using measurements in several cameras * @param poses Camera poses * @param sharedCal shared pointer to single calibration object @@ -150,69 +143,63 @@ namespace gtsam * @param initialEstimate * @return refined Point3 */ - template - Point3 triangulateNonlinear(const std::vector &poses, - boost::shared_ptr sharedCal, - const Point2Vector &measurements, const Point3 &initialEstimate) - { +template +Point3 triangulateNonlinear(const std::vector& poses, + boost::shared_ptr sharedCal, + const Point2Vector& measurements, const Point3& initialEstimate) { - // Create a factor graph and initial values - Values values; - NonlinearFactorGraph graph; - boost::tie(graph, values) = triangulationGraph // - (poses, sharedCal, measurements, Symbol('p', 0), initialEstimate); + // Create a factor graph and initial values + Values values; + NonlinearFactorGraph graph; + boost::tie(graph, values) = triangulationGraph // + (poses, sharedCal, measurements, Symbol('p', 0), initialEstimate); - return optimize(graph, values, Symbol('p', 0)); - } + return optimize(graph, values, Symbol('p', 0)); +} - /** +/** * Given an initial estimate , refine a point using measurements in several cameras * @param cameras pinhole cameras (monocular or stereo) * @param measurements 2D measurements * @param initialEstimate * @return refined Point3 */ - template - Point3 triangulateNonlinear( - const CameraSet &cameras, - const typename CAMERA::MeasurementVector &measurements, const Point3 &initialEstimate) - { +template +Point3 triangulateNonlinear( + const CameraSet& cameras, + const typename CAMERA::MeasurementVector& measurements, const Point3& initialEstimate) { - // Create a factor graph and initial values - Values values; - NonlinearFactorGraph graph; - boost::tie(graph, values) = triangulationGraph // - (cameras, measurements, Symbol('p', 0), initialEstimate); + // Create a factor graph and initial values + Values values; + NonlinearFactorGraph graph; + boost::tie(graph, values) = triangulationGraph // + (cameras, measurements, Symbol('p', 0), initialEstimate); - return optimize(graph, values, Symbol('p', 0)); - } + return optimize(graph, values, Symbol('p', 0)); +} - /** +/** * Create a 3*4 camera projection matrix from calibration and pose. * Functor for partial application on calibration * @param pose The camera pose * @param cal The calibration * @return Returns a Matrix34 */ - template - struct CameraProjectionMatrix - { - CameraProjectionMatrix(const CALIBRATION &calibration) : K_(calibration.K()) - { - } - Matrix34 operator()(const Pose3 &pose) const - { - return K_ * (pose.inverse().matrix()).block<3, 4>(0, 0); - } +template +struct CameraProjectionMatrix { + CameraProjectionMatrix(const CALIBRATION& calibration) : + K_(calibration.K()) { + } + Matrix34 operator()(const Pose3& pose) const { + return K_ * (pose.inverse().matrix()).block<3, 4>(0, 0); + } +private: + const Matrix3 K_; +public: + GTSAM_MAKE_ALIGNED_OPERATOR_NEW +}; - private: - const Matrix3 K_; - - public: - GTSAM_MAKE_ALIGNED_OPERATOR_NEW - }; - - /** +/** * Function to triangulate 3D landmark point from an arbitrary number * of poses (at least 2) using the DLT. The function checks that the * resulting point lies in front of all cameras, but has no other checks @@ -224,45 +211,43 @@ namespace gtsam * @param optimize Flag to turn on nonlinear refinement of triangulation * @return Returns a Point3 */ - template - Point3 triangulatePoint3(const std::vector &poses, - boost::shared_ptr sharedCal, - const Point2Vector &measurements, double rank_tol = 1e-9, - bool optimize = false) - { +template +Point3 triangulatePoint3(const std::vector& poses, + boost::shared_ptr sharedCal, + const Point2Vector& measurements, double rank_tol = 1e-9, + bool optimize = false) { - assert(poses.size() == measurements.size()); - if (poses.size() < 2) - throw(TriangulationUnderconstrainedException()); + assert(poses.size() == measurements.size()); + if (poses.size() < 2) + throw(TriangulationUnderconstrainedException()); - // construct projection matrices from poses & calibration - std::vector> projection_matrices; - CameraProjectionMatrix createP(*sharedCal); // partially apply - for (const Pose3 &pose : poses) - projection_matrices.push_back(createP(pose)); + // construct projection matrices from poses & calibration + std::vector> projection_matrices; + CameraProjectionMatrix createP(*sharedCal); // partially apply + for(const Pose3& pose: poses) + projection_matrices.push_back(createP(pose)); - // Triangulate linearly - Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol); + // Triangulate linearly + Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol); - // Then refine using non-linear optimization - if (optimize) - point = triangulateNonlinear // - (poses, sharedCal, measurements, point); + // Then refine using non-linear optimization + if (optimize) + point = triangulateNonlinear // + (poses, sharedCal, measurements, point); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION - // verify that the triangulated point lies in front of all cameras - for (const Pose3 &pose : poses) - { - const Point3 &p_local = pose.transformTo(point); - if (p_local.z() <= 0) - throw(TriangulationCheiralityException()); - } + // verify that the triangulated point lies in front of all cameras + for(const Pose3& pose: poses) { + const Point3& p_local = pose.transformTo(point); + if (p_local.z() <= 0) + throw(TriangulationCheiralityException()); + } #endif - return point; - } + return point; +} - /** +/** * Function to triangulate 3D landmark point from an arbitrary number * of poses (at least 2) using the DLT. This function is similar to the one * above, except that each camera has its own calibration. The function @@ -274,76 +259,72 @@ namespace gtsam * @param optimize Flag to turn on nonlinear refinement of triangulation * @return Returns a Point3 */ - template - Point3 triangulatePoint3( - const CameraSet &cameras, - const typename CAMERA::MeasurementVector &measurements, double rank_tol = 1e-9, - bool optimize = false) - { +template +Point3 triangulatePoint3( + const CameraSet& cameras, + const typename CAMERA::MeasurementVector& measurements, double rank_tol = 1e-9, + bool optimize = false) { - size_t m = cameras.size(); - assert(measurements.size() == m); + size_t m = cameras.size(); + assert(measurements.size() == m); - if (m < 2) - throw(TriangulationUnderconstrainedException()); + if (m < 2) + throw(TriangulationUnderconstrainedException()); - // construct projection matrices from poses & calibration - std::vector> projection_matrices; - for (const CAMERA &camera : cameras) - projection_matrices.push_back( - CameraProjectionMatrix(camera.calibration())( - camera.pose())); - Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol); + // construct projection matrices from poses & calibration + std::vector> projection_matrices; + for(const CAMERA& camera: cameras) + projection_matrices.push_back( + CameraProjectionMatrix(camera.calibration())( + camera.pose())); + Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol); - // The n refine using non-linear optimization - if (optimize) - point = triangulateNonlinear(cameras, measurements, point); + // The n refine using non-linear optimization + if (optimize) + point = triangulateNonlinear(cameras, measurements, point); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION - // verify that the triangulated point lies in front of all cameras - for (const CAMERA &camera : cameras) - { - const Point3 &p_local = camera.pose().transformTo(point); - if (p_local.z() <= 0) - throw(TriangulationCheiralityException()); - } + // verify that the triangulated point lies in front of all cameras + for(const CAMERA& camera: cameras) { + const Point3& p_local = camera.pose().transformTo(point); + if (p_local.z() <= 0) + throw(TriangulationCheiralityException()); + } #endif - return point; - } + return point; +} - /// Pinhole-specific version - template - Point3 triangulatePoint3( - const CameraSet> &cameras, - const Point2Vector &measurements, double rank_tol = 1e-9, - bool optimize = false) - { - return triangulatePoint3> // - (cameras, measurements, rank_tol, optimize); - } +/// Pinhole-specific version +template +Point3 triangulatePoint3( + const CameraSet >& cameras, + const Point2Vector& measurements, double rank_tol = 1e-9, + bool optimize = false) { + return triangulatePoint3 > // + (cameras, measurements, rank_tol, optimize); +} - struct GTSAM_EXPORT TriangulationParameters - { +struct GTSAM_EXPORT TriangulationParameters { - double rankTolerance; ///< threshold to decide whether triangulation is result.degenerate - ///< (the rank is the number of singular values of the triangulation matrix which are larger than rankTolerance) - bool enableEPI; ///< if set to true, will refine triangulation using LM + double rankTolerance; ///< threshold to decide whether triangulation is result.degenerate + ///< (the rank is the number of singular values of the triangulation matrix which are larger than rankTolerance) + bool enableEPI; ///< if set to true, will refine triangulation using LM - /** + /** * if the landmark is triangulated at distance larger than this, * result is flagged as degenerate. */ - double landmarkDistanceThreshold; // + double landmarkDistanceThreshold; // - /** + /** * If this is nonnegative the we will check if the average reprojection error * is smaller than this threshold after triangulation, otherwise result is * flagged as degenerate. */ - double dynamicOutlierRejectionThreshold; + double dynamicOutlierRejectionThreshold; - /** + /** * Constructor * @param rankTol tolerance used to check if point triangulation is degenerate * @param enableEPI if true refine triangulation with embedded LM iterations @@ -351,194 +332,173 @@ namespace gtsam * @param dynamicOutlierRejectionThreshold or if average error larger than this * */ - TriangulationParameters(const double _rankTolerance = 1.0, - const bool _enableEPI = false, double _landmarkDistanceThreshold = -1, - double _dynamicOutlierRejectionThreshold = -1) : rankTolerance(_rankTolerance), enableEPI(_enableEPI), // - landmarkDistanceThreshold(_landmarkDistanceThreshold), // - dynamicOutlierRejectionThreshold(_dynamicOutlierRejectionThreshold) - { - } - - // stream to output - friend std::ostream &operator<<(std::ostream &os, - const TriangulationParameters &p) - { - os << "rankTolerance = " << p.rankTolerance << std::endl; - os << "enableEPI = " << p.enableEPI << std::endl; - os << "landmarkDistanceThreshold = " << p.landmarkDistanceThreshold - << std::endl; - os << "dynamicOutlierRejectionThreshold = " - << p.dynamicOutlierRejectionThreshold << std::endl; - return os; - } - - private: - /// Serialization function - friend class boost::serialization::access; - template - void serialize(ARCHIVE &ar, const unsigned int version) - { - ar &BOOST_SERIALIZATION_NVP(rankTolerance); - ar &BOOST_SERIALIZATION_NVP(enableEPI); - ar &BOOST_SERIALIZATION_NVP(landmarkDistanceThreshold); - ar &BOOST_SERIALIZATION_NVP(dynamicOutlierRejectionThreshold); - } - }; - - /** - * TriangulationResult is an optional point, along with the reasons why it is invalid. - */ - class TriangulationResult : public boost::optional - { - enum Status - { - VALID, - DEGENERATE, - BEHIND_CAMERA, - OUTLIER, - FAR_POINT - }; - Status status_; - TriangulationResult(Status s) : status_(s) - { - } - - public: - /** - * Default constructor, only for serialization - */ - TriangulationResult() {} - - /** - * Constructor - */ - TriangulationResult(const Point3 &p) : status_(VALID) - { - reset(p); - } - static TriangulationResult Degenerate() - { - return TriangulationResult(DEGENERATE); - } - static TriangulationResult Outlier() - { - return TriangulationResult(OUTLIER); - } - static TriangulationResult FarPoint() - { - return TriangulationResult(FAR_POINT); - } - static TriangulationResult BehindCamera() - { - return TriangulationResult(BEHIND_CAMERA); - } - bool valid() const - { - return status_ == VALID; - } - bool degenerate() const - { - return status_ == DEGENERATE; - } - bool outlier() const - { - return status_ == OUTLIER; - } - bool farPoint() const - { - return status_ == FAR_POINT; - } - bool behindCamera() const - { - return status_ == BEHIND_CAMERA; - } - // stream to output - friend std::ostream &operator<<(std::ostream &os, - const TriangulationResult &result) - { - if (result) - os << "point = " << *result << std::endl; - else - os << "no point, status = " << result.status_ << std::endl; - return os; - } - - private: - /// Serialization function - friend class boost::serialization::access; - template - void serialize(ARCHIVE &ar, const unsigned int version) - { - ar &BOOST_SERIALIZATION_NVP(status_); - } - }; - - /// triangulateSafe: extensive checking of the outcome - template - TriangulationResult triangulateSafe(const CameraSet &cameras, - const typename CAMERA::MeasurementVector &measured, - const TriangulationParameters ¶ms) - { - - size_t m = cameras.size(); - - // if we have a single pose the corresponding factor is uninformative - if (m < 2) - return TriangulationResult::Degenerate(); - else - // We triangulate the 3D position of the landmark - try - { - Point3 point = triangulatePoint3(cameras, measured, - params.rankTolerance, params.enableEPI); - - // Check landmark distance and re-projection errors to avoid outliers - size_t i = 0; - double maxReprojError = 0.0; - for (const CAMERA &camera : cameras) - { - const Pose3 &pose = camera.pose(); - if (params.landmarkDistanceThreshold > 0 && distance3(pose.translation(), point) > params.landmarkDistanceThreshold) - return TriangulationResult::FarPoint(); -#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION - // verify that the triangulated point lies in front of all cameras - // Only needed if this was not yet handled by exception - const Point3 &p_local = pose.transformTo(point); - if (p_local.z() <= 0) - return TriangulationResult::BehindCamera(); -#endif - // Check reprojection error - if (params.dynamicOutlierRejectionThreshold > 0) - { - const Point2 &zi = measured.at(i); - Point2 reprojectionError(camera.project(point) - zi); - maxReprojError = std::max(maxReprojError, reprojectionError.norm()); - } - i += 1; - } - // Flag as degenerate if average reprojection error is too large - if (params.dynamicOutlierRejectionThreshold > 0 && maxReprojError > params.dynamicOutlierRejectionThreshold) - return TriangulationResult::Outlier(); - - // all good! - return TriangulationResult(point); - } - catch (TriangulationUnderconstrainedException &) - { - // This exception is thrown if - // 1) There is a single pose for triangulation - this should not happen because we checked the number of poses before - // 2) The rank of the matrix used for triangulation is < 3: rotation-only, parallel cameras (or motion towards the landmark) - return TriangulationResult::Degenerate(); - } - catch (TriangulationCheiralityException &) - { - // point is behind one of the cameras: can be the case of close-to-parallel cameras or may depend on outliers - return TriangulationResult::BehindCamera(); - } + TriangulationParameters(const double _rankTolerance = 1.0, + const bool _enableEPI = false, double _landmarkDistanceThreshold = -1, + double _dynamicOutlierRejectionThreshold = -1) : + rankTolerance(_rankTolerance), enableEPI(_enableEPI), // + landmarkDistanceThreshold(_landmarkDistanceThreshold), // + dynamicOutlierRejectionThreshold(_dynamicOutlierRejectionThreshold) { } - // Vector of Cameras - used by the Python/MATLAB wrapper - using CameraSetCal3Bundler = CameraSet>; - using CameraSetCal3_S2 = CameraSet>; + // stream to output + friend std::ostream &operator<<(std::ostream &os, + const TriangulationParameters& p) { + os << "rankTolerance = " << p.rankTolerance << std::endl; + os << "enableEPI = " << p.enableEPI << std::endl; + os << "landmarkDistanceThreshold = " << p.landmarkDistanceThreshold + << std::endl; + os << "dynamicOutlierRejectionThreshold = " + << p.dynamicOutlierRejectionThreshold << std::endl; + return os; + } + +private: + + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(rankTolerance); + ar & BOOST_SERIALIZATION_NVP(enableEPI); + ar & BOOST_SERIALIZATION_NVP(landmarkDistanceThreshold); + ar & BOOST_SERIALIZATION_NVP(dynamicOutlierRejectionThreshold); + } +}; + +/** + * TriangulationResult is an optional point, along with the reasons why it is invalid. + */ +class TriangulationResult: public boost::optional { + enum Status { + VALID, DEGENERATE, BEHIND_CAMERA, OUTLIER, FAR_POINT + }; + Status status_; + TriangulationResult(Status s) : + status_(s) { + } +public: + + /** + * Default constructor, only for serialization + */ + TriangulationResult() {} + + /** + * Constructor + */ + TriangulationResult(const Point3& p) : + status_(VALID) { + reset(p); + } + static TriangulationResult Degenerate() { + return TriangulationResult(DEGENERATE); + } + static TriangulationResult Outlier() { + return TriangulationResult(OUTLIER); + } + static TriangulationResult FarPoint() { + return TriangulationResult(FAR_POINT); + } + static TriangulationResult BehindCamera() { + return TriangulationResult(BEHIND_CAMERA); + } + bool valid() const { + return status_ == VALID; + } + bool degenerate() const { + return status_ == DEGENERATE; + } + bool outlier() const { + return status_ == OUTLIER; + } + bool farPoint() const { + return status_ == FAR_POINT; + } + bool behindCamera() const { + return status_ == BEHIND_CAMERA; + } + // stream to output + friend std::ostream &operator<<(std::ostream &os, + const TriangulationResult& result) { + if (result) + os << "point = " << *result << std::endl; + else + os << "no point, status = " << result.status_ << std::endl; + return os; + } + +private: + + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(status_); + } +}; + +/// triangulateSafe: extensive checking of the outcome +template +TriangulationResult triangulateSafe(const CameraSet& cameras, + const typename CAMERA::MeasurementVector& measured, + const TriangulationParameters& params) { + + size_t m = cameras.size(); + + // if we have a single pose the corresponding factor is uninformative + if (m < 2) + return TriangulationResult::Degenerate(); + else + // We triangulate the 3D position of the landmark + try { + Point3 point = triangulatePoint3(cameras, measured, + params.rankTolerance, params.enableEPI); + + // Check landmark distance and re-projection errors to avoid outliers + size_t i = 0; + double maxReprojError = 0.0; + for(const CAMERA& camera: cameras) { + const Pose3& pose = camera.pose(); + if (params.landmarkDistanceThreshold > 0 + && distance3(pose.translation(), point) + > params.landmarkDistanceThreshold) + return TriangulationResult::FarPoint(); +#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION + // verify that the triangulated point lies in front of all cameras + // Only needed if this was not yet handled by exception + const Point3& p_local = pose.transformTo(point); + if (p_local.z() <= 0) + return TriangulationResult::BehindCamera(); +#endif + // Check reprojection error + if (params.dynamicOutlierRejectionThreshold > 0) { + const Point2& zi = measured.at(i); + Point2 reprojectionError(camera.project(point) - zi); + maxReprojError = std::max(maxReprojError, reprojectionError.norm()); + } + i += 1; + } + // Flag as degenerate if average reprojection error is too large + if (params.dynamicOutlierRejectionThreshold > 0 + && maxReprojError > params.dynamicOutlierRejectionThreshold) + return TriangulationResult::Outlier(); + + // all good! + return TriangulationResult(point); + } catch (TriangulationUnderconstrainedException&) { + // This exception is thrown if + // 1) There is a single pose for triangulation - this should not happen because we checked the number of poses before + // 2) The rank of the matrix used for triangulation is < 3: rotation-only, parallel cameras (or motion towards the landmark) + return TriangulationResult::Degenerate(); + } catch (TriangulationCheiralityException&) { + // point is behind one of the cameras: can be the case of close-to-parallel cameras or may depend on outliers + return TriangulationResult::BehindCamera(); + } +} + +// Vector of Cameras - used by the Python/MATLAB wrapper +using CameraSetCal3Bundler = CameraSet>; +using CameraSetCal3_S2 = CameraSet>; + +} // \namespace gtsam -} // namespace gtsam diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index b33e7ad6e..64e57a1d5 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -118,735 +118,708 @@ * - TODO: Add generalized serialization support via boost.serialization with hooks to matlab save/load */ -namespace gtsam -{ +namespace gtsam { // Actually a FastList #include - class KeyList - { - KeyList(); - KeyList(const gtsam::KeyList &other); +class KeyList { + KeyList(); + KeyList(const gtsam::KeyList& other); - // Note: no print function + // Note: no print function - // common STL methods - size_t size() const; - bool empty() const; - void clear(); + // common STL methods + size_t size() const; + bool empty() const; + void clear(); - // structure specific methods - size_t front() const; - size_t back() const; - void push_back(size_t key); - void push_front(size_t key); - void pop_back(); - void pop_front(); - void sort(); - void remove(size_t key); + // structure specific methods + size_t front() const; + size_t back() const; + void push_back(size_t key); + void push_front(size_t key); + void pop_back(); + void pop_front(); + void sort(); + void remove(size_t key); - void serialize() const; - }; + void serialize() const; +}; - // Actually a FastSet - class KeySet - { - KeySet(); - KeySet(const gtsam::KeySet &set); - KeySet(const gtsam::KeyVector &vector); - KeySet(const gtsam::KeyList &list); +// Actually a FastSet +class KeySet { + KeySet(); + KeySet(const gtsam::KeySet& set); + KeySet(const gtsam::KeyVector& vector); + KeySet(const gtsam::KeyList& list); - // Testable - void print(string s) const; - bool equals(const gtsam::KeySet &other) const; + // Testable + void print(string s) const; + bool equals(const gtsam::KeySet& other) const; - // common STL methods - size_t size() const; - bool empty() const; - void clear(); + // common STL methods + size_t size() const; + bool empty() const; + void clear(); - // structure specific methods - void insert(size_t key); - void merge(const gtsam::KeySet &other); - bool erase(size_t key); // returns true if value was removed - bool count(size_t key) const; // returns true if value exists + // structure specific methods + void insert(size_t key); + void merge(const gtsam::KeySet& other); + bool erase(size_t key); // returns true if value was removed + bool count(size_t key) const; // returns true if value exists - void serialize() const; - }; + void serialize() const; +}; - // Actually a vector - class KeyVector - { - KeyVector(); - KeyVector(const gtsam::KeyVector &other); +// Actually a vector +class KeyVector { + KeyVector(); + KeyVector(const gtsam::KeyVector& other); - // Note: no print function + // Note: no print function - // common STL methods - size_t size() const; - bool empty() const; - void clear(); + // 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; + // 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; - }; + void serialize() const; +}; - // Actually a FastMap - class KeyGroupMap - { - KeyGroupMap(); +// Actually a FastMap +class KeyGroupMap { + KeyGroupMap(); - // Note: no print function + // Note: no print function - // common STL methods - size_t size() const; - bool empty() const; - void clear(); + // common STL methods + size_t size() const; + bool empty() const; + void clear(); - // structure specific methods - size_t at(size_t key) const; - int erase(size_t key); - bool insert2(size_t key, int val); - }; + // structure specific methods + size_t at(size_t key) const; + int erase(size_t key); + bool insert2(size_t key, int val); +}; - // Actually a FastSet - class FactorIndexSet - { - FactorIndexSet(); - FactorIndexSet(const gtsam::FactorIndexSet &set); +// Actually a FastSet +class FactorIndexSet { + FactorIndexSet(); + FactorIndexSet(const gtsam::FactorIndexSet& set); - // common STL methods - size_t size() const; - bool empty() const; - void clear(); + // 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 - }; + // 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); +// Actually a vector +class FactorIndices { + FactorIndices(); + FactorIndices(const gtsam::FactorIndices& other); - // common STL methods - size_t size() const; - bool empty() const; - void clear(); + // 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; - }; + // 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; +}; - //************************************************************************* - // base - //************************************************************************* +//************************************************************************* +// base +//************************************************************************* - /** gtsam namespace functions */ +/** gtsam namespace functions */ #include - bool isDebugVersion(); +bool isDebugVersion(); #include - class IndexPair - { - IndexPair(); - IndexPair(size_t i, size_t j); - size_t i() const; - size_t j() const; - }; +class IndexPair { + IndexPair(); + IndexPair(size_t i, size_t j); + size_t i() const; + size_t j() const; +}; - // template - // class DSFMap { - // DSFMap(); - // KEY find(const KEY& key) const; - // void merge(const KEY& x, const KEY& y); - // std::map sets(); - // }; +// template +// class DSFMap { +// DSFMap(); +// KEY find(const KEY& key) const; +// void merge(const KEY& x, const KEY& y); +// std::map sets(); +// }; - class IndexPairSet - { - IndexPairSet(); - // common STL methods - size_t size() const; - bool empty() const; - void clear(); +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 - }; + // 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); +class IndexPairVector { + IndexPairVector(); + IndexPairVector(const gtsam::IndexPairVector& other); - // common STL methods - size_t size() const; - bool empty() const; - void clear(); + // 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; - }; + // structure specific methods + gtsam::IndexPair at(size_t i) const; + void push_back(gtsam::IndexPair key) const; +}; - gtsam::IndexPairVector IndexPairSetAsArray(gtsam::IndexPairSet &set); +gtsam::IndexPairVector IndexPairSetAsArray(gtsam::IndexPairSet& set); - class IndexPairSetMap - { - IndexPairSetMap(); - // common STL methods - size_t size() const; - bool empty() const; - void clear(); +class IndexPairSetMap { + IndexPairSetMap(); + // common STL methods + size_t size() const; + bool empty() const; + void clear(); - // structure specific methods - gtsam::IndexPairSet at(gtsam::IndexPair &key); - }; + // structure specific methods + gtsam::IndexPairSet at(gtsam::IndexPair& key); +}; - class DSFMapIndexPair - { - DSFMapIndexPair(); - gtsam::IndexPair find(const gtsam::IndexPair &key) const; - void merge(const gtsam::IndexPair &x, const gtsam::IndexPair &y); - gtsam::IndexPairSetMap sets(); - }; +class DSFMapIndexPair { + DSFMapIndexPair(); + gtsam::IndexPair find(const gtsam::IndexPair& key) const; + void merge(const gtsam::IndexPair& x, const gtsam::IndexPair& y); + gtsam::IndexPairSetMap sets(); +}; #include - bool linear_independent(Matrix A, Matrix B, double tol); +bool linear_independent(Matrix A, Matrix B, double tol); #include - virtual class Value - { - // No constructors because this is an abstract class +virtual class Value { + // No constructors because this is an abstract class - // Testable - void print(string s) const; + // Testable + void print(string s) const; - // Manifold - size_t dim() const; - }; + // Manifold + size_t dim() const; +}; #include - template - virtual class GenericValue : gtsam::Value - { - void serializable() const; - }; +template +virtual class GenericValue : gtsam::Value { + void serializable() const; +}; - //************************************************************************* - // geometry - //************************************************************************* +//************************************************************************* +// geometry +//************************************************************************* #include - class Point2 - { - // Standard Constructors - Point2(); - Point2(double x, double y); - Point2(Vector v); +class Point2 { + // Standard Constructors + Point2(); + Point2(double x, double y); + Point2(Vector v); - // Testable - void print(string s) const; - bool equals(const gtsam::Point2 &point, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::Point2& point, double tol) const; - // Group - static gtsam::Point2 identity(); + // Group + static gtsam::Point2 identity(); - // Standard Interface - double x() const; - double y() const; - Vector vector() const; - double distance(const gtsam::Point2 &p2) const; - double norm() const; + // Standard Interface + double x() const; + double y() const; + Vector vector() const; + double distance(const gtsam::Point2& p2) const; + double norm() const; - // enabling serialization functionality - void serialize() const; - }; + // enabling serialization functionality + void serialize() const; +}; // std::vector #include - class Point2Vector - { - // Constructors - Point2Vector(); - Point2Vector(const gtsam::Point2Vector &v); +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); + //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; + //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(); - }; + //Modifiers + void assign(size_t n, const gtsam::Point2& u); + void push_back(const gtsam::Point2& x); + void pop_back(); +}; #include - class StereoPoint2 - { - // Standard Constructors - StereoPoint2(); - StereoPoint2(double uL, double uR, double v); +class StereoPoint2 { + // Standard Constructors + StereoPoint2(); + StereoPoint2(double uL, double uR, double v); - // Testable - void print(string s) const; - bool equals(const gtsam::StereoPoint2 &point, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::StereoPoint2& point, double tol) const; - // Group - static gtsam::StereoPoint2 identity(); - gtsam::StereoPoint2 inverse() const; - gtsam::StereoPoint2 compose(const gtsam::StereoPoint2 &p2) const; - gtsam::StereoPoint2 between(const gtsam::StereoPoint2 &p2) const; + // Group + static gtsam::StereoPoint2 identity(); + gtsam::StereoPoint2 inverse() const; + gtsam::StereoPoint2 compose(const gtsam::StereoPoint2& p2) const; + gtsam::StereoPoint2 between(const gtsam::StereoPoint2& p2) const; - // Manifold - gtsam::StereoPoint2 retract(Vector v) const; - Vector localCoordinates(const gtsam::StereoPoint2 &p) const; + // Manifold + gtsam::StereoPoint2 retract(Vector v) const; + Vector localCoordinates(const gtsam::StereoPoint2& p) const; - // Lie Group - static gtsam::StereoPoint2 Expmap(Vector v); - static Vector Logmap(const gtsam::StereoPoint2 &p); + // Lie Group + static gtsam::StereoPoint2 Expmap(Vector v); + static Vector Logmap(const gtsam::StereoPoint2& p); - // Standard Interface - Vector vector() const; - double uL() const; - double uR() const; - double v() const; + // Standard Interface + Vector vector() const; + double uL() const; + double uR() const; + double v() const; - // enabling serialization functionality - void serialize() const; - }; + // enabling serialization functionality + void serialize() const; +}; #include - class Point3 - { - // Standard Constructors - Point3(); - Point3(double x, double y, double z); - Point3(Vector v); +class Point3 { + // Standard Constructors + Point3(); + Point3(double x, double y, double z); + Point3(Vector v); - // Testable - void print(string s) const; - bool equals(const gtsam::Point3 &p, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::Point3& p, double tol) const; - // Group - static gtsam::Point3 identity(); + // Group + static gtsam::Point3 identity(); - // Standard Interface - Vector vector() const; - double x() const; - double y() const; - double z() const; + // Standard Interface + Vector vector() const; + double x() const; + double y() const; + double z() const; - // enabling serialization functionality - void serialize() const; - }; + // enabling serialization functionality + void serialize() const; +}; #include - class Rot2 - { - // Standard Constructors and Named Constructors - Rot2(); - Rot2(double theta); - static gtsam::Rot2 fromAngle(double theta); - static gtsam::Rot2 fromDegrees(double theta); - static gtsam::Rot2 fromCosSin(double c, double s); +class Rot2 { + // Standard Constructors and Named Constructors + Rot2(); + Rot2(double theta); + static gtsam::Rot2 fromAngle(double theta); + static gtsam::Rot2 fromDegrees(double theta); + static gtsam::Rot2 fromCosSin(double c, double s); - // Testable - void print(string s) const; - bool equals(const gtsam::Rot2 &rot, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::Rot2& rot, double tol) const; - // Group - static gtsam::Rot2 identity(); - gtsam::Rot2 inverse(); - gtsam::Rot2 compose(const gtsam::Rot2 &p2) const; - gtsam::Rot2 between(const gtsam::Rot2 &p2) const; + // Group + static gtsam::Rot2 identity(); + gtsam::Rot2 inverse(); + gtsam::Rot2 compose(const gtsam::Rot2& p2) const; + gtsam::Rot2 between(const gtsam::Rot2& p2) const; - // Manifold - gtsam::Rot2 retract(Vector v) const; - Vector localCoordinates(const gtsam::Rot2 &p) const; + // Manifold + gtsam::Rot2 retract(Vector v) const; + Vector localCoordinates(const gtsam::Rot2& p) const; - // Lie Group - static gtsam::Rot2 Expmap(Vector v); - static Vector Logmap(const gtsam::Rot2 &p); - Vector logmap(const gtsam::Rot2 &p); + // Lie Group + static gtsam::Rot2 Expmap(Vector v); + static Vector Logmap(const gtsam::Rot2& p); + Vector logmap(const gtsam::Rot2& p); - // Group Action on Point2 - gtsam::Point2 rotate(const gtsam::Point2 &point) const; - gtsam::Point2 unrotate(const gtsam::Point2 &point) const; + // Group Action on Point2 + gtsam::Point2 rotate(const gtsam::Point2& point) const; + gtsam::Point2 unrotate(const gtsam::Point2& point) const; - // Standard Interface - static gtsam::Rot2 relativeBearing(const gtsam::Point2 &d); // Ignoring derivative - static gtsam::Rot2 atan2(double y, double x); - double theta() const; - double degrees() const; - double c() const; - double s() const; - Matrix matrix() const; + // Standard Interface + static gtsam::Rot2 relativeBearing(const gtsam::Point2& d); // Ignoring derivative + static gtsam::Rot2 atan2(double y, double x); + double theta() const; + double degrees() const; + double c() const; + double s() const; + Matrix matrix() const; - // enabling serialization functionality - void serialize() const; - }; + // enabling serialization functionality + void serialize() const; +}; #include - class SO3 - { - // Standard Constructors - SO3(); - SO3(Matrix R); - static gtsam::SO3 FromMatrix(Matrix R); - static gtsam::SO3 AxisAngle(const Vector axis, double theta); - static gtsam::SO3 ClosestTo(const Matrix M); +class SO3 { + // Standard Constructors + SO3(); + SO3(Matrix R); + static gtsam::SO3 FromMatrix(Matrix R); + static gtsam::SO3 AxisAngle(const Vector axis, double theta); + static gtsam::SO3 ClosestTo(const Matrix M); - // Testable - void print(string s) const; - bool equals(const gtsam::SO3 &other, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::SO3& other, double tol) const; - // Group - static gtsam::SO3 identity(); - gtsam::SO3 inverse() const; - gtsam::SO3 between(const gtsam::SO3 &R) const; - gtsam::SO3 compose(const gtsam::SO3 &R) const; + // Group + static gtsam::SO3 identity(); + gtsam::SO3 inverse() const; + gtsam::SO3 between(const gtsam::SO3& R) const; + gtsam::SO3 compose(const gtsam::SO3& R) const; - // Manifold - gtsam::SO3 retract(Vector v) const; - Vector localCoordinates(const gtsam::SO3 &R) const; - static gtsam::SO3 Expmap(Vector v); + // Manifold + gtsam::SO3 retract(Vector v) const; + Vector localCoordinates(const gtsam::SO3& R) const; + static gtsam::SO3 Expmap(Vector v); - // Other methods - Vector vec() const; - Matrix matrix() const; - }; + // Other methods + Vector vec() const; + Matrix matrix() const; +}; #include - class SO4 - { - // Standard Constructors - SO4(); - SO4(Matrix R); - static gtsam::SO4 FromMatrix(Matrix R); +class SO4 { + // Standard Constructors + SO4(); + SO4(Matrix R); + static gtsam::SO4 FromMatrix(Matrix R); - // Testable - void print(string s) const; - bool equals(const gtsam::SO4 &other, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::SO4& other, double tol) const; - // Group - static gtsam::SO4 identity(); - gtsam::SO4 inverse() const; - gtsam::SO4 between(const gtsam::SO4 &Q) const; - gtsam::SO4 compose(const gtsam::SO4 &Q) const; + // Group + static gtsam::SO4 identity(); + gtsam::SO4 inverse() const; + gtsam::SO4 between(const gtsam::SO4& Q) const; + gtsam::SO4 compose(const gtsam::SO4& Q) const; - // Manifold - gtsam::SO4 retract(Vector v) const; - Vector localCoordinates(const gtsam::SO4 &Q) const; - static gtsam::SO4 Expmap(Vector v); + // Manifold + gtsam::SO4 retract(Vector v) const; + Vector localCoordinates(const gtsam::SO4& Q) const; + static gtsam::SO4 Expmap(Vector v); - // Other methods - Vector vec() const; - Matrix matrix() const; - }; + // Other methods + Vector vec() const; + Matrix matrix() const; +}; #include - class SOn - { - // Standard Constructors - SOn(size_t n); - static gtsam::SOn FromMatrix(Matrix R); - static gtsam::SOn Lift(size_t n, Matrix R); +class SOn { + // Standard Constructors + SOn(size_t n); + static gtsam::SOn FromMatrix(Matrix R); + static gtsam::SOn Lift(size_t n, Matrix R); - // Testable - void print(string s) const; - bool equals(const gtsam::SOn &other, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::SOn& other, double tol) const; - // Group - static gtsam::SOn identity(); - gtsam::SOn inverse() const; - gtsam::SOn between(const gtsam::SOn &Q) const; - gtsam::SOn compose(const gtsam::SOn &Q) const; + // Group + static gtsam::SOn identity(); + gtsam::SOn inverse() const; + gtsam::SOn between(const gtsam::SOn& Q) const; + gtsam::SOn compose(const gtsam::SOn& Q) const; - // Manifold - gtsam::SOn retract(Vector v) const; - Vector localCoordinates(const gtsam::SOn &Q) const; - static gtsam::SOn Expmap(Vector v); + // Manifold + gtsam::SOn retract(Vector v) const; + Vector localCoordinates(const gtsam::SOn& Q) const; + static gtsam::SOn Expmap(Vector v); - // Other methods - Vector vec() const; - Matrix matrix() const; + // Other methods + Vector vec() const; + Matrix matrix() const; - // enabling serialization functionality - void serialize() const; - }; + // enabling serialization functionality + void serialize() const; +}; #include - class Quaternion - { - double w() const; - double x() const; - double y() const; - double z() const; - Vector coeffs() const; - }; +class Quaternion { + double w() const; + double x() const; + double y() const; + double z() const; + Vector coeffs() const; +}; #include - class Rot3 - { - // Standard Constructors and Named Constructors - Rot3(); - Rot3(Matrix R); - Rot3(const gtsam::Point3 &col1, const gtsam::Point3 &col2, const gtsam::Point3 &col3); - Rot3(double R11, double R12, double R13, - double R21, double R22, double R23, - double R31, double R32, double R33); - Rot3(double w, double x, double y, double z); +class Rot3 { + // Standard Constructors and Named Constructors + Rot3(); + Rot3(Matrix R); + Rot3(const gtsam::Point3& col1, const gtsam::Point3& col2, const gtsam::Point3& col3); + Rot3(double R11, double R12, double R13, + double R21, double R22, double R23, + double R31, double R32, double R33); + Rot3(double w, double x, double y, double z); - static gtsam::Rot3 Rx(double t); - static gtsam::Rot3 Ry(double t); - static gtsam::Rot3 Rz(double t); - static gtsam::Rot3 RzRyRx(double x, double y, double z); - static gtsam::Rot3 RzRyRx(Vector xyz); - static gtsam::Rot3 Yaw(double t); // positive yaw is to right (as in aircraft heading) - static gtsam::Rot3 Pitch(double t); // positive pitch is up (increasing aircraft altitude) - static gtsam::Rot3 Roll(double t); // positive roll is to right (increasing yaw in aircraft) - static gtsam::Rot3 Ypr(double y, double p, double r); - static gtsam::Rot3 Quaternion(double w, double x, double y, double z); - static gtsam::Rot3 AxisAngle(const gtsam::Point3 &axis, double angle); - static gtsam::Rot3 Rodrigues(Vector v); - static gtsam::Rot3 Rodrigues(double wx, double wy, double wz); - static gtsam::Rot3 ClosestTo(const Matrix M); + static gtsam::Rot3 Rx(double t); + static gtsam::Rot3 Ry(double t); + static gtsam::Rot3 Rz(double t); + static gtsam::Rot3 RzRyRx(double x, double y, double z); + static gtsam::Rot3 RzRyRx(Vector xyz); + static gtsam::Rot3 Yaw(double t); // positive yaw is to right (as in aircraft heading) + static gtsam::Rot3 Pitch(double t); // positive pitch is up (increasing aircraft altitude) + static gtsam::Rot3 Roll(double t); // positive roll is to right (increasing yaw in aircraft) + static gtsam::Rot3 Ypr(double y, double p, double r); + static gtsam::Rot3 Quaternion(double w, double x, double y, double z); + static gtsam::Rot3 AxisAngle(const gtsam::Point3& axis, double angle); + static gtsam::Rot3 Rodrigues(Vector v); + static gtsam::Rot3 Rodrigues(double wx, double wy, double wz); + static gtsam::Rot3 ClosestTo(const Matrix M); - // Testable - void print(string s) const; - bool equals(const gtsam::Rot3 &rot, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::Rot3& rot, double tol) const; - // Group - static gtsam::Rot3 identity(); + // Group + static gtsam::Rot3 identity(); gtsam::Rot3 inverse() const; - gtsam::Rot3 compose(const gtsam::Rot3 &p2) const; - gtsam::Rot3 between(const gtsam::Rot3 &p2) const; + gtsam::Rot3 compose(const gtsam::Rot3& p2) const; + gtsam::Rot3 between(const gtsam::Rot3& p2) const; - // Manifold - //gtsam::Rot3 retractCayley(Vector v) const; // TODO, does not exist in both Matrix and Quaternion options - gtsam::Rot3 retract(Vector v) const; - Vector localCoordinates(const gtsam::Rot3 &p) const; + // Manifold + //gtsam::Rot3 retractCayley(Vector v) const; // TODO, does not exist in both Matrix and Quaternion options + gtsam::Rot3 retract(Vector v) const; + Vector localCoordinates(const gtsam::Rot3& p) const; - // Group Action on Point3 - gtsam::Point3 rotate(const gtsam::Point3 &p) const; - gtsam::Point3 unrotate(const gtsam::Point3 &p) const; + // Group Action on Point3 + gtsam::Point3 rotate(const gtsam::Point3& p) const; + gtsam::Point3 unrotate(const gtsam::Point3& p) const; - // Standard Interface - static gtsam::Rot3 Expmap(Vector v); - static Vector Logmap(const gtsam::Rot3 &p); - Vector logmap(const gtsam::Rot3 &p); - Matrix matrix() const; - Matrix transpose() const; - gtsam::Point3 column(size_t index) const; - Vector xyz() const; - Vector ypr() const; - Vector rpy() const; - double roll() const; - double pitch() const; - double yaw() const; - pair axisAngle() const; - gtsam::Quaternion toQuaternion() const; - Vector quaternion() const; - gtsam::Rot3 slerp(double t, const gtsam::Rot3 &other) const; + // Standard Interface + static gtsam::Rot3 Expmap(Vector v); + static Vector Logmap(const gtsam::Rot3& p); + Vector logmap(const gtsam::Rot3& p); + Matrix matrix() const; + Matrix transpose() const; + gtsam::Point3 column(size_t index) const; + Vector xyz() const; + Vector ypr() const; + Vector rpy() const; + double roll() const; + double pitch() const; + double yaw() const; + pair axisAngle() const; + gtsam::Quaternion toQuaternion() const; + Vector quaternion() const; + gtsam::Rot3 slerp(double t, const gtsam::Rot3& other) const; - // enabling serialization functionality - void serialize() const; - }; + // enabling serialization functionality + void serialize() const; +}; #include - class Pose2 - { - // Standard Constructor - Pose2(); - Pose2(const gtsam::Pose2 &other); - Pose2(double x, double y, double theta); - Pose2(double theta, const gtsam::Point2 &t); - Pose2(const gtsam::Rot2 &r, const gtsam::Point2 &t); - Pose2(Vector v); +class Pose2 { + // Standard Constructor + Pose2(); + Pose2(const gtsam::Pose2& other); + Pose2(double x, double y, double theta); + Pose2(double theta, const gtsam::Point2& t); + Pose2(const gtsam::Rot2& r, const gtsam::Point2& t); + Pose2(Vector v); - // Testable - void print(string s) const; - bool equals(const gtsam::Pose2 &pose, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::Pose2& pose, double tol) const; - // Group - static gtsam::Pose2 identity(); - gtsam::Pose2 inverse() const; - gtsam::Pose2 compose(const gtsam::Pose2 &p2) const; - gtsam::Pose2 between(const gtsam::Pose2 &p2) const; + // Group + static gtsam::Pose2 identity(); + gtsam::Pose2 inverse() const; + gtsam::Pose2 compose(const gtsam::Pose2& p2) const; + gtsam::Pose2 between(const gtsam::Pose2& p2) const; - // Manifold - gtsam::Pose2 retract(Vector v) const; - Vector localCoordinates(const gtsam::Pose2 &p) const; + // Manifold + gtsam::Pose2 retract(Vector v) const; + Vector localCoordinates(const gtsam::Pose2& p) const; - // Lie Group - static gtsam::Pose2 Expmap(Vector v); - static Vector Logmap(const gtsam::Pose2 &p); - Vector logmap(const gtsam::Pose2 &p); - static Matrix ExpmapDerivative(Vector v); - static Matrix LogmapDerivative(const gtsam::Pose2 &v); - Matrix AdjointMap() const; - Vector Adjoint(Vector xi) const; - static Matrix adjointMap_(Vector v); - static Vector adjoint_(Vector xi, Vector y); - static Vector adjointTranspose(Vector xi, Vector y); - static Matrix wedge(double vx, double vy, double w); + // Lie Group + static gtsam::Pose2 Expmap(Vector v); + static Vector Logmap(const gtsam::Pose2& p); + Vector logmap(const gtsam::Pose2& p); + static Matrix ExpmapDerivative(Vector v); + static Matrix LogmapDerivative(const gtsam::Pose2& v); + Matrix AdjointMap() const; + Vector Adjoint(Vector xi) const; + static Matrix adjointMap_(Vector v); + static Vector adjoint_(Vector xi, Vector y); + static Vector adjointTranspose(Vector xi, Vector y); + static Matrix wedge(double vx, double vy, double w); - // Group Actions on Point2 - gtsam::Point2 transformFrom(const gtsam::Point2 &p) const; - gtsam::Point2 transformTo(const gtsam::Point2 &p) const; + // Group Actions on Point2 + gtsam::Point2 transformFrom(const gtsam::Point2& p) const; + gtsam::Point2 transformTo(const gtsam::Point2& p) const; - // Standard Interface - double x() const; - double y() const; - double theta() const; - gtsam::Rot2 bearing(const gtsam::Point2 &point) const; - double range(const gtsam::Point2 &point) const; - gtsam::Point2 translation() const; - gtsam::Rot2 rotation() const; - Matrix matrix() const; + // Standard Interface + double x() const; + double y() const; + double theta() const; + gtsam::Rot2 bearing(const gtsam::Point2& point) const; + double range(const gtsam::Point2& point) const; + gtsam::Point2 translation() const; + gtsam::Rot2 rotation() const; + Matrix matrix() const; - // enabling serialization functionality - void serialize() const; - }; + // enabling serialization functionality + void serialize() const; +}; #include - class Pose3 - { - // Standard Constructors - Pose3(); - Pose3(const gtsam::Pose3 &other); - Pose3(const gtsam::Rot3 &r, const gtsam::Point3 &t); - Pose3(const gtsam::Pose2 &pose2); - Pose3(Matrix mat); +class Pose3 { + // Standard Constructors + Pose3(); + Pose3(const gtsam::Pose3& other); + Pose3(const gtsam::Rot3& r, const gtsam::Point3& t); + Pose3(const gtsam::Pose2& pose2); + Pose3(Matrix mat); - // Testable - void print(string s) const; - bool equals(const gtsam::Pose3 &pose, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::Pose3& pose, double tol) const; - // Group - static gtsam::Pose3 identity(); - gtsam::Pose3 inverse() const; - gtsam::Pose3 compose(const gtsam::Pose3 &pose) const; - gtsam::Pose3 between(const gtsam::Pose3 &pose) const; + // Group + static gtsam::Pose3 identity(); + gtsam::Pose3 inverse() const; + gtsam::Pose3 compose(const gtsam::Pose3& pose) const; + gtsam::Pose3 between(const gtsam::Pose3& pose) const; - // Manifold - gtsam::Pose3 retract(Vector v) const; - Vector localCoordinates(const gtsam::Pose3 &pose) const; + // Manifold + gtsam::Pose3 retract(Vector v) const; + Vector localCoordinates(const gtsam::Pose3& pose) const; - // Lie Group - static gtsam::Pose3 Expmap(Vector v); - static Vector Logmap(const gtsam::Pose3 &pose); - Vector logmap(const gtsam::Pose3 &pose); - Matrix AdjointMap() const; - Vector Adjoint(Vector xi) const; - static Matrix adjointMap_(Vector xi); - static Vector adjoint_(Vector xi, Vector y); - static Vector adjointTranspose(Vector xi, Vector y); - static Matrix ExpmapDerivative(Vector xi); - static Matrix LogmapDerivative(const gtsam::Pose3 &xi); - static Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz); + // Lie Group + static gtsam::Pose3 Expmap(Vector v); + static Vector Logmap(const gtsam::Pose3& pose); + Vector logmap(const gtsam::Pose3& pose); + Matrix AdjointMap() const; + Vector Adjoint(Vector xi) const; + static Matrix adjointMap_(Vector xi); + static Vector adjoint_(Vector xi, Vector y); + static Vector adjointTranspose(Vector xi, Vector y); + static Matrix ExpmapDerivative(Vector xi); + static Matrix LogmapDerivative(const gtsam::Pose3& xi); + static Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz); - // Group Action on Point3 - gtsam::Point3 transformFrom(const gtsam::Point3 &point) const; - gtsam::Point3 transformTo(const gtsam::Point3 &point) const; + // Group Action on Point3 + gtsam::Point3 transformFrom(const gtsam::Point3& point) const; + gtsam::Point3 transformTo(const gtsam::Point3& point) const; - // Standard Interface - gtsam::Rot3 rotation() const; - gtsam::Point3 translation() const; - double x() const; - double y() const; - double z() const; - Matrix matrix() const; - gtsam::Pose3 transformPoseFrom(const gtsam::Pose3 &pose) const; - gtsam::Pose3 transformPoseTo(const gtsam::Pose3 &pose) const; - double range(const gtsam::Point3 &point); - double range(const gtsam::Pose3 &pose); + // Standard Interface + gtsam::Rot3 rotation() const; + gtsam::Point3 translation() const; + double x() const; + double y() const; + double z() const; + Matrix matrix() const; + gtsam::Pose3 transformPoseFrom(const gtsam::Pose3& pose) const; + gtsam::Pose3 transformPoseTo(const gtsam::Pose3& pose) const; + double range(const gtsam::Point3& point); + double range(const gtsam::Pose3& pose); - // enabling serialization functionality - void serialize() const; - }; + // enabling serialization functionality + void serialize() const; +}; // std::vector #include - class Pose3Vector - { - Pose3Vector(); - size_t size() const; - bool empty() const; - gtsam::Pose3 at(size_t n) const; - void push_back(const gtsam::Pose3 &pose); - }; +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 - { - // Standard Constructors - Unit3(); - Unit3(const gtsam::Point3 &pose); +class Unit3 { + // Standard Constructors + Unit3(); + Unit3(const gtsam::Point3& pose); - // Testable - void print(string s) const; - bool equals(const gtsam::Unit3 &pose, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::Unit3& pose, double tol) const; - // Other functionality - Matrix basis() const; - Matrix skew() const; - gtsam::Point3 point3() const; + // Other functionality + Matrix basis() const; + Matrix skew() const; + gtsam::Point3 point3() const; - // Manifold - static size_t Dim(); - size_t dim() const; - gtsam::Unit3 retract(Vector v) const; - Vector localCoordinates(const gtsam::Unit3 &s) const; - }; + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::Unit3 retract(Vector v) const; + Vector localCoordinates(const gtsam::Unit3& s) const; +}; #include - class EssentialMatrix - { - // Standard Constructors - EssentialMatrix(const gtsam::Rot3 &aRb, const gtsam::Unit3 &aTb); +class EssentialMatrix { + // Standard Constructors + EssentialMatrix(const gtsam::Rot3& aRb, const gtsam::Unit3& aTb); - // Testable - void print(string s) const; - bool equals(const gtsam::EssentialMatrix &pose, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::EssentialMatrix& pose, double tol) const; - // Manifold - static size_t Dim(); - size_t dim() const; - gtsam::EssentialMatrix retract(Vector v) const; - Vector localCoordinates(const gtsam::EssentialMatrix &s) const; + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::EssentialMatrix retract(Vector v) const; + Vector localCoordinates(const gtsam::EssentialMatrix& s) const; - // Other methods: - gtsam::Rot3 rotation() const; - gtsam::Unit3 direction() const; - Matrix matrix() const; - double error(Vector vA, Vector vB); - }; + // Other methods: + gtsam::Rot3 rotation() const; + gtsam::Unit3 direction() const; + Matrix matrix() const; + double error(Vector vA, Vector vB); +}; #include class Cal3_S2 { @@ -886,402 +859,389 @@ class Cal3_S2 { }; #include - virtual class Cal3DS2_Base - { - // Standard Constructors - Cal3DS2_Base(); +virtual class Cal3DS2_Base { + // Standard Constructors + Cal3DS2_Base(); - // Testable - void print(string s) const; + // Testable + void print(string s) const; - // Standard Interface - double fx() const; - double fy() const; - double skew() const; - double px() const; - double py() const; - double k1() const; - double k2() const; - Matrix K() const; - Vector k() const; - Vector vector() const; + // Standard Interface + double fx() const; + double fy() const; + double skew() const; + double px() const; + double py() const; + double k1() const; + double k2() const; + Matrix K() const; + Vector k() const; + Vector vector() const; - // Action on Point2 - gtsam::Point2 uncalibrate(const gtsam::Point2 &p) const; - gtsam::Point2 calibrate(const gtsam::Point2 &p) const; + // Action on Point2 + gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; + gtsam::Point2 calibrate(const gtsam::Point2& p) const; - // enabling serialization functionality - void serialize() const; - }; + // enabling serialization functionality + void serialize() const; +}; #include - virtual class Cal3DS2 : gtsam::Cal3DS2_Base - { - // Standard Constructors - Cal3DS2(); - Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2); - Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double p1, double p2); - Cal3DS2(Vector v); +virtual class Cal3DS2 : gtsam::Cal3DS2_Base { + // Standard Constructors + Cal3DS2(); + Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2); + Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double p1, double p2); + Cal3DS2(Vector v); - // Testable - bool equals(const gtsam::Cal3DS2 &rhs, double tol) const; + // Testable + bool equals(const gtsam::Cal3DS2& rhs, double tol) const; - // Manifold - size_t dim() const; - static size_t Dim(); - gtsam::Cal3DS2 retract(Vector v) const; - Vector localCoordinates(const gtsam::Cal3DS2 &c) const; + // Manifold + size_t dim() const; + static size_t Dim(); + gtsam::Cal3DS2 retract(Vector v) const; + Vector localCoordinates(const gtsam::Cal3DS2& c) const; - // enabling serialization functionality - void serialize() const; - }; + // enabling serialization functionality + void serialize() const; +}; #include - virtual class Cal3Unified : gtsam::Cal3DS2_Base - { - // Standard Constructors - Cal3Unified(); - Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, double k2); - Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, double k2, double p1, double p2, double xi); - Cal3Unified(Vector v); +virtual class Cal3Unified : gtsam::Cal3DS2_Base { + // Standard Constructors + Cal3Unified(); + Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, double k2); + Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, double k2, double p1, double p2, double xi); + Cal3Unified(Vector v); - // Testable - bool equals(const gtsam::Cal3Unified &rhs, double tol) const; + // Testable + bool equals(const gtsam::Cal3Unified& rhs, double tol) const; - // Standard Interface - double xi() const; - gtsam::Point2 spaceToNPlane(const gtsam::Point2 &p) const; - gtsam::Point2 nPlaneToSpace(const gtsam::Point2 &p) const; + // Standard Interface + double xi() const; + gtsam::Point2 spaceToNPlane(const gtsam::Point2& p) const; + gtsam::Point2 nPlaneToSpace(const gtsam::Point2& p) const; - // Manifold - size_t dim() const; - static size_t Dim(); - gtsam::Cal3Unified retract(Vector v) const; - Vector localCoordinates(const gtsam::Cal3Unified &c) const; + // Manifold + size_t dim() const; + static size_t Dim(); + gtsam::Cal3Unified retract(Vector v) const; + Vector localCoordinates(const gtsam::Cal3Unified& c) const; - // enabling serialization functionality - void serialize() const; - }; + // enabling serialization functionality + void serialize() const; +}; #include - class Cal3_S2Stereo - { - // Standard Constructors - Cal3_S2Stereo(); - Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b); - Cal3_S2Stereo(Vector v); +class Cal3_S2Stereo { + // Standard Constructors + Cal3_S2Stereo(); + Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b); + Cal3_S2Stereo(Vector v); - // Testable - void print(string s) const; - bool equals(const gtsam::Cal3_S2Stereo &K, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::Cal3_S2Stereo& K, double tol) const; - // Standard Interface - double fx() const; - double fy() const; - double skew() const; - double px() const; - double py() const; - gtsam::Point2 principalPoint() const; - double baseline() const; - }; + // Standard Interface + double fx() const; + double fy() const; + double skew() const; + double px() const; + double py() const; + gtsam::Point2 principalPoint() const; + double baseline() const; +}; #include - class Cal3Bundler - { - // Standard Constructors - Cal3Bundler(); - Cal3Bundler(double fx, double k1, double k2, double u0, double v0); - Cal3Bundler(double fx, double k1, double k2, double u0, double v0, double tol); +class Cal3Bundler { + // Standard Constructors + Cal3Bundler(); + Cal3Bundler(double fx, double k1, double k2, double u0, double v0); + Cal3Bundler(double fx, double k1, double k2, double u0, double v0, double tol); - // Testable - void print(string s) const; - bool equals(const gtsam::Cal3Bundler &rhs, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::Cal3Bundler& rhs, double tol) const; - // Manifold - static size_t Dim(); - size_t dim() const; - gtsam::Cal3Bundler retract(Vector v) const; - Vector localCoordinates(const gtsam::Cal3Bundler &c) const; + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::Cal3Bundler retract(Vector v) const; + Vector localCoordinates(const gtsam::Cal3Bundler& c) const; - // Action on Point2 - gtsam::Point2 calibrate(const gtsam::Point2 &p) const; - gtsam::Point2 uncalibrate(const gtsam::Point2 &p) const; + // Action on Point2 + gtsam::Point2 calibrate(const gtsam::Point2& p) const; + gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; - // Standard Interface - double fx() const; - double fy() const; - double k1() const; - double k2() const; - double px() const; - double py() const; - Vector vector() const; - Vector k() const; - Matrix K() const; + // Standard Interface + double fx() const; + double fy() const; + double k1() const; + double k2() const; + double px() const; + double py() const; + Vector vector() const; + Vector k() const; + Matrix K() const; - // enabling serialization functionality - void serialize() const; - }; + // enabling serialization functionality + void serialize() const; +}; #include - class CalibratedCamera - { - // Standard Constructors and Named Constructors - CalibratedCamera(); - CalibratedCamera(const gtsam::Pose3 &pose); - CalibratedCamera(Vector v); - static gtsam::CalibratedCamera Level(const gtsam::Pose2 &pose2, double height); +class CalibratedCamera { + // Standard Constructors and Named Constructors + CalibratedCamera(); + CalibratedCamera(const gtsam::Pose3& pose); + CalibratedCamera(Vector v); + static gtsam::CalibratedCamera Level(const gtsam::Pose2& pose2, double height); - // Testable - void print(string s) const; - bool equals(const gtsam::CalibratedCamera &camera, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::CalibratedCamera& camera, double tol) const; - // Manifold - static size_t Dim(); - size_t dim() const; - gtsam::CalibratedCamera retract(Vector d) const; - Vector localCoordinates(const gtsam::CalibratedCamera &T2) const; + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::CalibratedCamera retract(Vector d) const; + Vector localCoordinates(const gtsam::CalibratedCamera& T2) const; - // Action on Point3 - gtsam::Point2 project(const gtsam::Point3 &point) const; - static gtsam::Point2 Project(const gtsam::Point3 &cameraPoint); + // Action on Point3 + gtsam::Point2 project(const gtsam::Point3& point) const; + static gtsam::Point2 Project(const gtsam::Point3& cameraPoint); - // Standard Interface - gtsam::Pose3 pose() const; - double range(const gtsam::Point3 &p) const; // TODO: Other overloaded range methods + // Standard Interface + gtsam::Pose3 pose() const; + double range(const gtsam::Point3& p) const; // TODO: Other overloaded range methods - // enabling serialization functionality - void serialize() const; - }; + // enabling serialization functionality + void serialize() const; +}; #include - template - class PinholeCamera - { - // Standard Constructors and Named Constructors - PinholeCamera(); - PinholeCamera(const gtsam::Pose3 &pose); - PinholeCamera(const gtsam::Pose3 &pose, const CALIBRATION &K); - static This Level(const CALIBRATION &K, const gtsam::Pose2 &pose, double height); - 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); +template +class PinholeCamera { + // Standard Constructors and Named Constructors + PinholeCamera(); + PinholeCamera(const gtsam::Pose3& pose); + PinholeCamera(const gtsam::Pose3& pose, const CALIBRATION& K); + static This Level(const CALIBRATION& K, const gtsam::Pose2& pose, double height); + 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) const; - bool equals(const This &camera, double tol) const; + // Testable + void print(string s) const; + bool equals(const This& camera, double tol) const; - // Standard Interface - gtsam::Pose3 pose() const; - CALIBRATION calibration() 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(); + // 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::Point3 backproject(const gtsam::Point2 &p, double depth) const; - double range(const gtsam::Point3 &point); - double range(const gtsam::Pose3 &pose); + // 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::Point3 backproject(const gtsam::Point2& p, double depth) const; + double range(const gtsam::Point3& point); + double range(const gtsam::Pose3& pose); - // enabling serialization functionality - void serialize() const; - }; + // enabling serialization functionality + 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; +// 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; - template - class CameraSet - { - CameraSet(); - // structure specific methods - T at(size_t i) const; - void push_back(const T &cam); - }; +template +class CameraSet { + CameraSet(); + + // structure specific methods + T at(size_t i) const; + void push_back(const T& cam); +}; #include - class StereoCamera - { - // Standard Constructors and Named Constructors - StereoCamera(); - StereoCamera(const gtsam::Pose3 &pose, const gtsam::Cal3_S2Stereo *K); +class StereoCamera { + // Standard Constructors and Named Constructors + StereoCamera(); + StereoCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2Stereo* K); - // Testable - void print(string s) const; - bool equals(const gtsam::StereoCamera &camera, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::StereoCamera& camera, double tol) const; - // Standard Interface - gtsam::Pose3 pose() const; - double baseline() const; - gtsam::Cal3_S2Stereo calibration() const; + // Standard Interface + gtsam::Pose3 pose() const; + double baseline() const; + gtsam::Cal3_S2Stereo calibration() const; - // Manifold - gtsam::StereoCamera retract(Vector d) const; - Vector localCoordinates(const gtsam::StereoCamera &T2) const; - size_t dim() const; - static size_t Dim(); + // Manifold + gtsam::StereoCamera retract(Vector d) const; + Vector localCoordinates(const gtsam::StereoCamera& T2) const; + size_t dim() const; + static size_t Dim(); - // Transformations and measurement functions - gtsam::StereoPoint2 project(const gtsam::Point3 &point); - gtsam::Point3 backproject(const gtsam::StereoPoint2 &p) const; + // Transformations and measurement functions + gtsam::StereoPoint2 project(const gtsam::Point3& point); + gtsam::Point3 backproject(const gtsam::StereoPoint2& p) const; - // enabling serialization functionality - void serialize() const; - }; + // enabling serialization functionality + void serialize() const; +}; #include - // Templates appear not yet supported for free functions - issue raised at borglab/wrap#14 to add support - gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector &poses, - gtsam::Cal3_S2 *sharedCal, const gtsam::Point2Vector &measurements, - double rank_tol, bool optimize); - gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector &poses, - gtsam::Cal3DS2 *sharedCal, const gtsam::Point2Vector &measurements, - double rank_tol, bool optimize); - gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector &poses, - gtsam::Cal3Bundler *sharedCal, const gtsam::Point2Vector &measurements, - double rank_tol, bool optimize); - gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3_S2 &cameras, - const gtsam::Point2Vector &measurements, double rank_tol, - bool optimize); - gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Bundler &cameras, - const gtsam::Point2Vector &measurements, double rank_tol, - bool optimize); - - //************************************************************************* - // Symbolic - //************************************************************************* +// Templates appear not yet supported for free functions - issue raised at borglab/wrap#14 to add support +gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, + gtsam::Cal3_S2* sharedCal, const gtsam::Point2Vector& measurements, + double rank_tol, bool optimize); +gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, + gtsam::Cal3DS2* sharedCal, const gtsam::Point2Vector& measurements, + double rank_tol, bool optimize); +gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, + gtsam::Cal3Bundler* sharedCal, const gtsam::Point2Vector& measurements, + double rank_tol, bool optimize); +gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3_S2& cameras, + const gtsam::Point2Vector& measurements, double rank_tol, + bool optimize); +gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Bundler& cameras, + const gtsam::Point2Vector& measurements, double rank_tol, + bool optimize); + +//************************************************************************* +// Symbolic +//************************************************************************* #include - virtual class SymbolicFactor - { - // Standard Constructors and Named Constructors - SymbolicFactor(const gtsam::SymbolicFactor &f); - SymbolicFactor(); - SymbolicFactor(size_t j); - SymbolicFactor(size_t j1, size_t j2); - SymbolicFactor(size_t j1, size_t j2, size_t j3); - SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4); - SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4, size_t j5); - SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4, size_t j5, size_t j6); - static gtsam::SymbolicFactor FromKeys(const gtsam::KeyVector &js); +virtual class SymbolicFactor { + // Standard Constructors and Named Constructors + SymbolicFactor(const gtsam::SymbolicFactor& f); + SymbolicFactor(); + SymbolicFactor(size_t j); + SymbolicFactor(size_t j1, size_t j2); + SymbolicFactor(size_t j1, size_t j2, size_t j3); + SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4); + SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4, size_t j5); + SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4, size_t j5, size_t j6); + static gtsam::SymbolicFactor FromKeys(const gtsam::KeyVector& js); - // From Factor - size_t size() const; - void print(string s) const; - bool equals(const gtsam::SymbolicFactor &other, double tol) const; - gtsam::KeyVector keys(); - }; + // From Factor + size_t size() const; + void print(string s) const; + bool equals(const gtsam::SymbolicFactor& other, double tol) const; + gtsam::KeyVector keys(); +}; #include - virtual class SymbolicFactorGraph - { - SymbolicFactorGraph(); - SymbolicFactorGraph(const gtsam::SymbolicBayesNet &bayesNet); - SymbolicFactorGraph(const gtsam::SymbolicBayesTree &bayesTree); +virtual class SymbolicFactorGraph { + SymbolicFactorGraph(); + SymbolicFactorGraph(const gtsam::SymbolicBayesNet& bayesNet); + SymbolicFactorGraph(const gtsam::SymbolicBayesTree& bayesTree); - // From FactorGraph - void push_back(gtsam::SymbolicFactor *factor); - void print(string s) const; - bool equals(const gtsam::SymbolicFactorGraph &rhs, double tol) const; - size_t size() const; - bool exists(size_t idx) const; + // From FactorGraph + void push_back(gtsam::SymbolicFactor* factor); + void print(string s) const; + bool equals(const gtsam::SymbolicFactorGraph& rhs, double tol) const; + size_t size() const; + bool exists(size_t idx) const; - // Standard interface - gtsam::KeySet keys() const; - void push_back(const gtsam::SymbolicFactorGraph &graph); - void push_back(const gtsam::SymbolicBayesNet &bayesNet); - void push_back(const gtsam::SymbolicBayesTree &bayesTree); + // Standard interface + gtsam::KeySet keys() const; + void push_back(const gtsam::SymbolicFactorGraph& graph); + void push_back(const gtsam::SymbolicBayesNet& bayesNet); + void push_back(const gtsam::SymbolicBayesTree& bayesTree); - //Advanced Interface - void push_factor(size_t key); - void push_factor(size_t key1, size_t key2); - void push_factor(size_t key1, size_t key2, size_t key3); - void push_factor(size_t key1, size_t key2, size_t key3, size_t key4); + //Advanced Interface + void push_factor(size_t key); + void push_factor(size_t key1, size_t key2); + void push_factor(size_t key1, size_t key2, size_t key3); + void push_factor(size_t key1, size_t key2, size_t key3, size_t key4); - gtsam::SymbolicBayesNet *eliminateSequential(); - gtsam::SymbolicBayesNet *eliminateSequential(const gtsam::Ordering &ordering); - gtsam::SymbolicBayesTree *eliminateMultifrontal(); - gtsam::SymbolicBayesTree *eliminateMultifrontal(const gtsam::Ordering &ordering); - pair eliminatePartialSequential( - const gtsam::Ordering &ordering); - pair eliminatePartialSequential( - const gtsam::KeyVector &keys); - pair eliminatePartialMultifrontal( - const gtsam::Ordering &ordering); - pair eliminatePartialMultifrontal( - const gtsam::KeyVector &keys); - gtsam::SymbolicBayesNet *marginalMultifrontalBayesNet(const gtsam::Ordering &ordering); - gtsam::SymbolicBayesNet *marginalMultifrontalBayesNet(const gtsam::KeyVector &key_vector); - gtsam::SymbolicBayesNet *marginalMultifrontalBayesNet(const gtsam::Ordering &ordering, - const gtsam::Ordering &marginalizedVariableOrdering); - gtsam::SymbolicBayesNet *marginalMultifrontalBayesNet(const gtsam::KeyVector &key_vector, - const gtsam::Ordering &marginalizedVariableOrdering); - gtsam::SymbolicFactorGraph *marginal(const gtsam::KeyVector &key_vector); - }; + gtsam::SymbolicBayesNet* eliminateSequential(); + gtsam::SymbolicBayesNet* eliminateSequential(const gtsam::Ordering& ordering); + gtsam::SymbolicBayesTree* eliminateMultifrontal(); + gtsam::SymbolicBayesTree* eliminateMultifrontal(const gtsam::Ordering& ordering); + pair eliminatePartialSequential( + const gtsam::Ordering& ordering); + pair eliminatePartialSequential( + const gtsam::KeyVector& keys); + pair eliminatePartialMultifrontal( + const gtsam::Ordering& ordering); + pair eliminatePartialMultifrontal( + const gtsam::KeyVector& keys); + gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& ordering); + gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& key_vector); + gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& ordering, + const gtsam::Ordering& marginalizedVariableOrdering); + gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& key_vector, + const gtsam::Ordering& marginalizedVariableOrdering); + gtsam::SymbolicFactorGraph* marginal(const gtsam::KeyVector& key_vector); +}; #include - virtual class SymbolicConditional : gtsam::SymbolicFactor - { - // Standard Constructors and Named Constructors - SymbolicConditional(); - SymbolicConditional(const gtsam::SymbolicConditional &other); - SymbolicConditional(size_t key); - SymbolicConditional(size_t key, size_t parent); - SymbolicConditional(size_t key, size_t parent1, size_t parent2); - SymbolicConditional(size_t key, size_t parent1, size_t parent2, size_t parent3); - static gtsam::SymbolicConditional FromKeys(const gtsam::KeyVector &keys, size_t nrFrontals); +virtual class SymbolicConditional : gtsam::SymbolicFactor { + // Standard Constructors and Named Constructors + SymbolicConditional(); + SymbolicConditional(const gtsam::SymbolicConditional& other); + SymbolicConditional(size_t key); + SymbolicConditional(size_t key, size_t parent); + SymbolicConditional(size_t key, size_t parent1, size_t parent2); + SymbolicConditional(size_t key, size_t parent1, size_t parent2, size_t parent3); + static gtsam::SymbolicConditional FromKeys(const gtsam::KeyVector& keys, size_t nrFrontals); - // Testable - void print(string s) const; - bool equals(const gtsam::SymbolicConditional &other, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::SymbolicConditional& other, double tol) const; - // Standard interface - size_t nrFrontals() const; - size_t nrParents() const; - }; + // Standard interface + size_t nrFrontals() const; + size_t nrParents() const; +}; #include - class SymbolicBayesNet - { - SymbolicBayesNet(); - SymbolicBayesNet(const gtsam::SymbolicBayesNet &other); - // Testable - void print(string s) const; - bool equals(const gtsam::SymbolicBayesNet &other, double tol) const; +class SymbolicBayesNet { + SymbolicBayesNet(); + SymbolicBayesNet(const gtsam::SymbolicBayesNet& other); + // Testable + void print(string s) const; + bool equals(const gtsam::SymbolicBayesNet& other, double tol) const; - // Standard interface - size_t size() const; - void saveGraph(string s) const; - gtsam::SymbolicConditional *at(size_t idx) const; - gtsam::SymbolicConditional *front() const; - gtsam::SymbolicConditional *back() const; - void push_back(gtsam::SymbolicConditional *conditional); - void push_back(const gtsam::SymbolicBayesNet &bayesNet); - }; + // Standard interface + size_t size() const; + void saveGraph(string s) const; + gtsam::SymbolicConditional* at(size_t idx) const; + gtsam::SymbolicConditional* front() const; + gtsam::SymbolicConditional* back() const; + void push_back(gtsam::SymbolicConditional* conditional); + void push_back(const gtsam::SymbolicBayesNet& bayesNet); +}; #include - class SymbolicBayesTree - { +class SymbolicBayesTree { //Constructors SymbolicBayesTree(); - SymbolicBayesTree(const gtsam::SymbolicBayesTree &other); + SymbolicBayesTree(const gtsam::SymbolicBayesTree& other); // Testable void print(string s); - bool equals(const gtsam::SymbolicBayesTree &other, double tol) const; + bool equals(const gtsam::SymbolicBayesTree& other, double tol) const; //Standard Interface //size_t findParentClique(const gtsam::IndexVector& parents) const; @@ -1291,1019 +1251,969 @@ class Cal3_S2 { void deleteCachedShortcuts(); size_t numCachedSeparatorMarginals() const; - gtsam::SymbolicConditional *marginalFactor(size_t key) const; - gtsam::SymbolicFactorGraph *joint(size_t key1, size_t key2) const; - gtsam::SymbolicBayesNet *jointBayesNet(size_t key1, size_t key2) const; - }; + gtsam::SymbolicConditional* marginalFactor(size_t key) const; + gtsam::SymbolicFactorGraph* joint(size_t key1, size_t key2) const; + gtsam::SymbolicBayesNet* jointBayesNet(size_t key1, size_t key2) const; +}; - // class SymbolicBayesTreeClique { - // BayesTreeClique(); - // BayesTreeClique(CONDITIONAL* conditional); - // // BayesTreeClique(const pair& result) : Base(result) {} - // - // bool equals(const This& other, double tol) const; - // void print(string s) const; - // void printTree() const; // Default indent of "" - // void printTree(string indent) const; - // size_t numCachedSeparatorMarginals() const; - // - // CONDITIONAL* conditional() const; - // bool isRoot() const; - // size_t treeSize() const; - // // const std::list& children() const { return children_; } - // // derived_ptr parent() const { return parent_.lock(); } - // - // // TODO: need wrapped versions graphs, BayesNet - // // BayesNet shortcut(derived_ptr root, Eliminate function) const; - // // FactorGraph marginal(derived_ptr root, Eliminate function) const; - // // FactorGraph joint(derived_ptr C2, derived_ptr root, Eliminate function) const; - // - // void deleteCachedShortcuts(); - // }; +// class SymbolicBayesTreeClique { +// BayesTreeClique(); +// BayesTreeClique(CONDITIONAL* conditional); +// // BayesTreeClique(const pair& result) : Base(result) {} +// +// bool equals(const This& other, double tol) const; +// void print(string s) const; +// void printTree() const; // Default indent of "" +// void printTree(string indent) const; +// size_t numCachedSeparatorMarginals() const; +// +// CONDITIONAL* conditional() const; +// bool isRoot() const; +// size_t treeSize() const; +// // const std::list& children() const { return children_; } +// // derived_ptr parent() const { return parent_.lock(); } +// +// // TODO: need wrapped versions graphs, BayesNet +// // BayesNet shortcut(derived_ptr root, Eliminate function) const; +// // FactorGraph marginal(derived_ptr root, Eliminate function) const; +// // FactorGraph joint(derived_ptr C2, derived_ptr root, Eliminate function) const; +// +// void deleteCachedShortcuts(); +// }; #include - 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); - VariableIndex(const gtsam::VariableIndex &other); +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); + VariableIndex(const gtsam::VariableIndex& other); - // Testable - bool equals(const gtsam::VariableIndex &other, double tol) const; - void print(string s) const; + // Testable + bool equals(const gtsam::VariableIndex& other, double tol) const; + void print(string s) const; - // Standard interface - size_t size() const; - size_t nFactors() const; - size_t nEntries() const; - }; + // Standard interface + size_t size() const; + size_t nFactors() const; + size_t nEntries() const; +}; - //************************************************************************* - // linear - //************************************************************************* +//************************************************************************* +// linear +//************************************************************************* - namespace noiseModel - { +namespace noiseModel { #include - virtual class Base - { - void print(string s) const; - // Methods below are available for all noise models. However, can't add them - // because wrap (incorrectly) thinks robust classes derive from this Base as well. - // bool isConstrained() const; - // bool isUnit() const; - // size_t dim() const; - // Vector sigmas() const; - }; - - virtual class Gaussian : gtsam::noiseModel::Base - { - static gtsam::noiseModel::Gaussian *Information(Matrix R); - static gtsam::noiseModel::Gaussian *SqrtInformation(Matrix R); - static gtsam::noiseModel::Gaussian *Covariance(Matrix R); - - bool equals(gtsam::noiseModel::Base &expected, double tol); - - // access to noise model - Matrix R() const; - Matrix information() const; - Matrix covariance() const; - - // Whitening operations - Vector whiten(Vector v) const; - Vector unwhiten(Vector v) const; - Matrix Whiten(Matrix H) const; - - // enabling serialization functionality - void serializable() const; - }; - - virtual class Diagonal : gtsam::noiseModel::Gaussian - { - static gtsam::noiseModel::Diagonal *Sigmas(Vector sigmas); - static gtsam::noiseModel::Diagonal *Variances(Vector variances); - static gtsam::noiseModel::Diagonal *Precisions(Vector precisions); - Matrix R() const; - - // access to noise model - Vector sigmas() const; - Vector invsigmas() const; - Vector precisions() const; - - // enabling serialization functionality - void serializable() const; - }; - - virtual class Constrained : gtsam::noiseModel::Diagonal - { - static gtsam::noiseModel::Constrained *MixedSigmas(Vector mu, Vector sigmas); - static gtsam::noiseModel::Constrained *MixedSigmas(double m, Vector sigmas); - static gtsam::noiseModel::Constrained *MixedVariances(Vector mu, Vector variances); - static gtsam::noiseModel::Constrained *MixedVariances(Vector variances); - static gtsam::noiseModel::Constrained *MixedPrecisions(Vector mu, Vector precisions); - static gtsam::noiseModel::Constrained *MixedPrecisions(Vector precisions); - - static gtsam::noiseModel::Constrained *All(size_t dim); - static gtsam::noiseModel::Constrained *All(size_t dim, double mu); - - gtsam::noiseModel::Constrained *unit() const; - - // enabling serialization functionality - void serializable() const; - }; - - virtual class Isotropic : gtsam::noiseModel::Diagonal - { - static gtsam::noiseModel::Isotropic *Sigma(size_t dim, double sigma); - static gtsam::noiseModel::Isotropic *Variance(size_t dim, double varianace); - static gtsam::noiseModel::Isotropic *Precision(size_t dim, double precision); - - // access to noise model - double sigma() const; - - // enabling serialization functionality - void serializable() const; - }; - - virtual class Unit : gtsam::noiseModel::Isotropic - { - static gtsam::noiseModel::Unit *Create(size_t dim); - - // enabling serialization functionality - void serializable() const; - }; - - namespace mEstimator - { - virtual class Base - { - void print(string s) const; - }; - - virtual class Null : gtsam::noiseModel::mEstimator::Base - { - Null(); - static gtsam::noiseModel::mEstimator::Null *Create(); - - // enabling serialization functionality - void serializable() const; - - double weight(double error) const; - double loss(double error) const; - }; - - virtual class Fair : gtsam::noiseModel::mEstimator::Base - { - Fair(double c); - static gtsam::noiseModel::mEstimator::Fair *Create(double c); - - // enabling serialization functionality - void serializable() const; - - double weight(double error) const; - double loss(double error) const; - }; - - virtual class Huber : gtsam::noiseModel::mEstimator::Base - { - Huber(double k); - static gtsam::noiseModel::mEstimator::Huber *Create(double k); - - // enabling serialization functionality - void serializable() const; - - double weight(double error) const; - double loss(double error) const; - }; - - virtual class Cauchy : gtsam::noiseModel::mEstimator::Base - { - Cauchy(double k); - static gtsam::noiseModel::mEstimator::Cauchy *Create(double k); - - // enabling serialization functionality - void serializable() const; - - double weight(double error) const; - double loss(double error) const; - }; - - virtual class Tukey : gtsam::noiseModel::mEstimator::Base - { - Tukey(double k); - static gtsam::noiseModel::mEstimator::Tukey *Create(double k); - - // enabling serialization functionality - void serializable() const; - - double weight(double error) const; - double loss(double error) const; - }; - - virtual class Welsch : gtsam::noiseModel::mEstimator::Base - { - Welsch(double k); - static gtsam::noiseModel::mEstimator::Welsch *Create(double k); - - // enabling serialization functionality - void serializable() const; - - double weight(double error) const; - double loss(double error) const; - }; - - virtual class GemanMcClure : gtsam::noiseModel::mEstimator::Base - { - GemanMcClure(double c); - static gtsam::noiseModel::mEstimator::GemanMcClure *Create(double c); - - // enabling serialization functionality - void serializable() const; - - double weight(double error) const; - double loss(double error) const; - }; - - virtual class DCS : gtsam::noiseModel::mEstimator::Base - { - DCS(double c); - static gtsam::noiseModel::mEstimator::DCS *Create(double c); - - // enabling serialization functionality - void serializable() const; - - double weight(double error) const; - double loss(double error) const; - }; - - virtual class L2WithDeadZone : gtsam::noiseModel::mEstimator::Base - { - L2WithDeadZone(double k); - static gtsam::noiseModel::mEstimator::L2WithDeadZone *Create(double k); - - // enabling serialization functionality - void serializable() const; - - double weight(double error) const; - double loss(double error) const; - }; - - } // namespace mEstimator - - virtual class Robust : gtsam::noiseModel::Base - { - Robust(const gtsam::noiseModel::mEstimator::Base *robust, const gtsam::noiseModel::Base *noise); - static gtsam::noiseModel::Robust *Create(const gtsam::noiseModel::mEstimator::Base *robust, const gtsam::noiseModel::Base *noise); - - // enabling serialization functionality - void serializable() const; - }; - - } // namespace noiseModel - -#include - class Sampler - { - // Constructors - Sampler(gtsam::noiseModel::Diagonal *model, int seed); - Sampler(Vector sigmas, int seed); - - // Standard Interface - size_t dim() const; - Vector sigmas() const; - gtsam::noiseModel::Diagonal *model() const; - Vector sample(); - }; - -#include - class VectorValues - { - //Constructors - VectorValues(); - VectorValues(const gtsam::VectorValues &other); - - //Named Constructors - static gtsam::VectorValues Zero(const gtsam::VectorValues &model); - - //Standard Interface - size_t size() const; - size_t dim(size_t j) const; - bool exists(size_t j) const; - void print(string s) const; - bool equals(const gtsam::VectorValues &expected, double tol) const; - void insert(size_t j, Vector value); - Vector vector() const; - Vector at(size_t j) const; - void update(const gtsam::VectorValues &values); - - //Advanced Interface - void setZero(); - - gtsam::VectorValues add(const gtsam::VectorValues &c) const; - void addInPlace(const gtsam::VectorValues &c); - gtsam::VectorValues subtract(const gtsam::VectorValues &c) const; - gtsam::VectorValues scale(double a) const; - void scaleInPlace(double a); - - bool hasSameStructure(const gtsam::VectorValues &other) const; - double dot(const gtsam::VectorValues &V) const; - double norm() const; - double squaredNorm() const; - - // enabling serialization functionality - void serialize() const; - }; - -#include - virtual class GaussianFactor - { - gtsam::KeyVector keys() const; - void print(string s) const; - bool equals(const gtsam::GaussianFactor &lf, double tol) const; - double error(const gtsam::VectorValues &c) const; - gtsam::GaussianFactor *clone() const; - gtsam::GaussianFactor *negate() const; - Matrix augmentedInformation() const; - Matrix information() const; - Matrix augmentedJacobian() const; - pair jacobian() const; - size_t size() const; - bool empty() const; - }; - -#include - virtual class JacobianFactor : gtsam::GaussianFactor - { - //Constructors - JacobianFactor(); - JacobianFactor(const gtsam::GaussianFactor &factor); - JacobianFactor(Vector b_in); - JacobianFactor(size_t i1, Matrix A1, Vector b, - const gtsam::noiseModel::Diagonal *model); - JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, Vector b, - const gtsam::noiseModel::Diagonal *model); - JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, size_t i3, Matrix A3, - Vector b, const gtsam::noiseModel::Diagonal *model); - JacobianFactor(const gtsam::GaussianFactorGraph &graph); - - //Testable - void print(string s) const; - void printKeys(string s) const; - bool equals(const gtsam::GaussianFactor &lf, double tol) const; - size_t size() const; - Vector unweighted_error(const gtsam::VectorValues &c) const; - Vector error_vector(const gtsam::VectorValues &c) const; - double error(const gtsam::VectorValues &c) const; - - //Standard Interface - Matrix getA() const; - Vector getb() const; - size_t rows() const; - size_t cols() const; - bool isConstrained() const; - pair jacobianUnweighted() const; - Matrix augmentedJacobianUnweighted() const; - - void transposeMultiplyAdd(double alpha, Vector e, gtsam::VectorValues &x) const; - gtsam::JacobianFactor whiten() const; - - pair eliminate(const gtsam::Ordering &keys) const; - - void setModel(bool anyConstrained, Vector sigmas); - - gtsam::noiseModel::Diagonal *get_model() const; - - // enabling serialization functionality - void serialize() const; - }; - -#include - virtual class HessianFactor : gtsam::GaussianFactor - { - //Constructors - HessianFactor(); - HessianFactor(const gtsam::GaussianFactor &factor); - HessianFactor(size_t j, Matrix G, Vector g, double f); - HessianFactor(size_t j, Vector mu, Matrix Sigma); - HessianFactor(size_t j1, size_t j2, Matrix G11, Matrix G12, Vector g1, Matrix G22, - Vector g2, double f); - HessianFactor(size_t j1, size_t j2, size_t j3, Matrix G11, Matrix G12, Matrix G13, - Vector g1, Matrix G22, Matrix G23, Vector g2, Matrix G33, Vector g3, - double f); - HessianFactor(const gtsam::GaussianFactorGraph &factors); - - //Testable - size_t size() const; - void print(string s) const; - void printKeys(string s) const; - bool equals(const gtsam::GaussianFactor &lf, double tol) const; - double error(const gtsam::VectorValues &c) const; - - //Standard Interface - size_t rows() const; - Matrix information() const; - double constantTerm() const; - Vector linearTerm() const; - - // enabling serialization functionality - void serialize() const; - }; - -#include - class GaussianFactorGraph - { - GaussianFactorGraph(); - GaussianFactorGraph(const gtsam::GaussianBayesNet &bayesNet); - GaussianFactorGraph(const gtsam::GaussianBayesTree &bayesTree); - - // From FactorGraph - void print(string s) const; - bool equals(const gtsam::GaussianFactorGraph &lfgraph, double tol) const; - size_t size() const; - gtsam::GaussianFactor *at(size_t idx) const; - gtsam::KeySet keys() const; - gtsam::KeyVector keyVector() const; - bool exists(size_t idx) const; - - // Building the graph - void push_back(const gtsam::GaussianFactor *factor); - void push_back(const gtsam::GaussianConditional *conditional); - void push_back(const gtsam::GaussianFactorGraph &graph); - void push_back(const gtsam::GaussianBayesNet &bayesNet); - void push_back(const gtsam::GaussianBayesTree &bayesTree); - void add(const gtsam::GaussianFactor &factor); - void add(Vector b); - void add(size_t key1, Matrix A1, Vector b, const gtsam::noiseModel::Diagonal *model); - void add(size_t key1, Matrix A1, size_t key2, Matrix A2, Vector b, - const gtsam::noiseModel::Diagonal *model); - void add(size_t key1, Matrix A1, size_t key2, Matrix A2, size_t key3, Matrix A3, - Vector b, const gtsam::noiseModel::Diagonal *model); - - // error and probability - double error(const gtsam::VectorValues &c) const; - double probPrime(const gtsam::VectorValues &c) const; - - gtsam::GaussianFactorGraph clone() const; - gtsam::GaussianFactorGraph negate() const; - - // Optimizing and linear algebra - gtsam::VectorValues optimize() const; - gtsam::VectorValues optimize(const gtsam::Ordering &ordering) const; - gtsam::VectorValues optimizeGradientSearch() const; - gtsam::VectorValues gradient(const gtsam::VectorValues &x0) const; - gtsam::VectorValues gradientAtZero() const; - - // Elimination and marginals - gtsam::GaussianBayesNet *eliminateSequential(); - gtsam::GaussianBayesNet *eliminateSequential(const gtsam::Ordering &ordering); - gtsam::GaussianBayesTree *eliminateMultifrontal(); - gtsam::GaussianBayesTree *eliminateMultifrontal(const gtsam::Ordering &ordering); - pair eliminatePartialSequential( - const gtsam::Ordering &ordering); - pair eliminatePartialSequential( - const gtsam::KeyVector &keys); - pair eliminatePartialMultifrontal( - const gtsam::Ordering &ordering); - pair eliminatePartialMultifrontal( - const gtsam::KeyVector &keys); - gtsam::GaussianBayesNet *marginalMultifrontalBayesNet(const gtsam::Ordering &ordering); - gtsam::GaussianBayesNet *marginalMultifrontalBayesNet(const gtsam::KeyVector &key_vector); - gtsam::GaussianBayesNet *marginalMultifrontalBayesNet(const gtsam::Ordering &ordering, - const gtsam::Ordering &marginalizedVariableOrdering); - gtsam::GaussianBayesNet *marginalMultifrontalBayesNet(const gtsam::KeyVector &key_vector, - const gtsam::Ordering &marginalizedVariableOrdering); - gtsam::GaussianFactorGraph *marginal(const gtsam::KeyVector &key_vector); - - // Conversion to matrices - Matrix sparseJacobian_() const; - Matrix augmentedJacobian() const; - Matrix augmentedJacobian(const gtsam::Ordering &ordering) const; - pair jacobian() const; - pair jacobian(const gtsam::Ordering &ordering) const; - Matrix augmentedHessian() const; - Matrix augmentedHessian(const gtsam::Ordering &ordering) const; - pair hessian() const; - pair hessian(const gtsam::Ordering &ordering) const; - - // enabling serialization functionality - void serialize() const; - }; - -#include - virtual class GaussianConditional : gtsam::GaussianFactor - { - //Constructors - GaussianConditional(size_t key, Vector d, Matrix R, const gtsam::noiseModel::Diagonal *sigmas); - GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, - const gtsam::noiseModel::Diagonal *sigmas); - GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, - size_t name2, Matrix T, const gtsam::noiseModel::Diagonal *sigmas); - - //Constructors with no noise model - GaussianConditional(size_t key, Vector d, Matrix R); - GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S); - GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, - size_t name2, Matrix T); - - //Standard Interface - void print(string s) const; - bool equals(const gtsam::GaussianConditional &cg, double tol) const; - - //Advanced Interface - gtsam::VectorValues solve(const gtsam::VectorValues &parents) const; - gtsam::VectorValues solveOtherRHS(const gtsam::VectorValues &parents, const gtsam::VectorValues &rhs) const; - void solveTransposeInPlace(gtsam::VectorValues &gy) const; - void scaleFrontalsBySigma(gtsam::VectorValues &gy) const; - Matrix R() const; - Matrix S() const; - Vector d() const; - - // enabling serialization functionality - void serialize() const; - }; - -#include - virtual class GaussianDensity : gtsam::GaussianConditional - { - //Constructors - GaussianDensity(size_t key, Vector d, Matrix R, const gtsam::noiseModel::Diagonal *sigmas); - - //Standard Interface - void print(string s) const; - bool equals(const gtsam::GaussianDensity &cg, double tol) const; - Vector mean() const; - Matrix covariance() const; - }; - -#include - virtual class GaussianBayesNet - { - //Constructors - GaussianBayesNet(); - GaussianBayesNet(const gtsam::GaussianConditional *conditional); - - // Testable - void print(string s) const; - bool equals(const gtsam::GaussianBayesNet &other, double tol) const; - size_t size() const; - - // FactorGraph derived interface - // size_t size() const; - gtsam::GaussianConditional *at(size_t idx) const; - gtsam::KeySet keys() const; - bool exists(size_t idx) const; - - gtsam::GaussianConditional *front() const; - gtsam::GaussianConditional *back() const; - void push_back(gtsam::GaussianConditional *conditional); - void push_back(const gtsam::GaussianBayesNet &bayesNet); - - gtsam::VectorValues optimize() const; - gtsam::VectorValues optimize(gtsam::VectorValues &solutionForMissing) const; - gtsam::VectorValues optimizeGradientSearch() const; - gtsam::VectorValues gradient(const gtsam::VectorValues &x0) const; - gtsam::VectorValues gradientAtZero() const; - double error(const gtsam::VectorValues &x) const; - double determinant() const; - double logDeterminant() const; - gtsam::VectorValues backSubstitute(const gtsam::VectorValues &gx) const; - gtsam::VectorValues backSubstituteTranspose(const gtsam::VectorValues &gx) const; - }; - -#include - virtual class GaussianBayesTree - { - // Standard Constructors and Named Constructors - GaussianBayesTree(); - GaussianBayesTree(const gtsam::GaussianBayesTree &other); - bool equals(const gtsam::GaussianBayesTree &other, double tol) const; - void print(string s); - size_t size() const; - bool empty() const; - size_t numCachedSeparatorMarginals() const; - void saveGraph(string s) const; - - gtsam::VectorValues optimize() const; - gtsam::VectorValues optimizeGradientSearch() const; - gtsam::VectorValues gradient(const gtsam::VectorValues &x0) const; - gtsam::VectorValues gradientAtZero() const; - double error(const gtsam::VectorValues &x) const; - double determinant() const; - double logDeterminant() const; - Matrix marginalCovariance(size_t key) const; - gtsam::GaussianConditional *marginalFactor(size_t key) const; - gtsam::GaussianFactorGraph *joint(size_t key1, size_t key2) const; - gtsam::GaussianBayesNet *jointBayesNet(size_t key1, size_t key2) const; - }; - -#include - class Errors - { - //Constructors - Errors(); - Errors(const gtsam::VectorValues &V); - - //Testable - void print(string s); - bool equals(const gtsam::Errors &expected, double tol) const; - }; - -#include - class GaussianISAM - { - //Constructor - GaussianISAM(); - - //Standard Interface - void update(const gtsam::GaussianFactorGraph &newFactors); - void saveGraph(string s) const; - void clear(); - }; - -#include - virtual class IterativeOptimizationParameters - { - string getVerbosity() const; - void setVerbosity(string s); - void print() const; - }; - - //virtual class IterativeSolver { - // IterativeSolver(); - // gtsam::VectorValues optimize (); - //}; - -#include - virtual class ConjugateGradientParameters : gtsam::IterativeOptimizationParameters - { - ConjugateGradientParameters(); - int getMinIterations() const; - int getMaxIterations() const; - int getReset() const; - double getEpsilon_rel() const; - double getEpsilon_abs() const; - - void setMinIterations(int value); - void setMaxIterations(int value); - void setReset(int value); - void setEpsilon_rel(double value); - void setEpsilon_abs(double value); - void print() const; - }; - -#include - virtual class PreconditionerParameters - { - PreconditionerParameters(); - }; - - virtual class DummyPreconditionerParameters : gtsam::PreconditionerParameters - { - DummyPreconditionerParameters(); - }; - -#include - virtual class PCGSolverParameters : gtsam::ConjugateGradientParameters - { - PCGSolverParameters(); - void print(string s); - void setPreconditionerParams(gtsam::PreconditionerParameters *preconditioner); - }; - -#include - virtual class SubgraphSolverParameters : gtsam::ConjugateGradientParameters - { - SubgraphSolverParameters(); - void print() const; - }; - - virtual class SubgraphSolver - { - SubgraphSolver(const gtsam::GaussianFactorGraph &A, const gtsam::SubgraphSolverParameters ¶meters, const gtsam::Ordering &ordering); - SubgraphSolver(const gtsam::GaussianFactorGraph &Ab1, const gtsam::GaussianFactorGraph *Ab2, const gtsam::SubgraphSolverParameters ¶meters, const gtsam::Ordering &ordering); - gtsam::VectorValues optimize() const; - }; - -#include - class KalmanFilter - { - KalmanFilter(size_t n); - // gtsam::GaussianDensity* init(Vector x0, const gtsam::SharedDiagonal& P0); - gtsam::GaussianDensity *init(Vector x0, Matrix P0); - void print(string s) const; - static size_t step(gtsam::GaussianDensity *p); - gtsam::GaussianDensity *predict(gtsam::GaussianDensity *p, Matrix F, - Matrix B, Vector u, const gtsam::noiseModel::Diagonal *modelQ); - gtsam::GaussianDensity *predictQ(gtsam::GaussianDensity *p, Matrix F, - Matrix B, Vector u, Matrix Q); - gtsam::GaussianDensity *predict2(gtsam::GaussianDensity *p, Matrix A0, - Matrix A1, Vector b, const gtsam::noiseModel::Diagonal *model); - gtsam::GaussianDensity *update(gtsam::GaussianDensity *p, Matrix H, - Vector z, const gtsam::noiseModel::Diagonal *model); - gtsam::GaussianDensity *updateQ(gtsam::GaussianDensity *p, Matrix H, - Vector z, Matrix Q); - }; - - //************************************************************************* - // nonlinear - //************************************************************************* - -#include - size_t symbol(char chr, size_t index); - char symbolChr(size_t key); - size_t symbolIndex(size_t key); - - namespace symbol_shorthand - { - size_t A(size_t j); - size_t B(size_t j); - size_t C(size_t j); - size_t D(size_t j); - size_t E(size_t j); - size_t F(size_t j); - size_t G(size_t j); - size_t H(size_t j); - size_t I(size_t j); - size_t J(size_t j); - size_t K(size_t j); - size_t L(size_t j); - size_t M(size_t j); - size_t N(size_t j); - size_t O(size_t j); - size_t P(size_t j); - size_t Q(size_t j); - size_t R(size_t j); - size_t S(size_t j); - size_t T(size_t j); - size_t U(size_t j); - size_t V(size_t j); - size_t W(size_t j); - size_t X(size_t j); - size_t Y(size_t j); - size_t Z(size_t j); - } // namespace symbol_shorthand - - // Default keyformatter - void PrintKeyList(const gtsam::KeyList &keys); - void PrintKeyList(const gtsam::KeyList &keys, string s); - void PrintKeyVector(const gtsam::KeyVector &keys); - void PrintKeyVector(const gtsam::KeyVector &keys, string s); - void PrintKeySet(const gtsam::KeySet &keys); - void PrintKeySet(const gtsam::KeySet &keys, string s); - -#include - class LabeledSymbol - { - LabeledSymbol(size_t full_key); - LabeledSymbol(const gtsam::LabeledSymbol &key); - LabeledSymbol(unsigned char valType, unsigned char label, size_t j); - - size_t key() const; - unsigned char label() const; - unsigned char chr() const; - size_t index() const; - - gtsam::LabeledSymbol upper() const; - gtsam::LabeledSymbol lower() const; - gtsam::LabeledSymbol newChr(unsigned char c) const; - gtsam::LabeledSymbol newLabel(unsigned char label) const; - - void print(string s) const; - }; - - size_t mrsymbol(unsigned char c, unsigned char label, size_t j); - unsigned char mrsymbolChr(size_t key); - unsigned char mrsymbolLabel(size_t key); - size_t mrsymbolIndex(size_t key); - -#include - class Ordering - { - // Standard Constructors and Named Constructors - Ordering(); - Ordering(const gtsam::Ordering &other); - - // Testable - void print(string s) const; - bool equals(const gtsam::Ordering &ord, double tol) const; - - // Standard interface - size_t size() const; - size_t at(size_t key) const; - void push_back(size_t key); - - // enabling serialization functionality - void serialize() const; - }; - -#include - class NonlinearFactorGraph - { - NonlinearFactorGraph(); - NonlinearFactorGraph(const gtsam::NonlinearFactorGraph &graph); - - // FactorGraph - void print(string s) const; - bool equals(const gtsam::NonlinearFactorGraph &fg, double tol) const; - size_t size() const; - bool empty() const; - void remove(size_t i); - void replace(size_t i, gtsam::NonlinearFactor *factors); - void resize(size_t size); - size_t nrFactors() const; - gtsam::NonlinearFactor *at(size_t idx) const; - void push_back(const gtsam::NonlinearFactorGraph &factors); - void push_back(gtsam::NonlinearFactor *factor); - void add(gtsam::NonlinearFactor *factor); - bool exists(size_t idx) const; - gtsam::KeySet keys() const; - gtsam::KeyVector keyVector() const; - - template , gtsam::imuBias::ConstantBias}> - void addPrior(size_t key, const T &prior, const gtsam::noiseModel::Base *noiseModel); - - // NonlinearFactorGraph - void printErrors(const gtsam::Values &values) const; - double error(const gtsam::Values &values) const; - double probPrime(const gtsam::Values &values) const; - gtsam::Ordering orderingCOLAMD() const; - // Ordering* orderingCOLAMDConstrained(const gtsam::Values& c, const std::map& constraints) const; - gtsam::GaussianFactorGraph *linearize(const gtsam::Values &values) const; - gtsam::NonlinearFactorGraph clone() const; - - // enabling serialization functionality - void serialize() const; - }; - -#include - virtual class NonlinearFactor - { - // Factor base class - size_t size() const; - gtsam::KeyVector keys() const; - void print(string s) const; - void printKeys(string s) const; - // NonlinearFactor - bool equals(const gtsam::NonlinearFactor &other, double tol) const; - double error(const gtsam::Values &c) const; - size_t dim() const; - bool active(const gtsam::Values &c) const; - gtsam::GaussianFactor *linearize(const gtsam::Values &c) const; - gtsam::NonlinearFactor *clone() const; - // gtsam::NonlinearFactor* rekey(const gtsam::KeyVector& newKeys) const; //TODO: Conversion from KeyVector to std::vector does not happen - }; - -#include - virtual class NoiseModelFactor : gtsam::NonlinearFactor - { - bool equals(const gtsam::NoiseModelFactor &other, double tol) const; - gtsam::noiseModel::Base *noiseModel() const; - Vector unwhitenedError(const gtsam::Values &x) const; - Vector whitenedError(const gtsam::Values &x) const; - }; - -#include - class Values - { - Values(); - Values(const gtsam::Values &other); - - size_t size() const; - bool empty() const; - void clear(); - size_t dim() const; - - void print(string s) const; - bool equals(const gtsam::Values &other, double tol) const; - - void insert(const gtsam::Values &values); - void update(const gtsam::Values &values); - void erase(size_t j); - void swap(gtsam::Values &values); - - bool exists(size_t j) const; - gtsam::KeyVector keys() const; - - gtsam::VectorValues zeroVectors() const; - - gtsam::Values retract(const gtsam::VectorValues &delta) const; - gtsam::VectorValues localCoordinates(const gtsam::Values &cp) const; - - // enabling serialization functionality - void serialize() const; - - // New in 4.0, we have to specialize every insert/update/at to generate wrappers - // Instead of the old: - // void insert(size_t j, const gtsam::Value& value); - // void update(size_t j, const gtsam::Value& val); - // gtsam::Value at(size_t j) const; - - // The order is important: Vector has to precede Point2/Point3 so `atVector` - // can work for those fixed-size vectors. - void insert(size_t j, Vector vector); - void insert(size_t j, Matrix matrix); - void insert(size_t j, const gtsam::Point2 &point2); - void insert(size_t j, const gtsam::Point3 &point3); - void insert(size_t j, const gtsam::Rot2 &rot2); - void insert(size_t j, const gtsam::Pose2 &pose2); - void insert(size_t j, const gtsam::SO3 &R); - void insert(size_t j, const gtsam::SO4 &Q); - void insert(size_t j, const gtsam::SOn &P); - void insert(size_t j, const gtsam::Rot3 &rot3); - void insert(size_t j, const gtsam::Pose3 &pose3); - void insert(size_t j, const gtsam::Unit3 &unit3); - void insert(size_t j, const gtsam::Cal3_S2 &cal3_s2); - void insert(size_t j, const gtsam::Cal3DS2 &cal3ds2); - void insert(size_t j, const gtsam::Cal3Bundler &cal3bundler); - void insert(size_t j, const gtsam::EssentialMatrix &essential_matrix); - void insert(size_t j, const gtsam::PinholeCameraCal3_S2 &simple_camera); - void insert(size_t j, const gtsam::PinholeCamera &camera); - void insert(size_t j, const gtsam::imuBias::ConstantBias &constant_bias); - void insert(size_t j, const gtsam::NavState &nav_state); - - void update(size_t j, const gtsam::Point2 &point2); - void update(size_t j, const gtsam::Point3 &point3); - void update(size_t j, const gtsam::Rot2 &rot2); - void update(size_t j, const gtsam::Pose2 &pose2); - void update(size_t j, const gtsam::SO3 &R); - void update(size_t j, const gtsam::SO4 &Q); - void update(size_t j, const gtsam::SOn &P); - void update(size_t j, const gtsam::Rot3 &rot3); - void update(size_t j, const gtsam::Pose3 &pose3); - void update(size_t j, const gtsam::Unit3 &unit3); - void update(size_t j, const gtsam::Cal3_S2 &cal3_s2); - void update(size_t j, const gtsam::Cal3DS2 &cal3ds2); - void update(size_t j, const gtsam::Cal3Bundler &cal3bundler); - void update(size_t j, const gtsam::EssentialMatrix &essential_matrix); - void update(size_t j, const gtsam::PinholeCameraCal3_S2 &simple_camera); - void update(size_t j, const gtsam::PinholeCamera &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); - void update(size_t j, Matrix matrix); - - template , gtsam::imuBias::ConstantBias, gtsam::NavState, Vector, Matrix}> - T at(size_t j); - - /// version for double - void insertDouble(size_t j, double c); - double atDouble(size_t j) const; - }; - -#include - class Marginals - { - Marginals(const gtsam::NonlinearFactorGraph &graph, - const gtsam::Values &solution); - Marginals(const gtsam::GaussianFactorGraph &gfgraph, - const gtsam::Values &solution); - Marginals(const gtsam::GaussianFactorGraph &gfgraph, - const gtsam::VectorValues &solutionvec); - - void print(string s) const; - Matrix marginalCovariance(size_t variable) const; - Matrix marginalInformation(size_t variable) const; - gtsam::JointMarginal jointMarginalCovariance(const gtsam::KeyVector &variables) const; - gtsam::JointMarginal jointMarginalInformation(const gtsam::KeyVector &variables) const; - }; - - class JointMarginal - { - Matrix at(size_t iVariable, size_t jVariable) const; - Matrix fullMatrix() const; - void print(string s) const; - void print() const; - }; - -#include - virtual class LinearContainerFactor : gtsam::NonlinearFactor - { - - LinearContainerFactor(gtsam::GaussianFactor *factor, const gtsam::Values &linearizationPoint); - LinearContainerFactor(gtsam::GaussianFactor *factor); - - gtsam::GaussianFactor *factor() const; - // const boost::optional& linearizationPoint() const; - - bool isJacobian() const; - gtsam::JacobianFactor *toJacobian() const; - gtsam::HessianFactor *toHessian() const; - - static gtsam::NonlinearFactorGraph ConvertLinearGraph(const gtsam::GaussianFactorGraph &linear_graph, - const gtsam::Values &linearizationPoint); - - static gtsam::NonlinearFactorGraph ConvertLinearGraph(const gtsam::GaussianFactorGraph &linear_graph); +virtual class Base { + void print(string s) const; + // Methods below are available for all noise models. However, can't add them + // because wrap (incorrectly) thinks robust classes derive from this Base as well. + // bool isConstrained() const; + // bool isUnit() const; + // size_t dim() const; + // Vector sigmas() const; +}; + +virtual class Gaussian : gtsam::noiseModel::Base { + static gtsam::noiseModel::Gaussian* Information(Matrix R); + static gtsam::noiseModel::Gaussian* SqrtInformation(Matrix R); + static gtsam::noiseModel::Gaussian* Covariance(Matrix R); + + bool equals(gtsam::noiseModel::Base& expected, double tol); + + // access to noise model + Matrix R() const; + Matrix information() const; + Matrix covariance() const; + + // Whitening operations + Vector whiten(Vector v) const; + Vector unwhiten(Vector v) const; + Matrix Whiten(Matrix H) const; + + // enabling serialization functionality + void serializable() const; +}; + +virtual class Diagonal : gtsam::noiseModel::Gaussian { + static gtsam::noiseModel::Diagonal* Sigmas(Vector sigmas); + static gtsam::noiseModel::Diagonal* Variances(Vector variances); + static gtsam::noiseModel::Diagonal* Precisions(Vector precisions); + Matrix R() const; + + // access to noise model + Vector sigmas() const; + Vector invsigmas() const; + Vector precisions() const; + + // enabling serialization functionality + void serializable() const; +}; + +virtual class Constrained : gtsam::noiseModel::Diagonal { + static gtsam::noiseModel::Constrained* MixedSigmas(Vector mu, Vector sigmas); + static gtsam::noiseModel::Constrained* MixedSigmas(double m, Vector sigmas); + static gtsam::noiseModel::Constrained* MixedVariances(Vector mu, Vector variances); + static gtsam::noiseModel::Constrained* MixedVariances(Vector variances); + static gtsam::noiseModel::Constrained* MixedPrecisions(Vector mu, Vector precisions); + static gtsam::noiseModel::Constrained* MixedPrecisions(Vector precisions); + + static gtsam::noiseModel::Constrained* All(size_t dim); + static gtsam::noiseModel::Constrained* All(size_t dim, double mu); + + gtsam::noiseModel::Constrained* unit() const; // enabling serialization functionality void serializable() const; - }; // \class LinearContainerFactor +}; + +virtual class Isotropic : gtsam::noiseModel::Diagonal { + static gtsam::noiseModel::Isotropic* Sigma(size_t dim, double sigma); + static gtsam::noiseModel::Isotropic* Variance(size_t dim, double varianace); + static gtsam::noiseModel::Isotropic* Precision(size_t dim, double precision); + + // access to noise model + double sigma() const; + + // enabling serialization functionality + void serializable() const; +}; + +virtual class Unit : gtsam::noiseModel::Isotropic { + static gtsam::noiseModel::Unit* Create(size_t dim); + + // enabling serialization functionality + void serializable() const; +}; + +namespace mEstimator { +virtual class Base { + void print(string s) const; +}; + +virtual class Null: gtsam::noiseModel::mEstimator::Base { + Null(); + static gtsam::noiseModel::mEstimator::Null* Create(); + + // enabling serialization functionality + void serializable() const; + + double weight(double error) const; + double loss(double error) const; +}; + +virtual class Fair: gtsam::noiseModel::mEstimator::Base { + Fair(double c); + static gtsam::noiseModel::mEstimator::Fair* Create(double c); + + // enabling serialization functionality + void serializable() const; + + double weight(double error) const; + double loss(double error) const; +}; + +virtual class Huber: gtsam::noiseModel::mEstimator::Base { + Huber(double k); + static gtsam::noiseModel::mEstimator::Huber* Create(double k); + + // enabling serialization functionality + void serializable() const; + + double weight(double error) const; + double loss(double error) const; +}; + +virtual class Cauchy: gtsam::noiseModel::mEstimator::Base { + Cauchy(double k); + static gtsam::noiseModel::mEstimator::Cauchy* Create(double k); + + // enabling serialization functionality + void serializable() const; + + double weight(double error) const; + double loss(double error) const; +}; + +virtual class Tukey: gtsam::noiseModel::mEstimator::Base { + Tukey(double k); + static gtsam::noiseModel::mEstimator::Tukey* Create(double k); + + // enabling serialization functionality + void serializable() const; + + double weight(double error) const; + double loss(double error) const; +}; + +virtual class Welsch: gtsam::noiseModel::mEstimator::Base { + Welsch(double k); + static gtsam::noiseModel::mEstimator::Welsch* Create(double k); + + // enabling serialization functionality + void serializable() const; + + double weight(double error) const; + double loss(double error) const; +}; + +virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base { + GemanMcClure(double c); + static gtsam::noiseModel::mEstimator::GemanMcClure* Create(double c); + + // enabling serialization functionality + void serializable() const; + + double weight(double error) const; + double loss(double error) const; +}; + +virtual class DCS: gtsam::noiseModel::mEstimator::Base { + DCS(double c); + static gtsam::noiseModel::mEstimator::DCS* Create(double c); + + // enabling serialization functionality + void serializable() const; + + double weight(double error) const; + double loss(double error) const; +}; + +virtual class L2WithDeadZone: gtsam::noiseModel::mEstimator::Base { + L2WithDeadZone(double k); + static gtsam::noiseModel::mEstimator::L2WithDeadZone* Create(double k); + + // enabling serialization functionality + void serializable() const; + + double weight(double error) const; + double loss(double error) const; +}; + +}///\namespace mEstimator + +virtual class Robust : gtsam::noiseModel::Base { + Robust(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise); + static gtsam::noiseModel::Robust* Create(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise); + + // enabling serialization functionality + void serializable() const; +}; + +}///\namespace noiseModel + +#include +class Sampler { + // Constructors + Sampler(gtsam::noiseModel::Diagonal* model, int seed); + Sampler(Vector sigmas, int seed); + + // Standard Interface + size_t dim() const; + Vector sigmas() const; + gtsam::noiseModel::Diagonal* model() const; + Vector sample(); +}; + +#include +class VectorValues { + //Constructors + VectorValues(); + VectorValues(const gtsam::VectorValues& other); + + //Named Constructors + static gtsam::VectorValues Zero(const gtsam::VectorValues& model); + + //Standard Interface + size_t size() const; + size_t dim(size_t j) const; + bool exists(size_t j) const; + void print(string s) const; + bool equals(const gtsam::VectorValues& expected, double tol) const; + void insert(size_t j, Vector value); + Vector vector() const; + Vector at(size_t j) const; + void update(const gtsam::VectorValues& values); + + //Advanced Interface + void setZero(); + + gtsam::VectorValues add(const gtsam::VectorValues& c) const; + void addInPlace(const gtsam::VectorValues& c); + gtsam::VectorValues subtract(const gtsam::VectorValues& c) const; + gtsam::VectorValues scale(double a) const; + void scaleInPlace(double a); + + bool hasSameStructure(const gtsam::VectorValues& other) const; + double dot(const gtsam::VectorValues& V) const; + double norm() const; + double squaredNorm() const; + + // enabling serialization functionality + void serialize() const; +}; + +#include +virtual class GaussianFactor { + gtsam::KeyVector keys() const; + void print(string s) const; + bool equals(const gtsam::GaussianFactor& lf, double tol) const; + double error(const gtsam::VectorValues& c) const; + gtsam::GaussianFactor* clone() const; + gtsam::GaussianFactor* negate() const; + Matrix augmentedInformation() const; + Matrix information() const; + Matrix augmentedJacobian() const; + pair jacobian() const; + size_t size() const; + bool empty() const; +}; + +#include +virtual class JacobianFactor : gtsam::GaussianFactor { + //Constructors + JacobianFactor(); + JacobianFactor(const gtsam::GaussianFactor& factor); + JacobianFactor(Vector b_in); + JacobianFactor(size_t i1, Matrix A1, Vector b, + const gtsam::noiseModel::Diagonal* model); + JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, Vector b, + const gtsam::noiseModel::Diagonal* model); + JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, size_t i3, Matrix A3, + Vector b, const gtsam::noiseModel::Diagonal* model); + JacobianFactor(const gtsam::GaussianFactorGraph& graph); + + //Testable + void print(string s) const; + void printKeys(string s) const; + bool equals(const gtsam::GaussianFactor& lf, double tol) const; + size_t size() const; + Vector unweighted_error(const gtsam::VectorValues& c) const; + Vector error_vector(const gtsam::VectorValues& c) const; + double error(const gtsam::VectorValues& c) const; + + //Standard Interface + Matrix getA() const; + Vector getb() const; + size_t rows() const; + size_t cols() const; + bool isConstrained() const; + pair jacobianUnweighted() const; + Matrix augmentedJacobianUnweighted() const; + + void transposeMultiplyAdd(double alpha, Vector e, gtsam::VectorValues& x) const; + gtsam::JacobianFactor whiten() const; + + pair eliminate(const gtsam::Ordering& keys) const; + + void setModel(bool anyConstrained, Vector sigmas); + + gtsam::noiseModel::Diagonal* get_model() const; + + // enabling serialization functionality + void serialize() const; +}; + +#include +virtual class HessianFactor : gtsam::GaussianFactor { + //Constructors + HessianFactor(); + HessianFactor(const gtsam::GaussianFactor& factor); + HessianFactor(size_t j, Matrix G, Vector g, double f); + HessianFactor(size_t j, Vector mu, Matrix Sigma); + HessianFactor(size_t j1, size_t j2, Matrix G11, Matrix G12, Vector g1, Matrix G22, + Vector g2, double f); + HessianFactor(size_t j1, size_t j2, size_t j3, Matrix G11, Matrix G12, Matrix G13, + Vector g1, Matrix G22, Matrix G23, Vector g2, Matrix G33, Vector g3, + double f); + HessianFactor(const gtsam::GaussianFactorGraph& factors); + + //Testable + size_t size() const; + void print(string s) const; + void printKeys(string s) const; + bool equals(const gtsam::GaussianFactor& lf, double tol) const; + double error(const gtsam::VectorValues& c) const; + + //Standard Interface + size_t rows() const; + Matrix information() const; + double constantTerm() const; + Vector linearTerm() const; + + // enabling serialization functionality + void serialize() const; +}; + +#include +class GaussianFactorGraph { + GaussianFactorGraph(); + GaussianFactorGraph(const gtsam::GaussianBayesNet& bayesNet); + GaussianFactorGraph(const gtsam::GaussianBayesTree& bayesTree); + + // From FactorGraph + void print(string s) const; + bool equals(const gtsam::GaussianFactorGraph& lfgraph, double tol) const; + size_t size() const; + gtsam::GaussianFactor* at(size_t idx) const; + gtsam::KeySet keys() const; + gtsam::KeyVector keyVector() const; + bool exists(size_t idx) const; + + // Building the graph + void push_back(const gtsam::GaussianFactor* factor); + void push_back(const gtsam::GaussianConditional* conditional); + void push_back(const gtsam::GaussianFactorGraph& graph); + void push_back(const gtsam::GaussianBayesNet& bayesNet); + void push_back(const gtsam::GaussianBayesTree& bayesTree); + void add(const gtsam::GaussianFactor& factor); + void add(Vector b); + void add(size_t key1, Matrix A1, Vector b, const gtsam::noiseModel::Diagonal* model); + void add(size_t key1, Matrix A1, size_t key2, Matrix A2, Vector b, + const gtsam::noiseModel::Diagonal* model); + void add(size_t key1, Matrix A1, size_t key2, Matrix A2, size_t key3, Matrix A3, + Vector b, const gtsam::noiseModel::Diagonal* model); + + // error and probability + double error(const gtsam::VectorValues& c) const; + double probPrime(const gtsam::VectorValues& c) const; + + gtsam::GaussianFactorGraph clone() const; + gtsam::GaussianFactorGraph negate() const; + + // Optimizing and linear algebra + gtsam::VectorValues optimize() const; + gtsam::VectorValues optimize(const gtsam::Ordering& ordering) const; + gtsam::VectorValues optimizeGradientSearch() const; + gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const; + gtsam::VectorValues gradientAtZero() const; + + // Elimination and marginals + gtsam::GaussianBayesNet* eliminateSequential(); + gtsam::GaussianBayesNet* eliminateSequential(const gtsam::Ordering& ordering); + gtsam::GaussianBayesTree* eliminateMultifrontal(); + gtsam::GaussianBayesTree* eliminateMultifrontal(const gtsam::Ordering& ordering); + pair eliminatePartialSequential( + const gtsam::Ordering& ordering); + pair eliminatePartialSequential( + const gtsam::KeyVector& keys); + pair eliminatePartialMultifrontal( + const gtsam::Ordering& ordering); + pair eliminatePartialMultifrontal( + const gtsam::KeyVector& keys); + gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& ordering); + gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& key_vector); + gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& ordering, + const gtsam::Ordering& marginalizedVariableOrdering); + gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& key_vector, + const gtsam::Ordering& marginalizedVariableOrdering); + gtsam::GaussianFactorGraph* marginal(const gtsam::KeyVector& key_vector); + + // Conversion to matrices + Matrix sparseJacobian_() const; + Matrix augmentedJacobian() const; + Matrix augmentedJacobian(const gtsam::Ordering& ordering) const; + pair jacobian() const; + pair jacobian(const gtsam::Ordering& ordering) const; + Matrix augmentedHessian() const; + Matrix augmentedHessian(const gtsam::Ordering& ordering) const; + pair hessian() const; + pair hessian(const gtsam::Ordering& ordering) const; + + // enabling serialization functionality + void serialize() const; +}; + +#include +virtual class GaussianConditional : gtsam::GaussianFactor { + //Constructors + GaussianConditional(size_t key, Vector d, Matrix R, const gtsam::noiseModel::Diagonal* sigmas); + GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, + const gtsam::noiseModel::Diagonal* sigmas); + GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, + size_t name2, Matrix T, const gtsam::noiseModel::Diagonal* sigmas); + + //Constructors with no noise model + GaussianConditional(size_t key, Vector d, Matrix R); + GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S); + GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, + size_t name2, Matrix T); + + //Standard Interface + void print(string s) const; + bool equals(const gtsam::GaussianConditional &cg, double tol) const; + + //Advanced Interface + gtsam::VectorValues solve(const gtsam::VectorValues& parents) const; + gtsam::VectorValues solveOtherRHS(const gtsam::VectorValues& parents, const gtsam::VectorValues& rhs) const; + void solveTransposeInPlace(gtsam::VectorValues& gy) const; + void scaleFrontalsBySigma(gtsam::VectorValues& gy) const; + Matrix R() const; + Matrix S() const; + Vector d() const; + + // enabling serialization functionality + void serialize() const; +}; + +#include +virtual class GaussianDensity : gtsam::GaussianConditional { + //Constructors + GaussianDensity(size_t key, Vector d, Matrix R, const gtsam::noiseModel::Diagonal* sigmas); + + //Standard Interface + void print(string s) const; + bool equals(const gtsam::GaussianDensity &cg, double tol) const; + Vector mean() const; + Matrix covariance() const; +}; + +#include +virtual class GaussianBayesNet { + //Constructors + GaussianBayesNet(); + GaussianBayesNet(const gtsam::GaussianConditional* conditional); + + // Testable + void print(string s) const; + bool equals(const gtsam::GaussianBayesNet& other, double tol) const; + size_t size() const; + + // FactorGraph derived interface + // size_t size() const; + gtsam::GaussianConditional* at(size_t idx) const; + gtsam::KeySet keys() const; + bool exists(size_t idx) const; + + gtsam::GaussianConditional* front() const; + gtsam::GaussianConditional* back() const; + void push_back(gtsam::GaussianConditional* conditional); + void push_back(const gtsam::GaussianBayesNet& bayesNet); + + gtsam::VectorValues optimize() const; + gtsam::VectorValues optimize(gtsam::VectorValues& solutionForMissing) const; + gtsam::VectorValues optimizeGradientSearch() const; + gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const; + gtsam::VectorValues gradientAtZero() const; + double error(const gtsam::VectorValues& x) const; + double determinant() const; + double logDeterminant() const; + gtsam::VectorValues backSubstitute(const gtsam::VectorValues& gx) const; + gtsam::VectorValues backSubstituteTranspose(const gtsam::VectorValues& gx) const; +}; + +#include +virtual class GaussianBayesTree { + // Standard Constructors and Named Constructors + GaussianBayesTree(); + GaussianBayesTree(const gtsam::GaussianBayesTree& other); + bool equals(const gtsam::GaussianBayesTree& other, double tol) const; + void print(string s); + size_t size() const; + bool empty() const; + size_t numCachedSeparatorMarginals() const; + void saveGraph(string s) const; + + gtsam::VectorValues optimize() const; + gtsam::VectorValues optimizeGradientSearch() const; + gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const; + gtsam::VectorValues gradientAtZero() const; + double error(const gtsam::VectorValues& x) const; + double determinant() const; + double logDeterminant() const; + Matrix marginalCovariance(size_t key) const; + gtsam::GaussianConditional* marginalFactor(size_t key) const; + gtsam::GaussianFactorGraph* joint(size_t key1, size_t key2) const; + gtsam::GaussianBayesNet* jointBayesNet(size_t key1, size_t key2) const; +}; + +#include +class Errors { + //Constructors + Errors(); + Errors(const gtsam::VectorValues& V); + + //Testable + void print(string s); + bool equals(const gtsam::Errors& expected, double tol) const; +}; + +#include +class GaussianISAM { + //Constructor + GaussianISAM(); + + //Standard Interface + void update(const gtsam::GaussianFactorGraph& newFactors); + void saveGraph(string s) const; + void clear(); +}; + +#include +virtual class IterativeOptimizationParameters { + string getVerbosity() const; + void setVerbosity(string s) ; + void print() const; +}; + +//virtual class IterativeSolver { +// IterativeSolver(); +// gtsam::VectorValues optimize (); +//}; + +#include +virtual class ConjugateGradientParameters : gtsam::IterativeOptimizationParameters { + ConjugateGradientParameters(); + int getMinIterations() const ; + int getMaxIterations() const ; + int getReset() const; + double getEpsilon_rel() const; + double getEpsilon_abs() const; + + void setMinIterations(int value); + void setMaxIterations(int value); + void setReset(int value); + void setEpsilon_rel(double value); + void setEpsilon_abs(double value); + void print() const; +}; + +#include +virtual class PreconditionerParameters { + PreconditionerParameters(); +}; + +virtual class DummyPreconditionerParameters : gtsam::PreconditionerParameters { + DummyPreconditionerParameters(); +}; + +#include +virtual class PCGSolverParameters : gtsam::ConjugateGradientParameters { + PCGSolverParameters(); + void print(string s); + void setPreconditionerParams(gtsam::PreconditionerParameters* preconditioner); +}; + +#include +virtual class SubgraphSolverParameters : gtsam::ConjugateGradientParameters { + SubgraphSolverParameters(); + void print() const; +}; + +virtual class SubgraphSolver { + SubgraphSolver(const gtsam::GaussianFactorGraph &A, const gtsam::SubgraphSolverParameters ¶meters, const gtsam::Ordering& ordering); + SubgraphSolver(const gtsam::GaussianFactorGraph &Ab1, const gtsam::GaussianFactorGraph* Ab2, const gtsam::SubgraphSolverParameters ¶meters, const gtsam::Ordering& ordering); + gtsam::VectorValues optimize() const; +}; + +#include +class KalmanFilter { + KalmanFilter(size_t n); + // gtsam::GaussianDensity* init(Vector x0, const gtsam::SharedDiagonal& P0); + gtsam::GaussianDensity* init(Vector x0, Matrix P0); + void print(string s) const; + static size_t step(gtsam::GaussianDensity* p); + gtsam::GaussianDensity* predict(gtsam::GaussianDensity* p, Matrix F, + Matrix B, Vector u, const gtsam::noiseModel::Diagonal* modelQ); + gtsam::GaussianDensity* predictQ(gtsam::GaussianDensity* p, Matrix F, + Matrix B, Vector u, Matrix Q); + gtsam::GaussianDensity* predict2(gtsam::GaussianDensity* p, Matrix A0, + Matrix A1, Vector b, const gtsam::noiseModel::Diagonal* model); + gtsam::GaussianDensity* update(gtsam::GaussianDensity* p, Matrix H, + Vector z, const gtsam::noiseModel::Diagonal* model); + gtsam::GaussianDensity* updateQ(gtsam::GaussianDensity* p, Matrix H, + Vector z, Matrix Q); +}; + +//************************************************************************* +// nonlinear +//************************************************************************* + +#include +size_t symbol(char chr, size_t index); +char symbolChr(size_t key); +size_t symbolIndex(size_t key); + +namespace symbol_shorthand { + size_t A(size_t j); + size_t B(size_t j); + size_t C(size_t j); + size_t D(size_t j); + size_t E(size_t j); + size_t F(size_t j); + size_t G(size_t j); + size_t H(size_t j); + size_t I(size_t j); + size_t J(size_t j); + size_t K(size_t j); + size_t L(size_t j); + size_t M(size_t j); + size_t N(size_t j); + size_t O(size_t j); + size_t P(size_t j); + size_t Q(size_t j); + size_t R(size_t j); + size_t S(size_t j); + size_t T(size_t j); + size_t U(size_t j); + size_t V(size_t j); + size_t W(size_t j); + size_t X(size_t j); + size_t Y(size_t j); + size_t Z(size_t j); +}///\namespace symbol + +// Default keyformatter +void PrintKeyList (const gtsam::KeyList& keys); +void PrintKeyList (const gtsam::KeyList& keys, string s); +void PrintKeyVector(const gtsam::KeyVector& keys); +void PrintKeyVector(const gtsam::KeyVector& keys, string s); +void PrintKeySet (const gtsam::KeySet& keys); +void PrintKeySet (const gtsam::KeySet& keys, string s); + +#include +class LabeledSymbol { + LabeledSymbol(size_t full_key); + LabeledSymbol(const gtsam::LabeledSymbol& key); + LabeledSymbol(unsigned char valType, unsigned char label, size_t j); + + size_t key() const; + unsigned char label() const; + unsigned char chr() const; + size_t index() const; + + gtsam::LabeledSymbol upper() const; + gtsam::LabeledSymbol lower() const; + gtsam::LabeledSymbol newChr(unsigned char c) const; + gtsam::LabeledSymbol newLabel(unsigned char label) const; + + void print(string s) const; +}; + +size_t mrsymbol(unsigned char c, unsigned char label, size_t j); +unsigned char mrsymbolChr(size_t key); +unsigned char mrsymbolLabel(size_t key); +size_t mrsymbolIndex(size_t key); + +#include +class Ordering { + // Standard Constructors and Named Constructors + Ordering(); + Ordering(const gtsam::Ordering& other); + + // Testable + void print(string s) const; + bool equals(const gtsam::Ordering& ord, double tol) const; + + // Standard interface + size_t size() const; + size_t at(size_t key) const; + void push_back(size_t key); + + // enabling serialization functionality + void serialize() const; +}; + +#include +class NonlinearFactorGraph { + NonlinearFactorGraph(); + NonlinearFactorGraph(const gtsam::NonlinearFactorGraph& graph); + + // FactorGraph + void print(string s) const; + bool equals(const gtsam::NonlinearFactorGraph& fg, double tol) const; + size_t size() const; + bool empty() const; + void remove(size_t i); + void replace(size_t i, gtsam::NonlinearFactor* factors); + void resize(size_t size); + size_t nrFactors() const; + gtsam::NonlinearFactor* at(size_t idx) const; + void push_back(const gtsam::NonlinearFactorGraph& factors); + void push_back(gtsam::NonlinearFactor* factor); + void add(gtsam::NonlinearFactor* factor); + bool exists(size_t idx) const; + gtsam::KeySet keys() const; + gtsam::KeyVector keyVector() const; + + template, gtsam::imuBias::ConstantBias}> + void addPrior(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); + + // NonlinearFactorGraph + void printErrors(const gtsam::Values& values) const; + double error(const gtsam::Values& values) const; + double probPrime(const gtsam::Values& values) const; + gtsam::Ordering orderingCOLAMD() const; + // Ordering* orderingCOLAMDConstrained(const gtsam::Values& c, const std::map& constraints) const; + gtsam::GaussianFactorGraph* linearize(const gtsam::Values& values) const; + gtsam::NonlinearFactorGraph clone() const; + + // enabling serialization functionality + void serialize() const; +}; + +#include +virtual class NonlinearFactor { + // Factor base class + size_t size() const; + gtsam::KeyVector keys() const; + void print(string s) const; + void printKeys(string s) const; + // NonlinearFactor + bool equals(const gtsam::NonlinearFactor& other, double tol) const; + double error(const gtsam::Values& c) const; + size_t dim() const; + bool active(const gtsam::Values& c) const; + gtsam::GaussianFactor* linearize(const gtsam::Values& c) const; + gtsam::NonlinearFactor* clone() const; + // gtsam::NonlinearFactor* rekey(const gtsam::KeyVector& newKeys) const; //TODO: Conversion from KeyVector to std::vector does not happen +}; + +#include +virtual class NoiseModelFactor: gtsam::NonlinearFactor { + bool equals(const gtsam::NoiseModelFactor& other, double tol) const; + gtsam::noiseModel::Base* noiseModel() const; + Vector unwhitenedError(const gtsam::Values& x) const; + Vector whitenedError(const gtsam::Values& x) const; +}; + +#include +class Values { + Values(); + Values(const gtsam::Values& other); + + size_t size() const; + bool empty() const; + void clear(); + size_t dim() const; + + void print(string s) const; + bool equals(const gtsam::Values& other, double tol) const; + + void insert(const gtsam::Values& values); + void update(const gtsam::Values& values); + void erase(size_t j); + void swap(gtsam::Values& values); + + bool exists(size_t j) const; + gtsam::KeyVector keys() const; + + gtsam::VectorValues zeroVectors() const; + + gtsam::Values retract(const gtsam::VectorValues& delta) const; + gtsam::VectorValues localCoordinates(const gtsam::Values& cp) const; + + // enabling serialization functionality + void serialize() const; + + // New in 4.0, we have to specialize every insert/update/at to generate wrappers + // Instead of the old: + // void insert(size_t j, const gtsam::Value& value); + // void update(size_t j, const gtsam::Value& val); + // gtsam::Value at(size_t j) const; + + // The order is important: Vector has to precede Point2/Point3 so `atVector` + // can work for those fixed-size vectors. + void insert(size_t j, Vector vector); + void insert(size_t j, Matrix matrix); + void insert(size_t j, const gtsam::Point2& point2); + void insert(size_t j, const gtsam::Point3& point3); + void insert(size_t j, const gtsam::Rot2& rot2); + void insert(size_t j, const gtsam::Pose2& pose2); + void insert(size_t j, const gtsam::SO3& R); + void insert(size_t j, const gtsam::SO4& Q); + void insert(size_t j, const gtsam::SOn& P); + void insert(size_t j, const gtsam::Rot3& rot3); + void insert(size_t j, const gtsam::Pose3& pose3); + void insert(size_t j, const gtsam::Unit3& unit3); + void insert(size_t j, const gtsam::Cal3_S2& cal3_s2); + void insert(size_t j, const gtsam::Cal3DS2& cal3ds2); + void insert(size_t j, const gtsam::Cal3Bundler& cal3bundler); + void insert(size_t j, const gtsam::EssentialMatrix& essential_matrix); + void insert(size_t j, const gtsam::PinholeCameraCal3_S2& simple_camera); + void insert(size_t j, const gtsam::PinholeCamera& camera); + void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); + void insert(size_t j, const gtsam::NavState& nav_state); + + void update(size_t j, const gtsam::Point2& point2); + void update(size_t j, const gtsam::Point3& point3); + void update(size_t j, const gtsam::Rot2& rot2); + void update(size_t j, const gtsam::Pose2& pose2); + void update(size_t j, const gtsam::SO3& R); + void update(size_t j, const gtsam::SO4& Q); + void update(size_t j, const gtsam::SOn& P); + void update(size_t j, const gtsam::Rot3& rot3); + void update(size_t j, const gtsam::Pose3& pose3); + void update(size_t j, const gtsam::Unit3& unit3); + void update(size_t j, const gtsam::Cal3_S2& cal3_s2); + void update(size_t j, const gtsam::Cal3DS2& cal3ds2); + void update(size_t j, const gtsam::Cal3Bundler& cal3bundler); + void update(size_t j, const gtsam::EssentialMatrix& essential_matrix); + void update(size_t j, const gtsam::PinholeCameraCal3_S2& simple_camera); + void update(size_t j, const gtsam::PinholeCamera& 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); + void update(size_t j, Matrix matrix); + + template, gtsam::imuBias::ConstantBias, gtsam::NavState, Vector, Matrix}> + T at(size_t j); + + /// version for double + void insertDouble(size_t j, double c); + double atDouble(size_t j) const; +}; + +#include +class Marginals { + Marginals(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& solution); + Marginals(const gtsam::GaussianFactorGraph& gfgraph, + const gtsam::Values& solution); + Marginals(const gtsam::GaussianFactorGraph& gfgraph, + const gtsam::VectorValues& solutionvec); + + void print(string s) const; + Matrix marginalCovariance(size_t variable) const; + Matrix marginalInformation(size_t variable) const; + gtsam::JointMarginal jointMarginalCovariance(const gtsam::KeyVector& variables) const; + gtsam::JointMarginal jointMarginalInformation(const gtsam::KeyVector& variables) const; +}; + +class JointMarginal { + Matrix at(size_t iVariable, size_t jVariable) const; + Matrix fullMatrix() const; + void print(string s) const; + void print() const; +}; + +#include +virtual class LinearContainerFactor : gtsam::NonlinearFactor { + + LinearContainerFactor(gtsam::GaussianFactor* factor, const gtsam::Values& linearizationPoint); + LinearContainerFactor(gtsam::GaussianFactor* factor); + + gtsam::GaussianFactor* factor() const; +// const boost::optional& linearizationPoint() const; + + bool isJacobian() const; + gtsam::JacobianFactor* toJacobian() const; + gtsam::HessianFactor* toHessian() const; + + static gtsam::NonlinearFactorGraph ConvertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph, + const gtsam::Values& linearizationPoint); + + static gtsam::NonlinearFactorGraph ConvertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph); + + // enabling serialization functionality + void serializable() const; +}; // \class LinearContainerFactor // Summarization functionality //#include @@ -2321,210 +2231,196 @@ class Cal3_S2 { // Nonlinear optimizers //************************************************************************* #include - virtual class NonlinearOptimizerParams - { - NonlinearOptimizerParams(); - void print(string s) const; +virtual class NonlinearOptimizerParams { + NonlinearOptimizerParams(); + void print(string s) const; - int getMaxIterations() const; - double getRelativeErrorTol() const; - double getAbsoluteErrorTol() const; - double getErrorTol() const; - string getVerbosity() const; + int getMaxIterations() const; + double getRelativeErrorTol() const; + double getAbsoluteErrorTol() const; + double getErrorTol() const; + string getVerbosity() const; - void setMaxIterations(int value); - void setRelativeErrorTol(double value); - void setAbsoluteErrorTol(double value); - void setErrorTol(double value); - void setVerbosity(string s); + void setMaxIterations(int value); + void setRelativeErrorTol(double value); + void setAbsoluteErrorTol(double value); + void setErrorTol(double value); + void setVerbosity(string s); - string getLinearSolverType() const; - void setLinearSolverType(string solver); + string getLinearSolverType() const; + void setLinearSolverType(string solver); - void setIterativeParams(gtsam::IterativeOptimizationParameters *params); - void setOrdering(const gtsam::Ordering &ordering); - string getOrderingType() const; - void setOrderingType(string ordering); + void setIterativeParams(gtsam::IterativeOptimizationParameters* params); + void setOrdering(const gtsam::Ordering& ordering); + string getOrderingType() const; + void setOrderingType(string ordering); - bool isMultifrontal() const; - bool isSequential() const; - bool isCholmod() const; - bool isIterative() const; - }; + bool isMultifrontal() const; + bool isSequential() const; + bool isCholmod() const; + bool isIterative() const; +}; - bool checkConvergence(double relativeErrorTreshold, - double absoluteErrorTreshold, double errorThreshold, - double currentError, double newError); - bool checkConvergence(const gtsam::NonlinearOptimizerParams ¶ms, - double currentError, double newError); +bool checkConvergence(double relativeErrorTreshold, + double absoluteErrorTreshold, double errorThreshold, + double currentError, double newError); +bool checkConvergence(const gtsam::NonlinearOptimizerParams& params, + double currentError, double newError); #include - virtual class GaussNewtonParams : gtsam::NonlinearOptimizerParams - { - GaussNewtonParams(); - }; +virtual class GaussNewtonParams : gtsam::NonlinearOptimizerParams { + GaussNewtonParams(); +}; #include - virtual class LevenbergMarquardtParams : gtsam::NonlinearOptimizerParams - { - LevenbergMarquardtParams(); +virtual class LevenbergMarquardtParams : gtsam::NonlinearOptimizerParams { + LevenbergMarquardtParams(); - bool getDiagonalDamping() const; - double getlambdaFactor() const; - double getlambdaInitial() const; - double getlambdaLowerBound() const; - double getlambdaUpperBound() const; - bool getUseFixedLambdaFactor(); - string getLogFile() const; - string getVerbosityLM() const; + bool getDiagonalDamping() const; + double getlambdaFactor() const; + double getlambdaInitial() const; + double getlambdaLowerBound() const; + double getlambdaUpperBound() const; + bool getUseFixedLambdaFactor(); + string getLogFile() const; + string getVerbosityLM() const; - void setDiagonalDamping(bool flag); - void setlambdaFactor(double value); - void setlambdaInitial(double value); - void setlambdaLowerBound(double value); - void setlambdaUpperBound(double value); - void setUseFixedLambdaFactor(bool flag); - void setLogFile(string s); - void setVerbosityLM(string s); + void setDiagonalDamping(bool flag); + void setlambdaFactor(double value); + void setlambdaInitial(double value); + void setlambdaLowerBound(double value); + void setlambdaUpperBound(double value); + void setUseFixedLambdaFactor(bool flag); + void setLogFile(string s); + void setVerbosityLM(string s); - static gtsam::LevenbergMarquardtParams LegacyDefaults(); - static gtsam::LevenbergMarquardtParams CeresDefaults(); + static gtsam::LevenbergMarquardtParams LegacyDefaults(); + static gtsam::LevenbergMarquardtParams CeresDefaults(); - static gtsam::LevenbergMarquardtParams EnsureHasOrdering( - gtsam::LevenbergMarquardtParams params, - const gtsam::NonlinearFactorGraph &graph); - static gtsam::LevenbergMarquardtParams ReplaceOrdering( - gtsam::LevenbergMarquardtParams params, const gtsam::Ordering &ordering); - }; + static gtsam::LevenbergMarquardtParams EnsureHasOrdering( + gtsam::LevenbergMarquardtParams params, + const gtsam::NonlinearFactorGraph& graph); + static gtsam::LevenbergMarquardtParams ReplaceOrdering( + gtsam::LevenbergMarquardtParams params, const gtsam::Ordering& ordering); +}; #include - virtual class DoglegParams : gtsam::NonlinearOptimizerParams - { - DoglegParams(); +virtual class DoglegParams : gtsam::NonlinearOptimizerParams { + DoglegParams(); - double getDeltaInitial() const; - string getVerbosityDL() const; + double getDeltaInitial() const; + string getVerbosityDL() const; - void setDeltaInitial(double deltaInitial) const; - void setVerbosityDL(string verbosityDL) const; - }; + void setDeltaInitial(double deltaInitial) const; + void setVerbosityDL(string verbosityDL) const; +}; #include - virtual class NonlinearOptimizer - { - gtsam::Values optimize(); - gtsam::Values optimizeSafely(); - double error() const; - int iterations() const; - gtsam::Values values() const; - gtsam::NonlinearFactorGraph graph() const; - gtsam::GaussianFactorGraph *iterate() const; - }; +virtual class NonlinearOptimizer { + gtsam::Values optimize(); + gtsam::Values optimizeSafely(); + double error() const; + int iterations() const; + gtsam::Values values() const; + gtsam::NonlinearFactorGraph graph() const; + gtsam::GaussianFactorGraph* iterate() const; +}; #include - virtual class GaussNewtonOptimizer : gtsam::NonlinearOptimizer - { - GaussNewtonOptimizer(const gtsam::NonlinearFactorGraph &graph, const gtsam::Values &initialValues); - GaussNewtonOptimizer(const gtsam::NonlinearFactorGraph &graph, const gtsam::Values &initialValues, const gtsam::GaussNewtonParams ¶ms); - }; +virtual class GaussNewtonOptimizer : gtsam::NonlinearOptimizer { + GaussNewtonOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues); + GaussNewtonOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const gtsam::GaussNewtonParams& params); +}; #include - virtual class DoglegOptimizer : gtsam::NonlinearOptimizer - { - DoglegOptimizer(const gtsam::NonlinearFactorGraph &graph, const gtsam::Values &initialValues); - DoglegOptimizer(const gtsam::NonlinearFactorGraph &graph, const gtsam::Values &initialValues, const gtsam::DoglegParams ¶ms); - double getDelta() const; - }; +virtual class DoglegOptimizer : gtsam::NonlinearOptimizer { + DoglegOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues); + DoglegOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const gtsam::DoglegParams& params); + double getDelta() const; +}; #include - virtual class LevenbergMarquardtOptimizer : gtsam::NonlinearOptimizer - { - LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph &graph, const gtsam::Values &initialValues); - LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph &graph, const gtsam::Values &initialValues, const gtsam::LevenbergMarquardtParams ¶ms); - double lambda() const; - void print(string str) const; - }; +virtual class LevenbergMarquardtOptimizer : gtsam::NonlinearOptimizer { + LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues); + LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const gtsam::LevenbergMarquardtParams& params); + double lambda() const; + void print(string str) const; +}; #include - class ISAM2GaussNewtonParams - { - ISAM2GaussNewtonParams(); +class ISAM2GaussNewtonParams { + ISAM2GaussNewtonParams(); - void print(string str) const; + void print(string str) const; - /** Getters and Setters for all properties */ - double getWildfireThreshold() const; - void setWildfireThreshold(double wildfireThreshold); - }; + /** Getters and Setters for all properties */ + double getWildfireThreshold() const; + void setWildfireThreshold(double wildfireThreshold); +}; - class ISAM2DoglegParams - { - ISAM2DoglegParams(); +class ISAM2DoglegParams { + ISAM2DoglegParams(); - void print(string str) const; + void print(string str) const; - /** Getters and Setters for all properties */ - double getWildfireThreshold() const; - void setWildfireThreshold(double wildfireThreshold); - double getInitialDelta() const; - void setInitialDelta(double initialDelta); - string getAdaptationMode() const; - void setAdaptationMode(string adaptationMode); - bool isVerbose() const; - void setVerbose(bool verbose); - }; + /** Getters and Setters for all properties */ + double getWildfireThreshold() const; + void setWildfireThreshold(double wildfireThreshold); + double getInitialDelta() const; + void setInitialDelta(double initialDelta); + string getAdaptationMode() const; + void setAdaptationMode(string adaptationMode); + bool isVerbose() const; + void setVerbose(bool verbose); +}; - class ISAM2ThresholdMapValue - { - ISAM2ThresholdMapValue(char c, Vector thresholds); - ISAM2ThresholdMapValue(const gtsam::ISAM2ThresholdMapValue &other); - }; +class ISAM2ThresholdMapValue { + ISAM2ThresholdMapValue(char c, Vector thresholds); + ISAM2ThresholdMapValue(const gtsam::ISAM2ThresholdMapValue& other); +}; - class ISAM2ThresholdMap - { - ISAM2ThresholdMap(); - ISAM2ThresholdMap(const gtsam::ISAM2ThresholdMap &other); +class ISAM2ThresholdMap { + ISAM2ThresholdMap(); + ISAM2ThresholdMap(const gtsam::ISAM2ThresholdMap& other); - // Note: no print function + // Note: no print function - // common STL methods - size_t size() const; - bool empty() const; - void clear(); + // common STL methods + size_t size() const; + bool empty() const; + void clear(); - // structure specific methods - void insert(const gtsam::ISAM2ThresholdMapValue &value) const; - }; + // structure specific methods + void insert(const gtsam::ISAM2ThresholdMapValue& value) const; +}; - class ISAM2Params - { - ISAM2Params(); +class ISAM2Params { + ISAM2Params(); - void print(string str) const; + void print(string str) const; - /** Getters and Setters for all properties */ - void setOptimizationParams(const gtsam::ISAM2GaussNewtonParams &gauss_newton__params); - void setOptimizationParams(const gtsam::ISAM2DoglegParams &dogleg_params); - void setRelinearizeThreshold(double threshold); - void setRelinearizeThreshold(const gtsam::ISAM2ThresholdMap &threshold_map); - int getRelinearizeSkip() const; - void setRelinearizeSkip(int relinearizeSkip); - bool isEnableRelinearization() const; - void setEnableRelinearization(bool enableRelinearization); - bool isEvaluateNonlinearError() const; - void setEvaluateNonlinearError(bool evaluateNonlinearError); - string getFactorization() const; - void setFactorization(string factorization); - bool isCacheLinearizedFactors() const; - void setCacheLinearizedFactors(bool cacheLinearizedFactors); - bool isEnableDetailedResults() const; - void setEnableDetailedResults(bool enableDetailedResults); - bool isEnablePartialRelinearizationCheck() const; - void setEnablePartialRelinearizationCheck(bool enablePartialRelinearizationCheck); - }; + /** Getters and Setters for all properties */ + void setOptimizationParams(const gtsam::ISAM2GaussNewtonParams& gauss_newton__params); + void setOptimizationParams(const gtsam::ISAM2DoglegParams& dogleg_params); + void setRelinearizeThreshold(double threshold); + void setRelinearizeThreshold(const gtsam::ISAM2ThresholdMap& threshold_map); + int getRelinearizeSkip() const; + void setRelinearizeSkip(int relinearizeSkip); + bool isEnableRelinearization() const; + void setEnableRelinearization(bool enableRelinearization); + bool isEvaluateNonlinearError() const; + void setEvaluateNonlinearError(bool evaluateNonlinearError); + string getFactorization() const; + void setFactorization(string factorization); + bool isCacheLinearizedFactors() const; + void setCacheLinearizedFactors(bool cacheLinearizedFactors); + bool isEnableDetailedResults() const; + void setEnableDetailedResults(bool enableDetailedResults); + bool isEnablePartialRelinearizationCheck() const; + void setEnablePartialRelinearizationCheck(bool enablePartialRelinearizationCheck); +}; - class ISAM2Clique - { +class ISAM2Clique { //Constructors ISAM2Clique(); @@ -2532,77 +2428,74 @@ class Cal3_S2 { //Standard Interface Vector gradientContribution() const; void print(string s); - }; +}; - class ISAM2Result - { - ISAM2Result(); +class ISAM2Result { + ISAM2Result(); - void print(string str) const; + void print(string str) const; - /** Getters and Setters for all properties */ - size_t getVariablesRelinearized() const; - size_t getVariablesReeliminated() const; - size_t getCliques() const; - double getErrorBefore() const; - double getErrorAfter() const; - }; + /** Getters and Setters for all properties */ + size_t getVariablesRelinearized() const; + size_t getVariablesReeliminated() const; + size_t getCliques() const; + double getErrorBefore() const; + double getErrorAfter() const; +}; - class ISAM2 - { - ISAM2(); - ISAM2(const gtsam::ISAM2Params ¶ms); - ISAM2(const gtsam::ISAM2 &other); +class ISAM2 { + ISAM2(); + ISAM2(const gtsam::ISAM2Params& params); + ISAM2(const gtsam::ISAM2& other); - bool equals(const gtsam::ISAM2 &other, double tol) const; - void print(string s) const; - void printStats() const; - void saveGraph(string s) const; + bool equals(const gtsam::ISAM2& other, double tol) const; + void print(string s) const; + void printStats() const; + void saveGraph(string s) const; - gtsam::ISAM2Result update(); - gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph &newFactors, const gtsam::Values &newTheta); - gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph &newFactors, const gtsam::Values &newTheta, const gtsam::FactorIndices &removeFactorIndices); - gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph &newFactors, const gtsam::Values &newTheta, const gtsam::FactorIndices &removeFactorIndices, const gtsam::KeyGroupMap &constrainedKeys); - gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph &newFactors, const gtsam::Values &newTheta, const gtsam::FactorIndices &removeFactorIndices, gtsam::KeyGroupMap &constrainedKeys, const gtsam::KeyList &noRelinKeys); - gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph &newFactors, const gtsam::Values &newTheta, const gtsam::FactorIndices &removeFactorIndices, gtsam::KeyGroupMap &constrainedKeys, const gtsam::KeyList &noRelinKeys, const gtsam::KeyList &extraReelimKeys); - gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph &newFactors, const gtsam::Values &newTheta, const gtsam::FactorIndices &removeFactorIndices, gtsam::KeyGroupMap &constrainedKeys, const gtsam::KeyList &noRelinKeys, const gtsam::KeyList &extraReelimKeys, bool force_relinearize); + gtsam::ISAM2Result update(); + gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta); + gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices); + gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices, const gtsam::KeyGroupMap& constrainedKeys); + gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices, gtsam::KeyGroupMap& constrainedKeys, const gtsam::KeyList& noRelinKeys); + gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices, gtsam::KeyGroupMap& constrainedKeys, const gtsam::KeyList& noRelinKeys, const gtsam::KeyList& extraReelimKeys); + gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices, gtsam::KeyGroupMap& constrainedKeys, const gtsam::KeyList& noRelinKeys, const gtsam::KeyList& extraReelimKeys, bool force_relinearize); - gtsam::Values getLinearizationPoint() const; - gtsam::Values calculateEstimate() const; - template , - Vector, Matrix}> - VALUE calculateEstimate(size_t key) const; - gtsam::Values calculateBestEstimate() const; - Matrix marginalCovariance(size_t key) const; - gtsam::VectorValues getDelta() const; - gtsam::NonlinearFactorGraph getFactorsUnsafe() const; - gtsam::VariableIndex getVariableIndex() const; - gtsam::ISAM2Params params() const; - }; + gtsam::Values getLinearizationPoint() const; + gtsam::Values calculateEstimate() const; + template , + Vector, Matrix}> + VALUE calculateEstimate(size_t key) const; + gtsam::Values calculateBestEstimate() const; + Matrix marginalCovariance(size_t key) const; + gtsam::VectorValues getDelta() const; + gtsam::NonlinearFactorGraph getFactorsUnsafe() const; + gtsam::VariableIndex getVariableIndex() const; + gtsam::ISAM2Params params() const; +}; #include - class NonlinearISAM - { - NonlinearISAM(); - NonlinearISAM(int reorderInterval); - void print(string s) const; - void printStats() const; - void saveGraph(string s) const; - gtsam::Values estimate() const; - Matrix marginalCovariance(size_t key) const; - int reorderInterval() const; - int reorderCounter() const; - void update(const gtsam::NonlinearFactorGraph &newFactors, const gtsam::Values &initialValues); - void reorder_relinearize(); +class NonlinearISAM { + NonlinearISAM(); + NonlinearISAM(int reorderInterval); + void print(string s) const; + void printStats() const; + void saveGraph(string s) const; + gtsam::Values estimate() const; + Matrix marginalCovariance(size_t key) const; + int reorderInterval() const; + int reorderCounter() const; + void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& initialValues); + void reorder_relinearize(); - // These might be expensive as instead of a reference the wrapper will make a copy - gtsam::GaussianISAM bayesTree() const; - gtsam::Values getLinearizationPoint() const; - gtsam::NonlinearFactorGraph getFactorsUnsafe() const; - }; + // These might be expensive as instead of a reference the wrapper will make a copy + gtsam::GaussianISAM bayesTree() const; + gtsam::Values getLinearizationPoint() const; + gtsam::NonlinearFactorGraph getFactorsUnsafe() const; +}; //************************************************************************* // Nonlinear factor types @@ -2611,966 +2504,917 @@ class Cal3_S2 { #include #include - template }> - virtual class PriorFactor : gtsam::NoiseModelFactor - { - PriorFactor(size_t key, const T &prior, const gtsam::noiseModel::Base *noiseModel); - T prior() const; +template}> +virtual class PriorFactor : gtsam::NoiseModelFactor { + PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); + T prior() const; + + // enabling serialization functionality + void serialize() const; +}; - // enabling serialization functionality - void serialize() const; - }; #include - template - virtual class BetweenFactor : gtsam::NoiseModelFactor - { - BetweenFactor(size_t key1, size_t key2, const T &relativePose, const gtsam::noiseModel::Base *noiseModel); - T measured() const; +template +virtual class BetweenFactor : gtsam::NoiseModelFactor { + BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel); + T measured() const; - // enabling serialization functionality - void serialize() const; - }; + // enabling serialization functionality + void serialize() const; +}; #include - template - virtual class NonlinearEquality : gtsam::NoiseModelFactor - { - // Constructor - forces exact evaluation - NonlinearEquality(size_t j, const T &feasible); - // Constructor - allows inexact evaluation - NonlinearEquality(size_t j, const T &feasible, double error_gain); +template +virtual class NonlinearEquality : gtsam::NoiseModelFactor { + // Constructor - forces exact evaluation + NonlinearEquality(size_t j, const T& feasible); + // Constructor - allows inexact evaluation + NonlinearEquality(size_t j, const T& feasible, double error_gain); - // enabling serialization functionality - void serialize() const; - }; + // enabling serialization functionality + void serialize() const; +}; #include - template - virtual class RangeFactor : gtsam::NoiseModelFactor - { - RangeFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base *noiseModel); +template +virtual class RangeFactor : gtsam::NoiseModelFactor { + RangeFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel); - // enabling serialization functionality - void serialize() const; - }; + // enabling serialization functionality + void serialize() const; +}; + +typedef gtsam::RangeFactor RangeFactor2D; +typedef gtsam::RangeFactor RangeFactor3D; +typedef gtsam::RangeFactor RangeFactorPose2; +typedef gtsam::RangeFactor RangeFactorPose3; +typedef gtsam::RangeFactor RangeFactorCalibratedCameraPoint; +typedef gtsam::RangeFactor RangeFactorSimpleCameraPoint; +typedef gtsam::RangeFactor RangeFactorCalibratedCamera; +typedef gtsam::RangeFactor RangeFactorSimpleCamera; - typedef gtsam::RangeFactor RangeFactor2D; - typedef gtsam::RangeFactor RangeFactor3D; - typedef gtsam::RangeFactor RangeFactorPose2; - typedef gtsam::RangeFactor RangeFactorPose3; - typedef gtsam::RangeFactor RangeFactorCalibratedCameraPoint; - typedef gtsam::RangeFactor RangeFactorSimpleCameraPoint; - typedef gtsam::RangeFactor RangeFactorCalibratedCamera; - typedef gtsam::RangeFactor RangeFactorSimpleCamera; #include - template - virtual class RangeFactorWithTransform : gtsam::NoiseModelFactor - { - RangeFactorWithTransform(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base *noiseModel, const POSE &body_T_sensor); +template +virtual class RangeFactorWithTransform : gtsam::NoiseModelFactor { + RangeFactorWithTransform(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel, const POSE& body_T_sensor); - // enabling serialization functionality - void serialize() const; - }; + // enabling serialization functionality + void serialize() const; +}; - typedef gtsam::RangeFactorWithTransform RangeFactorWithTransform2D; - typedef gtsam::RangeFactorWithTransform RangeFactorWithTransform3D; - typedef gtsam::RangeFactorWithTransform RangeFactorWithTransformPose2; - typedef gtsam::RangeFactorWithTransform RangeFactorWithTransformPose3; +typedef gtsam::RangeFactorWithTransform RangeFactorWithTransform2D; +typedef gtsam::RangeFactorWithTransform RangeFactorWithTransform3D; +typedef gtsam::RangeFactorWithTransform RangeFactorWithTransformPose2; +typedef gtsam::RangeFactorWithTransform RangeFactorWithTransformPose3; #include - template - virtual class BearingFactor : gtsam::NoiseModelFactor - { - BearingFactor(size_t key1, size_t key2, const BEARING &measured, const gtsam::noiseModel::Base *noiseModel); +template +virtual class BearingFactor : gtsam::NoiseModelFactor { + BearingFactor(size_t key1, size_t key2, const BEARING& measured, const gtsam::noiseModel::Base* noiseModel); - // enabling serialization functionality - void serialize() const; - }; + // enabling serialization functionality + void serialize() const; +}; - typedef gtsam::BearingFactor BearingFactor2D; - typedef gtsam::BearingFactor BearingFactorPose2; +typedef gtsam::BearingFactor BearingFactor2D; +typedef gtsam::BearingFactor BearingFactorPose2; #include - template - class BearingRange - { - BearingRange(const BEARING &b, const RANGE &r); - BEARING bearing() const; - RANGE range() const; - static This Measure(const POSE &pose, const POINT &point); - static BEARING MeasureBearing(const POSE &pose, const POINT &point); - static RANGE MeasureRange(const POSE &pose, const POINT &point); - void print(string s) const; - }; +template +class BearingRange { + BearingRange(const BEARING& b, const RANGE& r); + BEARING bearing() const; + RANGE range() const; + static This Measure(const POSE& pose, const POINT& point); + static BEARING MeasureBearing(const POSE& pose, const POINT& point); + static RANGE MeasureRange(const POSE& pose, const POINT& point); + void print(string s) const; +}; - typedef gtsam::BearingRange BearingRange2D; +typedef gtsam::BearingRange BearingRange2D; #include - template - virtual class BearingRangeFactor : gtsam::NoiseModelFactor - { - BearingRangeFactor(size_t poseKey, size_t pointKey, - const BEARING &measuredBearing, const RANGE &measuredRange, - const gtsam::noiseModel::Base *noiseModel); +template +virtual class BearingRangeFactor : gtsam::NoiseModelFactor { + BearingRangeFactor(size_t poseKey, size_t pointKey, + const BEARING& measuredBearing, const RANGE& measuredRange, + const gtsam::noiseModel::Base* noiseModel); - // enabling serialization functionality - void serialize() const; - }; + // enabling serialization functionality + void serialize() const; +}; + +typedef gtsam::BearingRangeFactor BearingRangeFactor2D; +typedef gtsam::BearingRangeFactor BearingRangeFactorPose2; - typedef gtsam::BearingRangeFactor BearingRangeFactor2D; - typedef gtsam::BearingRangeFactor BearingRangeFactorPose2; #include - template - virtual class GenericProjectionFactor : gtsam::NoiseModelFactor - { - GenericProjectionFactor(const gtsam::Point2 &measured, const gtsam::noiseModel::Base *noiseModel, - size_t poseKey, size_t pointKey, const CALIBRATION *k); - GenericProjectionFactor(const gtsam::Point2 &measured, const gtsam::noiseModel::Base *noiseModel, - size_t poseKey, size_t pointKey, const CALIBRATION *k, const POSE &body_P_sensor); +template +virtual class GenericProjectionFactor : gtsam::NoiseModelFactor { + GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, + size_t poseKey, size_t pointKey, const CALIBRATION* k); + GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, + size_t poseKey, size_t pointKey, const CALIBRATION* k, const POSE& body_P_sensor); - GenericProjectionFactor(const gtsam::Point2 &measured, const gtsam::noiseModel::Base *noiseModel, - size_t poseKey, size_t pointKey, const CALIBRATION *k, bool throwCheirality, bool verboseCheirality); - GenericProjectionFactor(const gtsam::Point2 &measured, const gtsam::noiseModel::Base *noiseModel, - size_t poseKey, size_t pointKey, const CALIBRATION *k, bool throwCheirality, bool verboseCheirality, - const POSE &body_P_sensor); + GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, + size_t poseKey, size_t pointKey, const CALIBRATION* k, bool throwCheirality, bool verboseCheirality); + GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, + size_t poseKey, size_t pointKey, const CALIBRATION* k, bool throwCheirality, bool verboseCheirality, + const POSE& body_P_sensor); - gtsam::Point2 measured() const; - CALIBRATION *calibration() const; - bool verboseCheirality() const; - bool throwCheirality() const; + gtsam::Point2 measured() const; + CALIBRATION* calibration() const; + bool verboseCheirality() const; + bool throwCheirality() const; + + // enabling serialization functionality + void serialize() const; +}; +typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3_S2; +typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3DS2; - // enabling serialization functionality - void serialize() const; - }; - typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3_S2; - typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3DS2; #include - template - virtual class GeneralSFMFactor : gtsam::NoiseModelFactor - { - GeneralSFMFactor(const gtsam::Point2 &measured, const gtsam::noiseModel::Base *model, size_t cameraKey, size_t landmarkKey); - gtsam::Point2 measured() const; - }; - typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3_S2; - typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3DS2; - typedef gtsam::GeneralSFMFactor, gtsam::Point3> GeneralSFMFactorCal3Bundler; +template +virtual class GeneralSFMFactor : gtsam::NoiseModelFactor { + GeneralSFMFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model, size_t cameraKey, size_t landmarkKey); + gtsam::Point2 measured() const; +}; +typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3_S2; +typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3DS2; +typedef gtsam::GeneralSFMFactor, gtsam::Point3> GeneralSFMFactorCal3Bundler; - template - virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor - { - GeneralSFMFactor2(const gtsam::Point2 &measured, const gtsam::noiseModel::Base *model, size_t poseKey, size_t landmarkKey, size_t calibKey); - gtsam::Point2 measured() const; +template +virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor { + GeneralSFMFactor2(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model, size_t poseKey, size_t landmarkKey, size_t calibKey); + gtsam::Point2 measured() const; - // enabling serialization functionality - void serialize() const; - }; + // enabling serialization functionality + void serialize() const; +}; #include - class SmartProjectionParams - { - SmartProjectionParams(); - // TODO(frank): make these work: - // void setLinearizationMode(LinearizationMode linMode); - // void setDegeneracyMode(DegeneracyMode degMode); - void setRankTolerance(double rankTol); - void setEnableEPI(bool enableEPI); - void setLandmarkDistanceThreshold(bool landmarkDistanceThreshold); - void setDynamicOutlierRejectionThreshold(bool dynOutRejectionThreshold); - }; +class SmartProjectionParams { + SmartProjectionParams(); + // TODO(frank): make these work: + // void setLinearizationMode(LinearizationMode linMode); + // void setDegeneracyMode(DegeneracyMode degMode); + void setRankTolerance(double rankTol); + void setEnableEPI(bool enableEPI); + void setLandmarkDistanceThreshold(bool landmarkDistanceThreshold); + void setDynamicOutlierRejectionThreshold(bool dynOutRejectionThreshold); +}; #include - template - virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor - { +template +virtual class SmartProjectionPoseFactor: gtsam::NonlinearFactor { - SmartProjectionPoseFactor(const gtsam::noiseModel::Base *noise, - const CALIBRATION *K); - SmartProjectionPoseFactor(const gtsam::noiseModel::Base *noise, - const CALIBRATION *K, - const gtsam::Pose3 &body_P_sensor); - SmartProjectionPoseFactor(const gtsam::noiseModel::Base *noise, - const CALIBRATION *K, - const gtsam::SmartProjectionParams ¶ms); - SmartProjectionPoseFactor(const gtsam::noiseModel::Base *noise, - const CALIBRATION *K, - const gtsam::Pose3 &body_P_sensor, - const gtsam::SmartProjectionParams ¶ms); + SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, + const CALIBRATION* K); + SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, + const CALIBRATION* K, + const gtsam::Pose3& body_P_sensor); + SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, + const CALIBRATION* K, + const gtsam::SmartProjectionParams& params); + SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, + const CALIBRATION* K, + const gtsam::Pose3& body_P_sensor, + const gtsam::SmartProjectionParams& params); - void add(const gtsam::Point2 &measured_i, size_t poseKey_i); + void add(const gtsam::Point2& measured_i, size_t poseKey_i); - // enabling serialization functionality - //void serialize() const; - }; + // enabling serialization functionality + //void serialize() const; +}; + +typedef gtsam::SmartProjectionPoseFactor SmartProjectionPose3Factor; - typedef gtsam::SmartProjectionPoseFactor SmartProjectionPose3Factor; #include - template - virtual class GenericStereoFactor : gtsam::NoiseModelFactor - { - GenericStereoFactor(const gtsam::StereoPoint2 &measured, const gtsam::noiseModel::Base *noiseModel, - size_t poseKey, size_t landmarkKey, const gtsam::Cal3_S2Stereo *K); - gtsam::StereoPoint2 measured() const; - gtsam::Cal3_S2Stereo *calibration() const; +template +virtual class GenericStereoFactor : gtsam::NoiseModelFactor { + GenericStereoFactor(const gtsam::StereoPoint2& measured, const gtsam::noiseModel::Base* noiseModel, + size_t poseKey, size_t landmarkKey, const gtsam::Cal3_S2Stereo* K); + gtsam::StereoPoint2 measured() const; + gtsam::Cal3_S2Stereo* calibration() const; - // enabling serialization functionality - void serialize() const; - }; - typedef gtsam::GenericStereoFactor GenericStereoFactor3D; + // enabling serialization functionality + void serialize() const; +}; +typedef gtsam::GenericStereoFactor GenericStereoFactor3D; #include - template - virtual class PoseTranslationPrior : gtsam::NoiseModelFactor - { - PoseTranslationPrior(size_t key, const POSE &pose_z, const gtsam::noiseModel::Base *noiseModel); - }; +template +virtual class PoseTranslationPrior : gtsam::NoiseModelFactor { + PoseTranslationPrior(size_t key, const POSE& pose_z, const gtsam::noiseModel::Base* noiseModel); +}; - typedef gtsam::PoseTranslationPrior PoseTranslationPrior2D; - typedef gtsam::PoseTranslationPrior PoseTranslationPrior3D; +typedef gtsam::PoseTranslationPrior PoseTranslationPrior2D; +typedef gtsam::PoseTranslationPrior PoseTranslationPrior3D; #include - template - virtual class PoseRotationPrior : gtsam::NoiseModelFactor - { - PoseRotationPrior(size_t key, const POSE &pose_z, const gtsam::noiseModel::Base *noiseModel); - }; +template +virtual class PoseRotationPrior : gtsam::NoiseModelFactor { + PoseRotationPrior(size_t key, const POSE& pose_z, const gtsam::noiseModel::Base* noiseModel); +}; - typedef gtsam::PoseRotationPrior PoseRotationPrior2D; - typedef gtsam::PoseRotationPrior PoseRotationPrior3D; +typedef gtsam::PoseRotationPrior PoseRotationPrior2D; +typedef gtsam::PoseRotationPrior PoseRotationPrior3D; #include - virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor - { - EssentialMatrixFactor(size_t key, const gtsam::Point2 &pA, const gtsam::Point2 &pB, - const gtsam::noiseModel::Base *noiseModel); - }; +virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor { + EssentialMatrixFactor(size_t key, const gtsam::Point2& pA, const gtsam::Point2& pB, + const gtsam::noiseModel::Base* noiseModel); +}; #include - class SfmTrack - { - SfmTrack(); - SfmTrack(const gtsam::Point3 &pt); - const Point3 &point3() const; +class SfmTrack { + SfmTrack(); + SfmTrack(const gtsam::Point3& pt); + const Point3& point3() const; - double r; - double g; - double b; - // TODO Need to close wrap#10 to allow this to work. - // std::vector> measurements; + double r; + double g; + double b; + // TODO Need to close wrap#10 to allow this to work. + // std::vector> measurements; - size_t number_measurements() const; - pair measurement(size_t idx) const; - pair siftIndex(size_t idx) const; - void add_measurement(size_t idx, const gtsam::Point2 &m); - }; + size_t number_measurements() const; + pair measurement(size_t idx) const; + pair siftIndex(size_t idx) const; + void add_measurement(size_t idx, const gtsam::Point2& m); +}; - class SfmData - { - SfmData(); - size_t number_cameras() const; - size_t number_tracks() const; - gtsam::PinholeCamera camera(size_t idx) const; - gtsam::SfmTrack track(size_t idx) const; - void add_track(const gtsam::SfmTrack &t); - void add_camera(const gtsam::SfmCamera &cam); - }; +class SfmData { + SfmData(); + size_t number_cameras() const; + size_t number_tracks() const; + gtsam::PinholeCamera camera(size_t idx) const; + gtsam::SfmTrack track(size_t idx) const; + void add_track(const gtsam::SfmTrack& t) ; + void add_camera(const gtsam::SfmCamera& cam); +}; - gtsam::SfmData readBal(string filename); - bool writeBAL(string filename, gtsam::SfmData &data); - gtsam::Values initialCamerasEstimate(const gtsam::SfmData &db); - gtsam::Values initialCamerasAndPointsEstimate(const gtsam::SfmData &db); +gtsam::SfmData readBal(string filename); +bool writeBAL(string filename, gtsam::SfmData& data); +gtsam::Values initialCamerasEstimate(const gtsam::SfmData& db); +gtsam::Values initialCamerasAndPointsEstimate(const gtsam::SfmData& db); - pair load2D(string filename, - gtsam::noiseModel::Diagonal *model, int maxIndex, bool addNoise, bool smart); - pair load2D(string filename, - gtsam::noiseModel::Diagonal *model, int maxIndex, bool addNoise); - pair load2D(string filename, - gtsam::noiseModel::Diagonal *model, int maxIndex); - pair load2D(string filename, - gtsam::noiseModel::Diagonal *model); - pair load2D(string filename); - pair load2D_robust(string filename, - gtsam::noiseModel::Base *model, int maxIndex); - void save2D(const gtsam::NonlinearFactorGraph &graph, - const gtsam::Values &config, gtsam::noiseModel::Diagonal *model, - string filename); +pair load2D(string filename, + gtsam::noiseModel::Diagonal* model, int maxIndex, bool addNoise, bool smart); +pair load2D(string filename, + gtsam::noiseModel::Diagonal* model, int maxIndex, bool addNoise); +pair load2D(string filename, + gtsam::noiseModel::Diagonal* model, int maxIndex); +pair load2D(string filename, + gtsam::noiseModel::Diagonal* model); +pair load2D(string filename); +pair load2D_robust(string filename, + gtsam::noiseModel::Base* model, int maxIndex); +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[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); +// 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); +pair load3D(string filename); - pair readG2o(string filename); - pair readG2o(string filename, bool is3D); - void writeG2o(const gtsam::NonlinearFactorGraph &graph, - const gtsam::Values &estimate, string filename); +pair readG2o(string filename); +pair readG2o(string filename, bool is3D); +void writeG2o(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& estimate, string filename); #include - class InitializePose3 - { - static gtsam::Values computeOrientationsChordal( - const gtsam::NonlinearFactorGraph &pose3Graph); - static gtsam::Values computeOrientationsGradient( - const gtsam::NonlinearFactorGraph &pose3Graph, - const gtsam::Values &givenGuess, size_t maxIter, const bool setRefFrame); - static gtsam::Values computeOrientationsGradient( - const gtsam::NonlinearFactorGraph &pose3Graph, - const gtsam::Values &givenGuess); - static gtsam::NonlinearFactorGraph buildPose3graph( - const gtsam::NonlinearFactorGraph &graph); - static gtsam::Values initializeOrientations( - const gtsam::NonlinearFactorGraph &graph); - static gtsam::Values initialize(const gtsam::NonlinearFactorGraph &graph, - const gtsam::Values &givenGuess, - bool useGradient); - static gtsam::Values initialize(const gtsam::NonlinearFactorGraph &graph); - }; +class InitializePose3 { + static gtsam::Values computeOrientationsChordal( + const gtsam::NonlinearFactorGraph& pose3Graph); + static gtsam::Values computeOrientationsGradient( + const gtsam::NonlinearFactorGraph& pose3Graph, + const gtsam::Values& givenGuess, size_t maxIter, const bool setRefFrame); + static gtsam::Values computeOrientationsGradient( + const gtsam::NonlinearFactorGraph& pose3Graph, + const gtsam::Values& givenGuess); + static gtsam::NonlinearFactorGraph buildPose3graph( + const gtsam::NonlinearFactorGraph& graph); + static gtsam::Values initializeOrientations( + const gtsam::NonlinearFactorGraph& graph); + static gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& givenGuess, + bool useGradient); + static gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph); +}; #include - template - virtual class KarcherMeanFactor : gtsam::NonlinearFactor - { - KarcherMeanFactor(const gtsam::KeyVector &keys); - }; +template +virtual class KarcherMeanFactor : gtsam::NonlinearFactor { + KarcherMeanFactor(const gtsam::KeyVector& keys); +}; #include - gtsam::noiseModel::Isotropic *ConvertNoiseModel( - gtsam::noiseModel::Base *model, size_t d); +gtsam::noiseModel::Isotropic* ConvertNoiseModel( + gtsam::noiseModel::Base* model, size_t d); - template - virtual class FrobeniusFactor : gtsam::NoiseModelFactor - { - FrobeniusFactor(size_t key1, size_t key2); - FrobeniusFactor(size_t key1, size_t key2, gtsam::noiseModel::Base *model); +template +virtual class FrobeniusFactor : gtsam::NoiseModelFactor { + FrobeniusFactor(size_t key1, size_t key2); + FrobeniusFactor(size_t key1, size_t key2, gtsam::noiseModel::Base* model); - Vector evaluateError(const T &R1, const T &R2); - }; + Vector evaluateError(const T& R1, const T& R2); +}; - template - virtual class FrobeniusBetweenFactor : gtsam::NoiseModelFactor - { - FrobeniusBetweenFactor(size_t key1, size_t key2, const T &R12); - FrobeniusBetweenFactor(size_t key1, size_t key2, const T &R12, gtsam::noiseModel::Base *model); +template +virtual class FrobeniusBetweenFactor : gtsam::NoiseModelFactor { + FrobeniusBetweenFactor(size_t key1, size_t key2, const T& R12); + FrobeniusBetweenFactor(size_t key1, size_t key2, const T& R12, gtsam::noiseModel::Base* model); - Vector evaluateError(const T &R1, const T &R2); - }; + Vector evaluateError(const T& R1, const T& R2); +}; #include - virtual class ShonanFactor3 : gtsam::NoiseModelFactor - { - ShonanFactor3(size_t key1, size_t key2, const gtsam::Rot3 &R12, - size_t p); - ShonanFactor3(size_t key1, size_t key2, const gtsam::Rot3 &R12, - size_t p, gtsam::noiseModel::Base *model); - Vector evaluateError(const gtsam::SOn &Q1, const gtsam::SOn &Q2); - }; +virtual class ShonanFactor3 : gtsam::NoiseModelFactor { + ShonanFactor3(size_t key1, size_t key2, const gtsam::Rot3 &R12, + size_t p); + ShonanFactor3(size_t key1, size_t key2, const gtsam::Rot3 &R12, + size_t p, gtsam::noiseModel::Base *model); + Vector evaluateError(const gtsam::SOn &Q1, const gtsam::SOn &Q2); +}; #include - template - class BinaryMeasurement - { - BinaryMeasurement(size_t key1, size_t key2, const T &measured, - const gtsam::noiseModel::Base *model); - size_t key1() const; - size_t key2() const; - T measured() const; - gtsam::noiseModel::Base *noiseModel() const; - }; +template +class BinaryMeasurement { + BinaryMeasurement(size_t key1, size_t key2, const T& measured, + const gtsam::noiseModel::Base* model); + size_t key1() const; + size_t key2() const; + T measured() const; + gtsam::noiseModel::Base* noiseModel() const; +}; - typedef gtsam::BinaryMeasurement BinaryMeasurementUnit3; - typedef gtsam::BinaryMeasurement BinaryMeasurementRot3; +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); - }; +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 wrap! +// TODO(frank): copy/pasta below until we have integer template paremeters in wrap! - class ShonanAveragingParameters2 - { - ShonanAveragingParameters2(const gtsam::LevenbergMarquardtParams &lm); - ShonanAveragingParameters2(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(); - void setGaugesWeight(double value); - double getGaugesWeight(); - }; +class ShonanAveragingParameters2 { + ShonanAveragingParameters2(const gtsam::LevenbergMarquardtParams& lm); + ShonanAveragingParameters2(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(); + void setGaugesWeight(double value); + double getGaugesWeight(); +}; - 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 setAnchorWeight(double value); - double getAnchorWeight() const; - void setKarcherWeight(double value); - double getKarcherWeight(); - void setGaugesWeight(double value); - double getGaugesWeight(); - }; +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 setAnchorWeight(double value); + double getAnchorWeight() const; + void setKarcherWeight(double value); + double getKarcherWeight(); + void setGaugesWeight(double value); + double getGaugesWeight(); +}; - class ShonanAveraging2 - { - ShonanAveraging2(string g2oFile); - ShonanAveraging2(string g2oFile, - const gtsam::ShonanAveragingParameters2 ¶meters); +class ShonanAveraging2 { + ShonanAveraging2(string g2oFile); + ShonanAveraging2(string g2oFile, + const gtsam::ShonanAveragingParameters2 ¶meters); - // Query properties - size_t nrUnknowns() const; - size_t nrMeasurements() const; - gtsam::Rot2 measured(size_t i); - gtsam::KeyVector keys(size_t i); + // Query properties + size_t nrUnknowns() const; + size_t nrMeasurements() const; + gtsam::Rot2 measured(size_t i); + gtsam::KeyVector keys(size_t i); - // Matrix API (advanced use, debugging) - Matrix denseD() const; - Matrix denseQ() const; - Matrix denseL() const; - // Matrix computeLambda_(Matrix S) const; - Matrix computeLambda_(const gtsam::Values &values) const; - Matrix computeA_(const gtsam::Values &values) const; - double computeMinEigenValue(const gtsam::Values &values) const; - gtsam::Values initializeWithDescent(size_t p, const gtsam::Values &values, - const Vector &minEigenVector, double minEigenValue) const; + // Matrix API (advanced use, debugging) + Matrix denseD() const; + Matrix denseQ() const; + Matrix denseL() const; + // Matrix computeLambda_(Matrix S) const; + Matrix computeLambda_(const gtsam::Values& values) const; + Matrix computeA_(const gtsam::Values& values) const; + double computeMinEigenValue(const gtsam::Values& values) const; + gtsam::Values initializeWithDescent(size_t p, const gtsam::Values& values, + const Vector& minEigenVector, double minEigenValue) const; - // Advanced API - gtsam::NonlinearFactorGraph buildGraphAt(size_t p) const; - gtsam::Values initializeRandomlyAt(size_t p) const; - double costAt(size_t p, const gtsam::Values &values) const; - pair computeMinEigenVector(const gtsam::Values &values) const; - bool checkOptimality(const gtsam::Values &values) const; - gtsam::LevenbergMarquardtOptimizer *createOptimizerAt(size_t p, const gtsam::Values &initial); - // gtsam::Values tryOptimizingAt(size_t p) const; - gtsam::Values tryOptimizingAt(size_t p, const gtsam::Values &initial) const; - gtsam::Values projectFrom(size_t p, const gtsam::Values &values) const; - gtsam::Values roundSolution(const gtsam::Values &values) const; + // Advanced API + gtsam::NonlinearFactorGraph buildGraphAt(size_t p) const; + gtsam::Values initializeRandomlyAt(size_t p) const; + double costAt(size_t p, const gtsam::Values& values) const; + pair computeMinEigenVector(const gtsam::Values& values) const; + bool checkOptimality(const gtsam::Values& values) const; + gtsam::LevenbergMarquardtOptimizer* createOptimizerAt(size_t p, const gtsam::Values& initial); + // gtsam::Values tryOptimizingAt(size_t p) const; + gtsam::Values tryOptimizingAt(size_t p, const gtsam::Values& initial) const; + gtsam::Values projectFrom(size_t p, const gtsam::Values& values) const; + gtsam::Values roundSolution(const gtsam::Values& values) const; - // Basic API - double cost(const gtsam::Values &values) const; - gtsam::Values initializeRandomly() const; - pair run(const gtsam::Values &initial, size_t min_p, size_t max_p) const; - }; + // Basic API + double cost(const gtsam::Values& values) const; + gtsam::Values initializeRandomly() const; + pair run(const gtsam::Values& initial, size_t min_p, size_t max_p) const; +}; - class ShonanAveraging3 - { - ShonanAveraging3(string g2oFile); - ShonanAveraging3(string g2oFile, - const gtsam::ShonanAveragingParameters3 ¶meters); +class ShonanAveraging3 { + ShonanAveraging3(string g2oFile); + ShonanAveraging3(string g2oFile, + const gtsam::ShonanAveragingParameters3 ¶meters); + + // TODO(frank): deprecate once we land pybind wrapper + ShonanAveraging3(const gtsam::BetweenFactorPose3s &factors); + ShonanAveraging3(const gtsam::BetweenFactorPose3s &factors, + const gtsam::ShonanAveragingParameters3 ¶meters); - // TODO(frank): deprecate once we land pybind wrapper - ShonanAveraging3(const gtsam::BetweenFactorPose3s &factors); - ShonanAveraging3(const gtsam::BetweenFactorPose3s &factors, - const gtsam::ShonanAveragingParameters3 ¶meters); + // Query properties + size_t nrUnknowns() const; + size_t nrMeasurements() const; + gtsam::Rot3 measured(size_t i); + gtsam::KeyVector keys(size_t i); - // Query properties - size_t nrUnknowns() const; - size_t nrMeasurements() const; - gtsam::Rot3 measured(size_t i); - gtsam::KeyVector keys(size_t i); + // Matrix API (advanced use, debugging) + Matrix denseD() const; + Matrix denseQ() const; + Matrix denseL() const; + // Matrix computeLambda_(Matrix S) const; + Matrix computeLambda_(const gtsam::Values& values) const; + Matrix computeA_(const gtsam::Values& values) const; + double computeMinEigenValue(const gtsam::Values& values) const; + gtsam::Values initializeWithDescent(size_t p, const gtsam::Values& values, + const Vector& minEigenVector, double minEigenValue) const; - // Matrix API (advanced use, debugging) - Matrix denseD() const; - Matrix denseQ() const; - Matrix denseL() const; - // Matrix computeLambda_(Matrix S) const; - Matrix computeLambda_(const gtsam::Values &values) const; - Matrix computeA_(const gtsam::Values &values) const; - double computeMinEigenValue(const gtsam::Values &values) const; - gtsam::Values initializeWithDescent(size_t p, const gtsam::Values &values, - const Vector &minEigenVector, double minEigenValue) const; + // Advanced API + gtsam::NonlinearFactorGraph buildGraphAt(size_t p) const; + gtsam::Values initializeRandomlyAt(size_t p) const; + double costAt(size_t p, const gtsam::Values& values) const; + pair computeMinEigenVector(const gtsam::Values& values) const; + bool checkOptimality(const gtsam::Values& values) const; + gtsam::LevenbergMarquardtOptimizer* createOptimizerAt(size_t p, const gtsam::Values& initial); + // gtsam::Values tryOptimizingAt(size_t p) const; + gtsam::Values tryOptimizingAt(size_t p, const gtsam::Values& initial) const; + gtsam::Values projectFrom(size_t p, const gtsam::Values& values) const; + gtsam::Values roundSolution(const gtsam::Values& values) const; - // Advanced API - gtsam::NonlinearFactorGraph buildGraphAt(size_t p) const; - gtsam::Values initializeRandomlyAt(size_t p) const; - double costAt(size_t p, const gtsam::Values &values) const; - pair computeMinEigenVector(const gtsam::Values &values) const; - bool checkOptimality(const gtsam::Values &values) const; - gtsam::LevenbergMarquardtOptimizer *createOptimizerAt(size_t p, const gtsam::Values &initial); - // gtsam::Values tryOptimizingAt(size_t p) const; - gtsam::Values tryOptimizingAt(size_t p, const gtsam::Values &initial) const; - gtsam::Values projectFrom(size_t p, const gtsam::Values &values) const; - gtsam::Values roundSolution(const gtsam::Values &values) const; - - // Basic API - double cost(const gtsam::Values &values) const; - gtsam::Values initializeRandomly() const; - pair run(const gtsam::Values &initial, size_t min_p, size_t max_p) const; - }; + // Basic API + double cost(const gtsam::Values& values) const; + gtsam::Values initializeRandomly() const; + pair run(const gtsam::Values& initial, size_t min_p, size_t max_p) const; +}; #include - class KeyPairDoubleMap - { - KeyPairDoubleMap(); - KeyPairDoubleMap(const gtsam::KeyPairDoubleMap &other); +class KeyPairDoubleMap { + KeyPairDoubleMap(); + KeyPairDoubleMap(const gtsam::KeyPairDoubleMap& other); - size_t size() const; - bool empty() const; - void clear(); - size_t at(const pair &keypair) const; - }; + size_t size() const; + bool empty() const; + void clear(); + size_t at(const pair& keypair) const; +}; - class MFAS - { - MFAS(const gtsam::BinaryMeasurementsUnit3 &relativeTranslations, - const gtsam::Unit3 &projectionDirection); +class MFAS { + MFAS(const gtsam::BinaryMeasurementsUnit3& relativeTranslations, + const gtsam::Unit3& projectionDirection); - gtsam::KeyPairDoubleMap computeOutlierWeights() const; - gtsam::KeyVector computeOrdering() const; - }; + gtsam::KeyPairDoubleMap computeOutlierWeights() const; + gtsam::KeyVector computeOrdering() const; +}; #include - class TranslationRecovery - { - TranslationRecovery(const gtsam::BinaryMeasurementsUnit3 &relativeTranslations, - const gtsam::LevenbergMarquardtParams &lmParams); - TranslationRecovery( - const gtsam::BinaryMeasurementsUnit3 &relativeTranslations); // default LevenbergMarquardtParams - gtsam::Values run(const double scale) const; - gtsam::Values run() const; // default scale = 1.0 - }; +class TranslationRecovery { + TranslationRecovery(const gtsam::BinaryMeasurementsUnit3 &relativeTranslations, + const gtsam::LevenbergMarquardtParams &lmParams); + TranslationRecovery( + const gtsam::BinaryMeasurementsUnit3 & relativeTranslations); // default LevenbergMarquardtParams + gtsam::Values run(const double scale) const; + gtsam::Values run() const; // default scale = 1.0 +}; - //************************************************************************* - // Navigation - //************************************************************************* - namespace imuBias - { +//************************************************************************* +// Navigation +//************************************************************************* +namespace imuBias { #include - class ConstantBias - { - // Constructors - ConstantBias(); - ConstantBias(Vector biasAcc, Vector biasGyro); +class ConstantBias { + // Constructors + ConstantBias(); + ConstantBias(Vector biasAcc, Vector biasGyro); - // Testable - void print(string s) const; - bool equals(const gtsam::imuBias::ConstantBias &expected, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::imuBias::ConstantBias& expected, double tol) const; - // Group - static gtsam::imuBias::ConstantBias identity(); - gtsam::imuBias::ConstantBias inverse() const; - gtsam::imuBias::ConstantBias compose(const gtsam::imuBias::ConstantBias &b) const; - gtsam::imuBias::ConstantBias between(const gtsam::imuBias::ConstantBias &b) const; + // Group + static gtsam::imuBias::ConstantBias identity(); + gtsam::imuBias::ConstantBias inverse() const; + gtsam::imuBias::ConstantBias compose(const gtsam::imuBias::ConstantBias& b) const; + gtsam::imuBias::ConstantBias between(const gtsam::imuBias::ConstantBias& b) const; - // Manifold - gtsam::imuBias::ConstantBias retract(Vector v) const; - Vector localCoordinates(const gtsam::imuBias::ConstantBias &b) const; + // Manifold + gtsam::imuBias::ConstantBias retract(Vector v) const; + Vector localCoordinates(const gtsam::imuBias::ConstantBias& b) const; - // Lie Group - static gtsam::imuBias::ConstantBias Expmap(Vector v); - static Vector Logmap(const gtsam::imuBias::ConstantBias &b); + // Lie Group + static gtsam::imuBias::ConstantBias Expmap(Vector v); + static Vector Logmap(const gtsam::imuBias::ConstantBias& b); - // Standard Interface - Vector vector() const; - Vector accelerometer() const; - Vector gyroscope() const; - Vector correctAccelerometer(Vector measurement) const; - Vector correctGyroscope(Vector measurement) const; - }; + // Standard Interface + Vector vector() const; + Vector accelerometer() const; + Vector gyroscope() const; + Vector correctAccelerometer(Vector measurement) const; + Vector correctGyroscope(Vector measurement) const; +}; - } // namespace imuBias +}///\namespace imuBias #include - class NavState - { - // Constructors - NavState(); - NavState(const gtsam::Rot3 &R, const gtsam::Point3 &t, Vector v); - NavState(const gtsam::Pose3 &pose, Vector v); +class NavState { + // Constructors + NavState(); + NavState(const gtsam::Rot3& R, const gtsam::Point3& t, Vector v); + NavState(const gtsam::Pose3& pose, Vector v); - // Testable - void print(string s) const; - bool equals(const gtsam::NavState &expected, double tol) const; + // Testable + void print(string s) const; + bool equals(const gtsam::NavState& expected, double tol) const; - // Access - gtsam::Rot3 attitude() const; - gtsam::Point3 position() const; - Vector velocity() const; - gtsam::Pose3 pose() const; - }; + // Access + gtsam::Rot3 attitude() const; + gtsam::Point3 position() const; + Vector velocity() const; + gtsam::Pose3 pose() const; +}; #include - virtual class PreintegratedRotationParams - { - PreintegratedRotationParams(); +virtual class PreintegratedRotationParams { + PreintegratedRotationParams(); - // Testable - void print(string s) const; - bool equals(const gtsam::PreintegratedRotationParams &expected, double tol); + // Testable + void print(string s) const; + bool equals(const gtsam::PreintegratedRotationParams& expected, double tol); - void setGyroscopeCovariance(Matrix cov); - void setOmegaCoriolis(Vector omega); - void setBodyPSensor(const gtsam::Pose3 &pose); + void setGyroscopeCovariance(Matrix cov); + void setOmegaCoriolis(Vector omega); + void setBodyPSensor(const gtsam::Pose3& pose); - Matrix getGyroscopeCovariance() const; + Matrix getGyroscopeCovariance() const; - // TODO(frank): allow optional - // boost::optional getOmegaCoriolis() const; - // boost::optional getBodyPSensor() const; - }; + // TODO(frank): allow optional + // boost::optional getOmegaCoriolis() const; + // boost::optional getBodyPSensor() const; +}; #include - virtual class PreintegrationParams : gtsam::PreintegratedRotationParams - { - PreintegrationParams(Vector n_gravity); +virtual class PreintegrationParams : gtsam::PreintegratedRotationParams { + PreintegrationParams(Vector n_gravity); - static gtsam::PreintegrationParams *MakeSharedD(double g); - static gtsam::PreintegrationParams *MakeSharedU(double g); - static gtsam::PreintegrationParams *MakeSharedD(); // default g = 9.81 - static gtsam::PreintegrationParams *MakeSharedU(); // default g = 9.81 + static gtsam::PreintegrationParams* MakeSharedD(double g); + static gtsam::PreintegrationParams* MakeSharedU(double g); + static gtsam::PreintegrationParams* MakeSharedD(); // default g = 9.81 + static gtsam::PreintegrationParams* MakeSharedU(); // default g = 9.81 - // Testable - void print(string s) const; - bool equals(const gtsam::PreintegrationParams &expected, double tol); + // Testable + void print(string s) const; + bool equals(const gtsam::PreintegrationParams& expected, double tol); - void setAccelerometerCovariance(Matrix cov); - void setIntegrationCovariance(Matrix cov); - void setUse2ndOrderCoriolis(bool flag); + void setAccelerometerCovariance(Matrix cov); + void setIntegrationCovariance(Matrix cov); + void setUse2ndOrderCoriolis(bool flag); - Matrix getAccelerometerCovariance() const; - Matrix getIntegrationCovariance() const; - bool getUse2ndOrderCoriolis() const; - }; + Matrix getAccelerometerCovariance() const; + Matrix getIntegrationCovariance() const; + bool getUse2ndOrderCoriolis() const; +}; #include - class PreintegratedImuMeasurements - { - // Constructors - PreintegratedImuMeasurements(const gtsam::PreintegrationParams *params); - PreintegratedImuMeasurements(const gtsam::PreintegrationParams *params, - const gtsam::imuBias::ConstantBias &bias); +class PreintegratedImuMeasurements { + // Constructors + PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params); + PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params, + const gtsam::imuBias::ConstantBias& bias); - // Testable - void print(string s) const; - bool equals(const gtsam::PreintegratedImuMeasurements &expected, double tol); + // Testable + void print(string s) const; + bool equals(const gtsam::PreintegratedImuMeasurements& expected, double tol); - // Standard Interface - void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, - double deltaT); - void resetIntegration(); - void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias &biasHat); + // Standard Interface + void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, + double deltaT); + void resetIntegration(); + void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias& biasHat); - Matrix preintMeasCov() const; - Vector preintegrated() const; - double deltaTij() const; - gtsam::Rot3 deltaRij() const; - Vector deltaPij() const; - Vector deltaVij() const; - gtsam::imuBias::ConstantBias biasHat() const; - Vector biasHatVector() const; - gtsam::NavState predict(const gtsam::NavState &state_i, - const gtsam::imuBias::ConstantBias &bias) const; - }; + Matrix preintMeasCov() const; + Vector preintegrated() const; + double deltaTij() const; + gtsam::Rot3 deltaRij() const; + Vector deltaPij() const; + Vector deltaVij() const; + gtsam::imuBias::ConstantBias biasHat() const; + Vector biasHatVector() const; + gtsam::NavState predict(const gtsam::NavState& state_i, + const gtsam::imuBias::ConstantBias& bias) const; +}; - virtual class ImuFactor : gtsam::NonlinearFactor - { - ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, - size_t bias, - const gtsam::PreintegratedImuMeasurements &preintegratedMeasurements); +virtual class ImuFactor: gtsam::NonlinearFactor { + ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, + size_t bias, + const gtsam::PreintegratedImuMeasurements& preintegratedMeasurements); - // Standard Interface - gtsam::PreintegratedImuMeasurements preintegratedMeasurements() const; - Vector evaluateError(const gtsam::Pose3 &pose_i, Vector vel_i, - const gtsam::Pose3 &pose_j, Vector vel_j, - const gtsam::imuBias::ConstantBias &bias); - }; + // Standard Interface + gtsam::PreintegratedImuMeasurements preintegratedMeasurements() const; + Vector evaluateError(const gtsam::Pose3& pose_i, Vector vel_i, + const gtsam::Pose3& pose_j, Vector vel_j, + const gtsam::imuBias::ConstantBias& bias); +}; #include - virtual class PreintegrationCombinedParams : gtsam::PreintegrationParams - { - PreintegrationCombinedParams(Vector n_gravity); +virtual class PreintegrationCombinedParams : gtsam::PreintegrationParams { + PreintegrationCombinedParams(Vector n_gravity); - static gtsam::PreintegrationCombinedParams *MakeSharedD(double g); - static gtsam::PreintegrationCombinedParams *MakeSharedU(double g); - static gtsam::PreintegrationCombinedParams *MakeSharedD(); // default g = 9.81 - static gtsam::PreintegrationCombinedParams *MakeSharedU(); // default g = 9.81 + static gtsam::PreintegrationCombinedParams* MakeSharedD(double g); + static gtsam::PreintegrationCombinedParams* MakeSharedU(double g); + static gtsam::PreintegrationCombinedParams* MakeSharedD(); // default g = 9.81 + static gtsam::PreintegrationCombinedParams* MakeSharedU(); // default g = 9.81 - // Testable - void print(string s) const; - bool equals(const gtsam::PreintegrationCombinedParams &expected, double tol); + // Testable + void print(string s) const; + bool equals(const gtsam::PreintegrationCombinedParams& expected, double tol); - void setBiasAccCovariance(Matrix cov); - void setBiasOmegaCovariance(Matrix cov); - void setBiasAccOmegaInt(Matrix cov); + void setBiasAccCovariance(Matrix cov); + void setBiasOmegaCovariance(Matrix cov); + void setBiasAccOmegaInt(Matrix cov); + + Matrix getBiasAccCovariance() const ; + Matrix getBiasOmegaCovariance() const ; + Matrix getBiasAccOmegaInt() const; + +}; - Matrix getBiasAccCovariance() const; - Matrix getBiasOmegaCovariance() const; - Matrix getBiasAccOmegaInt() const; - }; +class PreintegratedCombinedMeasurements { +// Constructors + PreintegratedCombinedMeasurements(const gtsam::PreintegrationCombinedParams* params); + PreintegratedCombinedMeasurements(const gtsam::PreintegrationCombinedParams* params, + const gtsam::imuBias::ConstantBias& bias); + // Testable + void print(string s) const; + bool equals(const gtsam::PreintegratedCombinedMeasurements& expected, + double tol); - class PreintegratedCombinedMeasurements - { - // Constructors - PreintegratedCombinedMeasurements(const gtsam::PreintegrationCombinedParams *params); - PreintegratedCombinedMeasurements(const gtsam::PreintegrationCombinedParams *params, - const gtsam::imuBias::ConstantBias &bias); - // Testable - void print(string s) const; - bool equals(const gtsam::PreintegratedCombinedMeasurements &expected, - double tol); + // Standard Interface + void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, + double deltaT); + void resetIntegration(); + void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias& biasHat); - // Standard Interface - void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, - double deltaT); - void resetIntegration(); - void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias &biasHat); + Matrix preintMeasCov() const; + double deltaTij() const; + gtsam::Rot3 deltaRij() const; + Vector deltaPij() const; + Vector deltaVij() const; + gtsam::imuBias::ConstantBias biasHat() const; + Vector biasHatVector() const; + gtsam::NavState predict(const gtsam::NavState& state_i, + const gtsam::imuBias::ConstantBias& bias) const; +}; - Matrix preintMeasCov() const; - double deltaTij() const; - gtsam::Rot3 deltaRij() const; - Vector deltaPij() const; - Vector deltaVij() const; - gtsam::imuBias::ConstantBias biasHat() const; - Vector biasHatVector() const; - gtsam::NavState predict(const gtsam::NavState &state_i, - const gtsam::imuBias::ConstantBias &bias) const; - }; +virtual class CombinedImuFactor: gtsam::NonlinearFactor { + CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, + size_t bias_i, size_t bias_j, + const gtsam::PreintegratedCombinedMeasurements& CombinedPreintegratedMeasurements); - virtual class CombinedImuFactor : gtsam::NonlinearFactor - { - CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, - size_t bias_i, size_t bias_j, - const gtsam::PreintegratedCombinedMeasurements &CombinedPreintegratedMeasurements); - - // Standard Interface - gtsam::PreintegratedCombinedMeasurements preintegratedMeasurements() const; - Vector evaluateError(const gtsam::Pose3 &pose_i, Vector vel_i, - const gtsam::Pose3 &pose_j, Vector vel_j, - const gtsam::imuBias::ConstantBias &bias_i, - const gtsam::imuBias::ConstantBias &bias_j); - }; + // Standard Interface + gtsam::PreintegratedCombinedMeasurements preintegratedMeasurements() const; + Vector evaluateError(const gtsam::Pose3& pose_i, Vector vel_i, + const gtsam::Pose3& pose_j, Vector vel_j, + const gtsam::imuBias::ConstantBias& bias_i, + const gtsam::imuBias::ConstantBias& bias_j); +}; #include - class PreintegratedAhrsMeasurements - { - // Standard Constructor - PreintegratedAhrsMeasurements(Vector bias, Matrix measuredOmegaCovariance); - PreintegratedAhrsMeasurements(const gtsam::PreintegratedAhrsMeasurements &rhs); +class PreintegratedAhrsMeasurements { + // Standard Constructor + PreintegratedAhrsMeasurements(Vector bias, Matrix measuredOmegaCovariance); + PreintegratedAhrsMeasurements(const gtsam::PreintegratedAhrsMeasurements& rhs); - // Testable - void print(string s) const; - bool equals(const gtsam::PreintegratedAhrsMeasurements &expected, double tol); + // Testable + void print(string s) const; + bool equals(const gtsam::PreintegratedAhrsMeasurements& expected, double tol); - // get Data - gtsam::Rot3 deltaRij() const; - double deltaTij() const; - Vector biasHat() const; + // get Data + gtsam::Rot3 deltaRij() const; + double deltaTij() const; + Vector biasHat() const; - // Standard Interface - void integrateMeasurement(Vector measuredOmega, double deltaT); - void resetIntegration(); - }; + // Standard Interface + void integrateMeasurement(Vector measuredOmega, double deltaT); + void resetIntegration() ; +}; - virtual class AHRSFactor : gtsam::NonlinearFactor - { - AHRSFactor(size_t rot_i, size_t rot_j, size_t bias, - const gtsam::PreintegratedAhrsMeasurements &preintegratedMeasurements, Vector omegaCoriolis); - AHRSFactor(size_t rot_i, size_t rot_j, size_t bias, - const gtsam::PreintegratedAhrsMeasurements &preintegratedMeasurements, Vector omegaCoriolis, - const gtsam::Pose3 &body_P_sensor); +virtual class AHRSFactor : gtsam::NonlinearFactor { + AHRSFactor(size_t rot_i, size_t rot_j,size_t bias, + const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, Vector omegaCoriolis); + AHRSFactor(size_t rot_i, size_t rot_j, size_t bias, + const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, Vector omegaCoriolis, + const gtsam::Pose3& body_P_sensor); - // Standard Interface - gtsam::PreintegratedAhrsMeasurements preintegratedMeasurements() const; - Vector evaluateError(const gtsam::Rot3 &rot_i, const gtsam::Rot3 &rot_j, - Vector bias) const; - gtsam::Rot3 predict(const gtsam::Rot3 &rot_i, Vector bias, - const gtsam::PreintegratedAhrsMeasurements &preintegratedMeasurements, - Vector omegaCoriolis) const; - }; + // Standard Interface + gtsam::PreintegratedAhrsMeasurements preintegratedMeasurements() const; + Vector evaluateError(const gtsam::Rot3& rot_i, const gtsam::Rot3& rot_j, + Vector bias) const; + gtsam::Rot3 predict(const gtsam::Rot3& rot_i, Vector bias, + const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, + Vector omegaCoriolis) const; +}; #include - //virtual class AttitudeFactor : gtsam::NonlinearFactor { - // AttitudeFactor(const Unit3& nZ, const Unit3& bRef); - // AttitudeFactor(); - //}; - virtual class Rot3AttitudeFactor : gtsam::NonlinearFactor - { - Rot3AttitudeFactor(size_t key, const gtsam::Unit3 &nZ, const gtsam::noiseModel::Diagonal *model, - const gtsam::Unit3 &bRef); - Rot3AttitudeFactor(size_t key, const gtsam::Unit3 &nZ, const gtsam::noiseModel::Diagonal *model); - Rot3AttitudeFactor(); - void print(string s) const; - bool equals(const gtsam::NonlinearFactor &expected, double tol) const; - gtsam::Unit3 nZ() const; - gtsam::Unit3 bRef() const; - }; +//virtual class AttitudeFactor : gtsam::NonlinearFactor { +// AttitudeFactor(const Unit3& nZ, const Unit3& bRef); +// AttitudeFactor(); +//}; +virtual class Rot3AttitudeFactor : gtsam::NonlinearFactor{ + Rot3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model, + const gtsam::Unit3& bRef); + Rot3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model); + Rot3AttitudeFactor(); + void print(string s) const; + bool equals(const gtsam::NonlinearFactor& expected, double tol) const; + gtsam::Unit3 nZ() const; + gtsam::Unit3 bRef() const; +}; - virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor - { - Pose3AttitudeFactor(size_t key, const gtsam::Unit3 &nZ, - const gtsam::noiseModel::Diagonal *model, - const gtsam::Unit3 &bRef); - Pose3AttitudeFactor(size_t key, const gtsam::Unit3 &nZ, - const gtsam::noiseModel::Diagonal *model); - Pose3AttitudeFactor(); - void print(string s) const; - bool equals(const gtsam::NonlinearFactor &expected, double tol) const; - gtsam::Unit3 nZ() const; - gtsam::Unit3 bRef() const; - }; +virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor { + Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, + const gtsam::noiseModel::Diagonal* model, + const gtsam::Unit3& bRef); + Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, + const gtsam::noiseModel::Diagonal* model); + Pose3AttitudeFactor(); + void print(string s) const; + bool equals(const gtsam::NonlinearFactor& expected, double tol) const; + gtsam::Unit3 nZ() const; + gtsam::Unit3 bRef() const; +}; #include - virtual class GPSFactor : gtsam::NonlinearFactor - { - GPSFactor(size_t key, const gtsam::Point3 &gpsIn, - const gtsam::noiseModel::Base *model); +virtual class GPSFactor : gtsam::NonlinearFactor{ + GPSFactor(size_t key, const gtsam::Point3& gpsIn, + const gtsam::noiseModel::Base* model); - // Testable - void print(string s) const; - bool equals(const gtsam::GPSFactor &expected, double tol); + // Testable + void print(string s) const; + bool equals(const gtsam::GPSFactor& expected, double tol); - // Standard Interface - gtsam::Point3 measurementIn() const; - }; + // Standard Interface + gtsam::Point3 measurementIn() const; +}; - virtual class GPSFactor2 : gtsam::NonlinearFactor - { - GPSFactor2(size_t key, const gtsam::Point3 &gpsIn, - const gtsam::noiseModel::Base *model); +virtual class GPSFactor2 : gtsam::NonlinearFactor { + GPSFactor2(size_t key, const gtsam::Point3& gpsIn, + const gtsam::noiseModel::Base* model); - // Testable - void print(string s) const; - bool equals(const gtsam::GPSFactor2 &expected, double tol); + // Testable + void print(string s) const; + bool equals(const gtsam::GPSFactor2& expected, double tol); - // Standard Interface - gtsam::Point3 measurementIn() const; - }; + // Standard Interface + gtsam::Point3 measurementIn() const; +}; #include - virtual class Scenario - { - gtsam::Pose3 pose(double t) const; - Vector omega_b(double t) const; - Vector velocity_n(double t) const; - Vector acceleration_n(double t) const; - gtsam::Rot3 rotation(double t) const; - gtsam::NavState navState(double t) const; - Vector velocity_b(double t) const; - Vector acceleration_b(double t) const; - }; +virtual class Scenario { + gtsam::Pose3 pose(double t) const; + Vector omega_b(double t) const; + Vector velocity_n(double t) const; + Vector acceleration_n(double t) const; + gtsam::Rot3 rotation(double t) const; + gtsam::NavState navState(double t) const; + Vector velocity_b(double t) const; + Vector acceleration_b(double t) const; +}; - virtual class ConstantTwistScenario : gtsam::Scenario - { - ConstantTwistScenario(Vector w, Vector v); - ConstantTwistScenario(Vector w, Vector v, - const gtsam::Pose3 &nTb0); - }; +virtual class ConstantTwistScenario : gtsam::Scenario { + ConstantTwistScenario(Vector w, Vector v); + ConstantTwistScenario(Vector w, Vector v, + const gtsam::Pose3& nTb0); +}; - virtual class AcceleratingScenario : gtsam::Scenario - { - AcceleratingScenario(const gtsam::Rot3 &nRb, const gtsam::Point3 &p0, - Vector v0, Vector a_n, - Vector omega_b); - }; +virtual class AcceleratingScenario : gtsam::Scenario { + AcceleratingScenario(const gtsam::Rot3& nRb, const gtsam::Point3& p0, + Vector v0, Vector a_n, + Vector omega_b); +}; #include - class ScenarioRunner - { - ScenarioRunner(const gtsam::Scenario &scenario, - const gtsam::PreintegrationParams *p, - double imuSampleTime, - const gtsam::imuBias::ConstantBias &bias); - Vector gravity_n() const; - Vector actualAngularVelocity(double t) const; - Vector actualSpecificForce(double t) const; - Vector measuredAngularVelocity(double t) const; - Vector measuredSpecificForce(double t) const; - double imuSampleTime() const; - gtsam::PreintegratedImuMeasurements integrate( - double T, const gtsam::imuBias::ConstantBias &estimatedBias, - bool corrupted) const; - gtsam::NavState predict( - const gtsam::PreintegratedImuMeasurements &pim, - const gtsam::imuBias::ConstantBias &estimatedBias) const; - Matrix estimateCovariance( - double T, size_t N, - const gtsam::imuBias::ConstantBias &estimatedBias) const; - Matrix estimateNoiseCovariance(size_t N) const; - }; +class ScenarioRunner { + ScenarioRunner(const gtsam::Scenario& scenario, + const gtsam::PreintegrationParams* p, + double imuSampleTime, + const gtsam::imuBias::ConstantBias& bias); + Vector gravity_n() const; + Vector actualAngularVelocity(double t) const; + Vector actualSpecificForce(double t) const; + Vector measuredAngularVelocity(double t) const; + Vector measuredSpecificForce(double t) const; + double imuSampleTime() const; + gtsam::PreintegratedImuMeasurements integrate( + double T, const gtsam::imuBias::ConstantBias& estimatedBias, + bool corrupted) const; + gtsam::NavState predict( + const gtsam::PreintegratedImuMeasurements& pim, + const gtsam::imuBias::ConstantBias& estimatedBias) const; + Matrix estimateCovariance( + double T, size_t N, + const gtsam::imuBias::ConstantBias& estimatedBias) const; + Matrix estimateNoiseCovariance(size_t N) const; +}; - //************************************************************************* - // Utilities - //************************************************************************* +//************************************************************************* +// Utilities +//************************************************************************* - namespace utilities - { +namespace utilities { + + #include + gtsam::KeyList createKeyList(Vector I); + gtsam::KeyList createKeyList(string s, Vector I); + gtsam::KeyVector createKeyVector(Vector I); + gtsam::KeyVector createKeyVector(string s, Vector I); + gtsam::KeySet createKeySet(Vector I); + gtsam::KeySet createKeySet(string s, Vector I); + Matrix extractPoint2(const gtsam::Values& values); + Matrix extractPoint3(const gtsam::Values& values); + gtsam::Values allPose2s(gtsam::Values& values); + Matrix extractPose2(const gtsam::Values& values); + gtsam::Values allPose3s(gtsam::Values& values); + Matrix extractPose3(const gtsam::Values& values); + void perturbPoint2(gtsam::Values& values, double sigma, int seed); + void perturbPose2 (gtsam::Values& values, double sigmaT, double sigmaR, int seed); + void perturbPoint3(gtsam::Values& values, double sigma, int seed); + void insertBackprojections(gtsam::Values& values, const gtsam::PinholeCameraCal3_S2& c, Vector J, Matrix Z, double depth); + void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K); + void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K, const gtsam::Pose3& body_P_sensor); + Matrix reprojectionErrors(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values); + gtsam::Values localToWorld(const gtsam::Values& local, const gtsam::Pose2& base); + gtsam::Values localToWorld(const gtsam::Values& local, const gtsam::Pose2& base, const gtsam::KeyVector& keys); + +} //\namespace utilities #include - gtsam::KeyList createKeyList(Vector I); - gtsam::KeyList createKeyList(string s, Vector I); - gtsam::KeyVector createKeyVector(Vector I); - gtsam::KeyVector createKeyVector(string s, Vector I); - gtsam::KeySet createKeySet(Vector I); - gtsam::KeySet createKeySet(string s, Vector I); - Matrix extractPoint2(const gtsam::Values &values); - Matrix extractPoint3(const gtsam::Values &values); - gtsam::Values allPose2s(gtsam::Values &values); - Matrix extractPose2(const gtsam::Values &values); - gtsam::Values allPose3s(gtsam::Values &values); - Matrix extractPose3(const gtsam::Values &values); - void perturbPoint2(gtsam::Values &values, double sigma, int seed); - void perturbPose2(gtsam::Values &values, double sigmaT, double sigmaR, int seed); - void perturbPoint3(gtsam::Values &values, double sigma, int seed); - void insertBackprojections(gtsam::Values &values, const gtsam::PinholeCameraCal3_S2 &c, Vector J, Matrix Z, double depth); - void insertProjectionFactors(gtsam::NonlinearFactorGraph &graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base *model, const gtsam::Cal3_S2 *K); - void insertProjectionFactors(gtsam::NonlinearFactorGraph &graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base *model, const gtsam::Cal3_S2 *K, const gtsam::Pose3 &body_P_sensor); - Matrix reprojectionErrors(const gtsam::NonlinearFactorGraph &graph, const gtsam::Values &values); - gtsam::Values localToWorld(const gtsam::Values &local, const gtsam::Pose2 &base); - gtsam::Values localToWorld(const gtsam::Values &local, const gtsam::Pose2 &base, const gtsam::KeyVector &keys); +class RedirectCout { + RedirectCout(); + string str(); +}; - } // namespace utilities - -#include - class RedirectCout - { - RedirectCout(); - string str(); - }; - -} // namespace gtsam +} diff --git a/python/gtsam/preamble.h b/python/gtsam/preamble.h index fa98cd171..c8a577431 100644 --- a/python/gtsam/preamble.h +++ b/python/gtsam/preamble.h @@ -5,10 +5,10 @@ PYBIND11_MAKE_OPAQUE(std::vector>); #else PYBIND11_MAKE_OPAQUE(std::vector); #endif -PYBIND11_MAKE_OPAQUE(std::vector>); +PYBIND11_MAKE_OPAQUE(std::vector >); PYBIND11_MAKE_OPAQUE(std::vector); -PYBIND11_MAKE_OPAQUE(std::vector>>); -PYBIND11_MAKE_OPAQUE(std::vector>>); +PYBIND11_MAKE_OPAQUE(std::vector > >); +PYBIND11_MAKE_OPAQUE(std::vector > >); PYBIND11_MAKE_OPAQUE(std::vector); -PYBIND11_MAKE_OPAQUE(gtsam::CameraSet>); -PYBIND11_MAKE_OPAQUE(gtsam::CameraSet>); \ No newline at end of file +PYBIND11_MAKE_OPAQUE(gtsam::CameraSet >); +PYBIND11_MAKE_OPAQUE(gtsam::CameraSet >); \ No newline at end of file diff --git a/python/gtsam/specializations.h b/python/gtsam/specializations.h index 63694f6f4..431697aac 100644 --- a/python/gtsam/specializations.h +++ b/python/gtsam/specializations.h @@ -1,17 +1,17 @@ // Please refer to: https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html // These are required to save one copy operation on Python calls #ifdef GTSAM_ALLOCATOR_TBB -py::bind_vector>>(m_, "KeyVector"); +py::bind_vector > >(m_, "KeyVector"); #else -py::bind_vector>(m_, "KeyVector"); +py::bind_vector >(m_, "KeyVector"); #endif -py::bind_vector>>(m_, "Point2Vector"); -py::bind_vector>(m_, "Pose3Vector"); -py::bind_vector>>>(m_, "BetweenFactorPose3s"); -py::bind_vector>>>(m_, "BetweenFactorPose2s"); -py::bind_vector>>(m_, "BinaryMeasurementsUnit3"); +py::bind_vector > >(m_, "Point2Vector"); +py::bind_vector >(m_, "Pose3Vector"); +py::bind_vector > > >(m_, "BetweenFactorPose3s"); +py::bind_vector > > >(m_, "BetweenFactorPose2s"); +py::bind_vector > >(m_, "BinaryMeasurementsUnit3"); py::bind_map(m_, "IndexPairSetMap"); py::bind_vector(m_, "IndexPairVector"); py::bind_map(m_, "KeyPairDoubleMap"); -py::bind_vector>>(m_, "CameraSetCal3_S2"); -py::bind_vector>>(m_, "CameraSetCal3Bundler"); +py::bind_vector > >(m_, "CameraSetCal3_S2"); +py::bind_vector > >(m_, "CameraSetCal3Bundler"); diff --git a/python/gtsam/tests/test_Triangulation.py b/python/gtsam/tests/test_Triangulation.py index 901aad0b9..574452afa 100644 --- a/python/gtsam/tests/test_Triangulation.py +++ b/python/gtsam/tests/test_Triangulation.py @@ -14,10 +14,11 @@ import numpy as np import gtsam as g from gtsam.utils.test_case import GtsamTestCase -from gtsam import Cal3_S2, Cal3Bundler, CameraSetCal3_S2,\ - CameraSetCal3Bundler, PinholeCameraCal3_S2, PinholeCameraCal3Bundler, \ - Point3, Pose3, Point2Vector, Pose3Vector, Rot3, triangulatePoint3 - +from gtsam import Cal3_S2, Cal3Bundler, Rot3, Pose3, \ + PinholeCameraCal3_S2, PinholeCameraCal3Bundler, Point3, \ + Point2Vector, Pose3Vector, triangulatePoint3, \ + CameraSetCal3_S2, CameraSetCal3Bundler +from numpy.core.records import array class TestVisualISAMExample(GtsamTestCase): """ Tests for triangulation with shared and individual calibrations """ From a7248163e8d51d97885fdc0949c2d703d922ede8 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 5 Dec 2020 18:09:56 -0500 Subject: [PATCH 15/17] format python triangulation tests --- python/gtsam/tests/test_Triangulation.py | 45 ++++++++++++++---------- 1 file changed, 26 insertions(+), 19 deletions(-) diff --git a/python/gtsam/tests/test_Triangulation.py b/python/gtsam/tests/test_Triangulation.py index 574452afa..b54c05ce1 100644 --- a/python/gtsam/tests/test_Triangulation.py +++ b/python/gtsam/tests/test_Triangulation.py @@ -10,16 +10,16 @@ Author: Frank Dellaert & Fan Jiang (Python) """ import unittest -import numpy as np - import gtsam as g +import numpy as np +from gtsam import (Cal3_S2, Cal3Bundler, CameraSetCal3_S2, + CameraSetCal3Bundler, PinholeCameraCal3_S2, + PinholeCameraCal3Bundler, Point2Vector, Point3, Pose3, + Pose3Vector, Rot3, triangulatePoint3) from gtsam.utils.test_case import GtsamTestCase -from gtsam import Cal3_S2, Cal3Bundler, Rot3, Pose3, \ - PinholeCameraCal3_S2, PinholeCameraCal3Bundler, Point3, \ - Point2Vector, Pose3Vector, triangulatePoint3, \ - CameraSetCal3_S2, CameraSetCal3Bundler from numpy.core.records import array + class TestVisualISAMExample(GtsamTestCase): """ Tests for triangulation with shared and individual calibrations """ @@ -38,7 +38,7 @@ class TestVisualISAMExample(GtsamTestCase): # landmark ~5 meters infront of camera self.landmark = Point3(5, 0.5, 1.2) - + def generate_measurements(self, calibration, camera_model, camera_set, *cal_params): """ Generate vector of measurements for given calibration and camera model Args: @@ -53,26 +53,27 @@ class TestVisualISAMExample(GtsamTestCase): cameras = camera_set() else: cameras = [] - measurements = Point2Vector() + measurements = Point2Vector() for k, pose in zip(cal_params, self.poses): K = calibration(*k) camera = camera_model(pose, K) cameras.append(camera) z = camera.project(self.landmark) - measurements.append(z) + measurements.append(z) return measurements, cameras - def test_TriangulationExample(self): """ Tests triangulation with shared Cal3_S2 calibration""" # Some common constants - sharedCal = (1500, 1200, 0, 640, 480) + sharedCal = (1500, 1200, 0, 640, 480) - measurements, _ = self.generate_measurements(Cal3_S2, PinholeCameraCal3_S2, None, sharedCal, sharedCal) + measurements, _ = self.generate_measurements( + Cal3_S2, PinholeCameraCal3_S2, None, sharedCal, sharedCal) - triangulated_landmark = triangulatePoint3(self.poses, Cal3_S2(sharedCal), measurements, rank_tol=1e-9, optimize=True) + triangulated_landmark = triangulatePoint3(self.poses, Cal3_S2( + sharedCal), measurements, rank_tol=1e-9, optimize=True) self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9) # Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) @@ -80,7 +81,8 @@ class TestVisualISAMExample(GtsamTestCase): measurements_noisy.append(measurements[0] - np.array([0.1, 0.5])) measurements_noisy.append(measurements[1] - np.array([-0.2, 0.3])) - triangulated_landmark = triangulatePoint3(self.poses, Cal3_S2(sharedCal), measurements_noisy, rank_tol=1e-9, optimize=True) + triangulated_landmark = triangulatePoint3(self.poses, Cal3_S2( + sharedCal), measurements_noisy, rank_tol=1e-9, optimize=True) self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-2) def test_distinct_Ks(self): @@ -89,9 +91,11 @@ class TestVisualISAMExample(GtsamTestCase): K1 = (1500, 1200, 0, 640, 480) K2 = (1600, 1300, 0, 650, 440) - measurements, cameras = self.generate_measurements(Cal3_S2, PinholeCameraCal3_S2, CameraSetCal3_S2, K1, K2) + measurements, cameras = self.generate_measurements( + Cal3_S2, PinholeCameraCal3_S2, CameraSetCal3_S2, K1, K2) - triangulated_landmark = triangulatePoint3(cameras, measurements, rank_tol=1e-9, optimize=True) + triangulated_landmark = triangulatePoint3( + cameras, measurements, rank_tol=1e-9, optimize=True) self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9) def test_distinct_Ks_Bundler(self): @@ -100,10 +104,13 @@ class TestVisualISAMExample(GtsamTestCase): K1 = (1500, 0, 0, 640, 480) K2 = (1600, 0, 0, 650, 440) - measurements, cameras = self.generate_measurements(Cal3Bundler, PinholeCameraCal3Bundler, CameraSetCal3Bundler, K1, K2) + measurements, cameras = self.generate_measurements( + Cal3Bundler, PinholeCameraCal3Bundler, CameraSetCal3Bundler, K1, K2) + + triangulated_landmark = triangulatePoint3( + cameras, measurements, rank_tol=1e-9, optimize=True) + self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9) - triangulated_landmark = triangulatePoint3(cameras, measurements, rank_tol=1e-9, optimize=True) - self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9) if __name__ == "__main__": unittest.main() From d05f360c1146d0af3a90df30629df2f89eddefe0 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 5 Dec 2020 18:15:53 -0500 Subject: [PATCH 16/17] more formatting --- python/gtsam/preamble.h | 2 +- python/gtsam/tests/test_Triangulation.py | 46 ++++++++++++++++-------- 2 files changed, 32 insertions(+), 16 deletions(-) diff --git a/python/gtsam/preamble.h b/python/gtsam/preamble.h index c8a577431..b56766c72 100644 --- a/python/gtsam/preamble.h +++ b/python/gtsam/preamble.h @@ -11,4 +11,4 @@ PYBIND11_MAKE_OPAQUE(std::vector > >); PYBIND11_MAKE_OPAQUE(std::vector); PYBIND11_MAKE_OPAQUE(gtsam::CameraSet >); -PYBIND11_MAKE_OPAQUE(gtsam::CameraSet >); \ No newline at end of file +PYBIND11_MAKE_OPAQUE(gtsam::CameraSet >); diff --git a/python/gtsam/tests/test_Triangulation.py b/python/gtsam/tests/test_Triangulation.py index b54c05ce1..6fdc4d148 100644 --- a/python/gtsam/tests/test_Triangulation.py +++ b/python/gtsam/tests/test_Triangulation.py @@ -39,12 +39,14 @@ class TestVisualISAMExample(GtsamTestCase): # landmark ~5 meters infront of camera self.landmark = Point3(5, 0.5, 1.2) - def generate_measurements(self, calibration, camera_model, camera_set, *cal_params): - """ Generate vector of measurements for given calibration and camera model + def generate_measurements(self, calibration, camera_model, cal_params, camera_set=None): + """ + Generate vector of measurements for given calibration and camera model. + Args: calibration: Camera calibration e.g. Cal3_S2 camera_model: Camera model e.g. PinholeCameraCal3_S2 - cal_params: (list of) camera parameters e.g. K1, K2 + cal_params: Iterable of camera parameters for `calibration` e.g. [K1, K2] camera_set: Cameraset object (for individual calibrations) Returns: list of measurements and list/CameraSet object for cameras @@ -69,11 +71,15 @@ class TestVisualISAMExample(GtsamTestCase): # Some common constants sharedCal = (1500, 1200, 0, 640, 480) - measurements, _ = self.generate_measurements( - Cal3_S2, PinholeCameraCal3_S2, None, sharedCal, sharedCal) + measurements, _ = self.generate_measurements(Cal3_S2, + PinholeCameraCal3_S2, + (sharedCal, sharedCal)) - triangulated_landmark = triangulatePoint3(self.poses, Cal3_S2( - sharedCal), measurements, rank_tol=1e-9, optimize=True) + triangulated_landmark = triangulatePoint3(self.poses, + Cal3_S2(sharedCal), + measurements, + rank_tol=1e-9, + optimize=True) self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9) # Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) @@ -81,8 +87,12 @@ class TestVisualISAMExample(GtsamTestCase): measurements_noisy.append(measurements[0] - np.array([0.1, 0.5])) measurements_noisy.append(measurements[1] - np.array([-0.2, 0.3])) - triangulated_landmark = triangulatePoint3(self.poses, Cal3_S2( - sharedCal), measurements_noisy, rank_tol=1e-9, optimize=True) + triangulated_landmark = triangulatePoint3(self.poses, + Cal3_S2(sharedCal), + measurements_noisy, + rank_tol=1e-9, + optimize=True) + self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-2) def test_distinct_Ks(self): @@ -91,8 +101,10 @@ class TestVisualISAMExample(GtsamTestCase): K1 = (1500, 1200, 0, 640, 480) K2 = (1600, 1300, 0, 650, 440) - measurements, cameras = self.generate_measurements( - Cal3_S2, PinholeCameraCal3_S2, CameraSetCal3_S2, K1, K2) + measurements, cameras = self.generate_measurements(Cal3_S2, + PinholeCameraCal3_S2, + (K1, K2), + camera_set=CameraSetCal3_S2) triangulated_landmark = triangulatePoint3( cameras, measurements, rank_tol=1e-9, optimize=True) @@ -104,11 +116,15 @@ class TestVisualISAMExample(GtsamTestCase): K1 = (1500, 0, 0, 640, 480) K2 = (1600, 0, 0, 650, 440) - measurements, cameras = self.generate_measurements( - Cal3Bundler, PinholeCameraCal3Bundler, CameraSetCal3Bundler, K1, K2) + measurements, cameras = self.generate_measurements(Cal3Bundler, + PinholeCameraCal3Bundler, + (K1, K2), + camera_set=CameraSetCal3Bundler) - triangulated_landmark = triangulatePoint3( - cameras, measurements, rank_tol=1e-9, optimize=True) + triangulated_landmark = triangulatePoint3(cameras, + measurements, + rank_tol=1e-9, + optimize=True) self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9) From 3da28858317bf46cea8b3cb21abb17e2674c7fce Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 5 Dec 2020 18:18:30 -0500 Subject: [PATCH 17/17] remove unused imports --- python/gtsam/tests/test_Triangulation.py | 40 +++++++++++++----------- 1 file changed, 21 insertions(+), 19 deletions(-) diff --git a/python/gtsam/tests/test_Triangulation.py b/python/gtsam/tests/test_Triangulation.py index 6fdc4d148..399cf019d 100644 --- a/python/gtsam/tests/test_Triangulation.py +++ b/python/gtsam/tests/test_Triangulation.py @@ -10,14 +10,14 @@ Author: Frank Dellaert & Fan Jiang (Python) """ import unittest -import gtsam as g import numpy as np + +import gtsam from gtsam import (Cal3_S2, Cal3Bundler, CameraSetCal3_S2, CameraSetCal3Bundler, PinholeCameraCal3_S2, PinholeCameraCal3Bundler, Point2Vector, Point3, Pose3, - Pose3Vector, Rot3, triangulatePoint3) + Pose3Vector, Rot3) from gtsam.utils.test_case import GtsamTestCase -from numpy.core.records import array class TestVisualISAMExample(GtsamTestCase): @@ -75,11 +75,11 @@ class TestVisualISAMExample(GtsamTestCase): PinholeCameraCal3_S2, (sharedCal, sharedCal)) - triangulated_landmark = triangulatePoint3(self.poses, - Cal3_S2(sharedCal), - measurements, - rank_tol=1e-9, - optimize=True) + triangulated_landmark = gtsam.triangulatePoint3(self.poses, + Cal3_S2(sharedCal), + measurements, + rank_tol=1e-9, + optimize=True) self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9) # Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) @@ -87,11 +87,11 @@ class TestVisualISAMExample(GtsamTestCase): measurements_noisy.append(measurements[0] - np.array([0.1, 0.5])) measurements_noisy.append(measurements[1] - np.array([-0.2, 0.3])) - triangulated_landmark = triangulatePoint3(self.poses, - Cal3_S2(sharedCal), - measurements_noisy, - rank_tol=1e-9, - optimize=True) + triangulated_landmark = gtsam.triangulatePoint3(self.poses, + Cal3_S2(sharedCal), + measurements_noisy, + rank_tol=1e-9, + optimize=True) self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-2) @@ -106,8 +106,10 @@ class TestVisualISAMExample(GtsamTestCase): (K1, K2), camera_set=CameraSetCal3_S2) - triangulated_landmark = triangulatePoint3( - cameras, measurements, rank_tol=1e-9, optimize=True) + triangulated_landmark = gtsam.triangulatePoint3(cameras, + measurements, + rank_tol=1e-9, + optimize=True) self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9) def test_distinct_Ks_Bundler(self): @@ -121,10 +123,10 @@ class TestVisualISAMExample(GtsamTestCase): (K1, K2), camera_set=CameraSetCal3Bundler) - triangulated_landmark = triangulatePoint3(cameras, - measurements, - rank_tol=1e-9, - optimize=True) + triangulated_landmark = gtsam.triangulatePoint3(cameras, + measurements, + rank_tol=1e-9, + optimize=True) self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9)