From 809923a6f2e5b6713124f8a0f5f8981a8e8f7d05 Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Fri, 15 Jul 2022 23:32:03 -0400 Subject: [PATCH 001/118] Start GTSFM's DSF port to C++ --- gtsam/sfm/DsfTrackGenerator.h | 180 +++++++++++++++++++++++++++++ gtsam/sfm/sfm.i | 47 ++++++++ python/CMakeLists.txt | 11 +- python/gtsam/preamble/sfm.h | 5 + python/gtsam/specializations/sfm.h | 6 + 5 files changed, 246 insertions(+), 3 deletions(-) create mode 100644 gtsam/sfm/DsfTrackGenerator.h diff --git a/gtsam/sfm/DsfTrackGenerator.h b/gtsam/sfm/DsfTrackGenerator.h new file mode 100644 index 000000000..5a3e6661e --- /dev/null +++ b/gtsam/sfm/DsfTrackGenerator.h @@ -0,0 +1,180 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file DsfTrackGenerator.h + * @date May 2022 + * @author John Lambert + * @brief Identifies connected components in the keypoint matched graph. + */ + +#pragma once +#include +#include + +#include + +#include +#include +#include +#include +#include + +namespace gtsam { + +typedef DSFMap DSFMapIndexPair; +typedef std::pair ImagePair; +typedef Eigen::MatrixX2i CorrespondenceIndices; // N x 2 array + + +using KeypointCoordinates = Eigen::MatrixX2d; + +struct Keypoints +{ + KeypointCoordinates coordinates; + // typedef'd for Eigen::VectorXd + boost::optional scales; + boost::optional responses; + + Keypoints(const gtsam::KeypointCoordinates& coordinates): coordinates(coordinates) {}; // boost::none +}; + + + + + +// TODO: THIS CLASSNAME IS DUPLICATED in SfmTrack.h, conflicts +// @param camera index +// @param 2d measurement +// Implemented as named tuple, instead of std::pair +struct NamedSfmMeasurement +{ + size_t i; + Point2 uv; + + NamedSfmMeasurement(size_t i, Point2 uv) : i(i), uv(uv) {} +}; + + +class SfmTrack2d +{ + private: + std::vector measurements_; + + public: + void addMeasurement(const NamedSfmMeasurement &m) { + measurements_.emplace_back(m); + } + std::vector measurements() {return measurements_; } + + // @brief Validates the track by checking that no two measurements are from the same camera. + // + // returns boolean result of the validation. + bool validate_unique_cameras() + { + std::vector track_cam_idxs; + for (auto & measurement: measurements_) + { + track_cam_idxs.emplace_back(measurement.i); + } + auto i = std::adjacent_find(track_cam_idxs.begin(), track_cam_idxs.end()); + bool all_cameras_unique = (i == track_cam_idxs.end()); + return all_cameras_unique; + } +}; + +using MatchIndicesMap = std::map; +using KeypointsList = std::vector; +using KeypointsVector = std::vector; // TODO: prefer KeypointsSet? + + +class DsfTrackGenerator +{ + + public: + DsfTrackGenerator() {} + + // Creates a list of tracks from 2d point correspondences. + // + // Creates a disjoint-set forest (DSF) and 2d tracks from pairwise matches. We create a singleton for union-find + // set elements from camera index of a detection and the index of that detection in that camera's keypoint list, + // i.e. (i,k). + std::vector generate_tracks_from_pairwise_matches( + const MatchIndicesMap matches_dict, + const KeypointsList keypoints_list) + { + std::vector track_2d_list; + + // TODO: put these checks into the Keypoints class. + // check to ensure dimensions of coordinates are correct + //dims_valid = all([kps.coordinates.ndim == 2 for kps in keypoints_list]) + //if not dims_valid: + // raise Exception("Dimensions for Keypoint coordinates incorrect. Array needs to be 2D") + + std::cout << "[SfmTrack2d] Starting Union-Find..." << std::endl; + // Generate the DSF to form tracks + DSFMapIndexPair dsf; + + // for DSF finally + // measurement_idxs represented by k's + for (const auto& [pair_idxs, corr_idxs]: matches_dict) { + size_t i1 = pair_idxs.first; + size_t i2 = pair_idxs.second; + for (size_t k = 0; k < corr_idxs.rows(); k++) + { + size_t k1 = corr_idxs(k,0); + size_t k2 = corr_idxs(k,1); + dsf.merge(IndexPair(i1, k1), IndexPair(i2, k2)); + } + } + + std::cout << "[SfmTrack2d] Union-Find Complete" << std::endl; + const std::map > key_sets = dsf.sets(); + + size_t erroneous_track_count = 0; + // create a landmark map: a list of tracks + // Each track will be represented as a list of (camera_idx, measurements) + for (const auto& [set_id, index_pair_set]: key_sets) { + // Initialize track from measurements + SfmTrack2d track_2d; + + for (const auto& index_pair: index_pair_set) + { + // camera_idx is represented by i + // measurement_idx is represented by k + size_t i = index_pair.i(); + size_t k = index_pair.j(); + // add measurement in this track + track_2d.addMeasurement(NamedSfmMeasurement(i, keypoints_list[i].coordinates.row(k))); + } + + // Skip erroneous track that had repeated measurements within the same image + // This is an expected result from an incorrect correspondence slipping through + if (track_2d.validate_unique_cameras()) + { + track_2d_list.emplace_back(track_2d); + } else { + erroneous_track_count += 1; + } + } + + double erroneous_track_pct = key_sets.size() > 0 ? erroneous_track_count / key_sets.size() * 100 : std::nan("1"); + // TODO: restrict decimal places to 2 decimals. + std::cout << "DSF Union-Find: " << erroneous_track_pct; + std::cout << "% of tracks discarded from multiple obs. in a single image." << std::endl; + + // TODO: return the Transitivity failure percentage here. + return track_2d_list; + } +}; + +} // namespace gtsam + diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index 83bd07b13..84988f06c 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -4,6 +4,53 @@ namespace gtsam { +#include +class GtsfmData +{ + GtsfmData(const int numberImages); + void write_points(std::vector& images, const std::string save_dir); + void write_images(std::vector& images, const std::string save_dir); +}; + +#include + +class MatchIndicesMap { + MatchIndicesMap(); + MatchIndicesMap(const gtsam::MatchIndicesMap& other); + + size_t size() const; + bool empty() const; + void clear(); + gtsam::CorrespondenceIndices at(const pair& keypair) const; +}; + + +class KeypointsList { + KeypointsList(); + KeypointsList(const gtsam::KeypointsList& other); + void push_back(const gtsam::Keypoints& keypoints); + size_t size() const; + bool empty() const; + void clear(); + gtsam::Keypoints at(const size_t& index) const; +}; + + +class Keypoints +{ + Keypoints(const gtsam::KeypointCoordinates& coordinates); + gtsam::KeypointCoordinates coordinates; +}; // check if this should be a method + + +class DsfTrackGenerator { + DsfTrackGenerator(); + std::vector generate_tracks_from_pairwise_matches( + const gtsam::MatchIndicesMap matches_dict, + const gtsam::KeypointsList keypoints_list); +}; + + #include #include #include diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index c14f02dda..91cb75edd 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -51,7 +51,9 @@ set(ignore gtsam::BinaryMeasurementsUnit3 gtsam::BinaryMeasurementsRot3 gtsam::DiscreteKey - gtsam::KeyPairDoubleMap) + gtsam::KeyPairDoubleMap + gtsam::MatchIndicesMap + gtsam::KeypointsList) set(interface_headers ${PROJECT_SOURCE_DIR}/gtsam/gtsam.i @@ -145,8 +147,11 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) gtsam::CameraSetCal3Bundler gtsam::CameraSetCal3Unified gtsam::CameraSetCal3Fisheye - gtsam::KeyPairDoubleMap) - + gtsam::KeyPairDoubleMap + gtsam::MatchIndicesMap + gtsam::KeypointsList) + + pybind_wrap(${GTSAM_PYTHON_UNSTABLE_TARGET} # target ${PROJECT_SOURCE_DIR}/gtsam_unstable/gtsam_unstable.i # interface_header "gtsam_unstable.cpp" # generated_cpp diff --git a/python/gtsam/preamble/sfm.h b/python/gtsam/preamble/sfm.h index 8ff0ea82e..a51e963dd 100644 --- a/python/gtsam/preamble/sfm.h +++ b/python/gtsam/preamble/sfm.h @@ -24,3 +24,8 @@ PYBIND11_MAKE_OPAQUE( std::vector>); PYBIND11_MAKE_OPAQUE( std::vector>); +PYBIND11_MAKE_OPAQUE( + std::vector); + + +PYBIND11_MAKE_OPAQUE(gtsam::MatchIndicesMap); \ No newline at end of file diff --git a/python/gtsam/specializations/sfm.h b/python/gtsam/specializations/sfm.h index 311b2c59b..297aee262 100644 --- a/python/gtsam/specializations/sfm.h +++ b/python/gtsam/specializations/sfm.h @@ -18,6 +18,12 @@ py::bind_vector > >( py::bind_vector > >( m_, "BinaryMeasurementsRot3"); py::bind_map(m_, "KeyPairDoubleMap"); +py::bind_map(m_, "MatchIndicesMap"); + +py::bind_vector< + std::vector >( + m_, "KeypointsList"); + py::bind_vector< std::vector >( From 6e13456db45380bcb99e68e1139e8a9b26b9b4f4 Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Fri, 15 Jul 2022 23:35:00 -0400 Subject: [PATCH 002/118] Add Python unit test --- gtsam/sfm/DsfTrackGenerator.h | 2 +- python/gtsam/tests/test_DsfTrackGenerator.py | 34 ++++++++++++++++++++ 2 files changed, 35 insertions(+), 1 deletion(-) create mode 100644 python/gtsam/tests/test_DsfTrackGenerator.py diff --git a/gtsam/sfm/DsfTrackGenerator.h b/gtsam/sfm/DsfTrackGenerator.h index 5a3e6661e..e26d60769 100644 --- a/gtsam/sfm/DsfTrackGenerator.h +++ b/gtsam/sfm/DsfTrackGenerator.h @@ -11,7 +11,7 @@ /** * @file DsfTrackGenerator.h - * @date May 2022 + * @date July 2022 * @author John Lambert * @brief Identifies connected components in the keypoint matched graph. */ diff --git a/python/gtsam/tests/test_DsfTrackGenerator.py b/python/gtsam/tests/test_DsfTrackGenerator.py new file mode 100644 index 000000000..88b12ce3c --- /dev/null +++ b/python/gtsam/tests/test_DsfTrackGenerator.py @@ -0,0 +1,34 @@ +""" + +Authors: John Lambert +""" + +import unittest + +import numpy as np + +import gtsam +from gtsam import DsfTrackGenerator, Keypoints, KeypointsList, MatchIndicesMap +from gtsam.utils.test_case import GtsamTestCase + + +class TestDsfTrackGenerator(GtsamTestCase): + """Tests for DsfTrackGenerator.""" + + def test_track_generation(self) -> None: + """ """ + kps_i0 = Keypoints(coordinates=np.array([[0,0],[1,1]])) + kps_i1 = Keypoints(coordinates=np.array([[2,2],[3,3],[4,4]])) + kps_i2 = Keypoints(coordinates=np.array([[5,5],[6,6]])) + + keypoints_list = KeypointsList() + keypoints_list.append(kps_i0) + keypoints_list.append(kps_i1) + keypoints_list.append(kps_i2) + + matches_dict = MatchIndicesMap() + matches_dict[(0,0)] = np.array([[0,0],[1,1]]) + matches_dict[(1,1)] = np.array([[2,2],[3,3],[4,4]]) + import pdb; pdb.set_trace() + + tracks = DsfTrackGenerator.generate_tracks_from_pairwise_matches(matches_dict, keypoints_list) From 629b9cd9555e6e4fd1cd24ace80b90710e1dcb91 Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Sat, 16 Jul 2022 00:26:57 -0400 Subject: [PATCH 003/118] clean up docstrings --- gtsam/sfm/DsfTrackGenerator.h | 40 ++++++++------------ python/gtsam/tests/test_DsfTrackGenerator.py | 13 ++++--- 2 files changed, 22 insertions(+), 31 deletions(-) diff --git a/gtsam/sfm/DsfTrackGenerator.h b/gtsam/sfm/DsfTrackGenerator.h index e26d60769..095faa72b 100644 --- a/gtsam/sfm/DsfTrackGenerator.h +++ b/gtsam/sfm/DsfTrackGenerator.h @@ -48,13 +48,9 @@ struct Keypoints }; - - - -// TODO: THIS CLASSNAME IS DUPLICATED in SfmTrack.h, conflicts // @param camera index // @param 2d measurement -// Implemented as named tuple, instead of std::pair +// Implemented as named tuple, instead of std::pair (like SfmMeasurement in SfmTrack.h) struct NamedSfmMeasurement { size_t i; @@ -93,7 +89,7 @@ class SfmTrack2d using MatchIndicesMap = std::map; using KeypointsList = std::vector; -using KeypointsVector = std::vector; // TODO: prefer KeypointsSet? +using KeypointsVector = std::vector; // TODO(johnwlambert): prefer KeypointsSet? class DsfTrackGenerator @@ -113,25 +109,20 @@ class DsfTrackGenerator { std::vector track_2d_list; - // TODO: put these checks into the Keypoints class. - // check to ensure dimensions of coordinates are correct - //dims_valid = all([kps.coordinates.ndim == 2 for kps in keypoints_list]) - //if not dims_valid: - // raise Exception("Dimensions for Keypoint coordinates incorrect. Array needs to be 2D") - std::cout << "[SfmTrack2d] Starting Union-Find..." << std::endl; - // Generate the DSF to form tracks + // Generate the DSF to form tracks. DSFMapIndexPair dsf; - // for DSF finally - // measurement_idxs represented by k's for (const auto& [pair_idxs, corr_idxs]: matches_dict) { + // Image pair is (i1,i2). size_t i1 = pair_idxs.first; size_t i2 = pair_idxs.second; for (size_t k = 0; k < corr_idxs.rows(); k++) { + // Measurement indices are found in a single matrix row, as (k1,k2). size_t k1 = corr_idxs(k,0); size_t k2 = corr_idxs(k,1); + // Unique keys for the DSF are (i,k), representing keypoint index in an image. dsf.merge(IndexPair(i1, k1), IndexPair(i2, k2)); } } @@ -140,24 +131,23 @@ class DsfTrackGenerator const std::map > key_sets = dsf.sets(); size_t erroneous_track_count = 0; - // create a landmark map: a list of tracks - // Each track will be represented as a list of (camera_idx, measurements) + // Create a list of tracks. + // Each track will be represented as a list of (camera_idx, measurements). for (const auto& [set_id, index_pair_set]: key_sets) { - // Initialize track from measurements + // Initialize track from measurements. SfmTrack2d track_2d; for (const auto& index_pair: index_pair_set) { - // camera_idx is represented by i - // measurement_idx is represented by k + // Camera index is represented by i, and measurement index is represented by k. size_t i = index_pair.i(); size_t k = index_pair.j(); - // add measurement in this track + // Add measurement to this track. track_2d.addMeasurement(NamedSfmMeasurement(i, keypoints_list[i].coordinates.row(k))); } - // Skip erroneous track that had repeated measurements within the same image - // This is an expected result from an incorrect correspondence slipping through + // Skip erroneous track that had repeated measurements within the same image. + // This is an expected result from an incorrect correspondence slipping through. if (track_2d.validate_unique_cameras()) { track_2d_list.emplace_back(track_2d); @@ -167,11 +157,11 @@ class DsfTrackGenerator } double erroneous_track_pct = key_sets.size() > 0 ? erroneous_track_count / key_sets.size() * 100 : std::nan("1"); - // TODO: restrict decimal places to 2 decimals. + // TODO(johnwlambert): restrict decimal places to 2 decimals. std::cout << "DSF Union-Find: " << erroneous_track_pct; std::cout << "% of tracks discarded from multiple obs. in a single image." << std::endl; - // TODO: return the Transitivity failure percentage here. + // TODO(johnwlambert): return the Transitivity failure percentage here. return track_2d_list; } }; diff --git a/python/gtsam/tests/test_DsfTrackGenerator.py b/python/gtsam/tests/test_DsfTrackGenerator.py index 88b12ce3c..5546453a6 100644 --- a/python/gtsam/tests/test_DsfTrackGenerator.py +++ b/python/gtsam/tests/test_DsfTrackGenerator.py @@ -16,19 +16,20 @@ class TestDsfTrackGenerator(GtsamTestCase): """Tests for DsfTrackGenerator.""" def test_track_generation(self) -> None: - """ """ - kps_i0 = Keypoints(coordinates=np.array([[0,0],[1,1]])) - kps_i1 = Keypoints(coordinates=np.array([[2,2],[3,3],[4,4]])) - kps_i2 = Keypoints(coordinates=np.array([[5,5],[6,6]])) + """Ensures that DSF generates two tracks from measurements in 3 images (H=200,W=400).""" + kps_i0 = Keypoints(coordinates=np.array([[10.,20],[30,40]])) + kps_i1 = Keypoints(coordinates=np.array([[50.,60],[70,80],[90,100]])) + kps_i2 = Keypoints(coordinates=np.array([[110.,120],[130,140]])) keypoints_list = KeypointsList() keypoints_list.append(kps_i0) keypoints_list.append(kps_i1) keypoints_list.append(kps_i2) + # For each image pair (i1,i2), we provide a (K,2) matrix of corresponding image indices (k1,k2). matches_dict = MatchIndicesMap() - matches_dict[(0,0)] = np.array([[0,0],[1,1]]) - matches_dict[(1,1)] = np.array([[2,2],[3,3],[4,4]]) + matches_dict[(0,1)] = np.array([[0,0],[1,1]]) + matches_dict[(1,2)] = np.array([[2,0],[1,1]]) import pdb; pdb.set_trace() tracks = DsfTrackGenerator.generate_tracks_from_pairwise_matches(matches_dict, keypoints_list) From 2434d3a41eac380ed3cd6d60e1d3671fb90bbcb6 Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Sat, 16 Jul 2022 01:23:54 -0400 Subject: [PATCH 004/118] Fix unwrapped SfmTrack2d, and instantiate generator before calling its method --- gtsam/sfm/DsfTrackGenerator.h | 4 ++-- gtsam/sfm/sfm.i | 14 +++++++++++--- python/gtsam/tests/test_DsfTrackGenerator.py | 16 ++++++++-------- 3 files changed, 21 insertions(+), 13 deletions(-) diff --git a/gtsam/sfm/DsfTrackGenerator.h b/gtsam/sfm/DsfTrackGenerator.h index 095faa72b..619ca40ca 100644 --- a/gtsam/sfm/DsfTrackGenerator.h +++ b/gtsam/sfm/DsfTrackGenerator.h @@ -104,8 +104,8 @@ class DsfTrackGenerator // set elements from camera index of a detection and the index of that detection in that camera's keypoint list, // i.e. (i,k). std::vector generate_tracks_from_pairwise_matches( - const MatchIndicesMap matches_dict, - const KeypointsList keypoints_list) + const MatchIndicesMap& matches_dict, + const KeypointsList& keypoints_list) { std::vector track_2d_list; diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index 84988f06c..89fc2b281 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -43,11 +43,19 @@ class Keypoints }; // check if this should be a method +class SfmTrack2d +{ + void addMeasurement(const gtsam::NamedSfmMeasurement &m); + std::vector measurements(); + bool validate_unique_cameras(); +}; + + class DsfTrackGenerator { DsfTrackGenerator(); - std::vector generate_tracks_from_pairwise_matches( - const gtsam::MatchIndicesMap matches_dict, - const gtsam::KeypointsList keypoints_list); + std::vector generate_tracks_from_pairwise_matches( + const gtsam::MatchIndicesMap& matches_dict, + const gtsam::KeypointsList& keypoints_list); }; diff --git a/python/gtsam/tests/test_DsfTrackGenerator.py b/python/gtsam/tests/test_DsfTrackGenerator.py index 5546453a6..51665df7a 100644 --- a/python/gtsam/tests/test_DsfTrackGenerator.py +++ b/python/gtsam/tests/test_DsfTrackGenerator.py @@ -1,4 +1,4 @@ -""" +"""Unit tests for track generation using a Disjoint Set Forest data structure. Authors: John Lambert """ @@ -17,9 +17,9 @@ class TestDsfTrackGenerator(GtsamTestCase): def test_track_generation(self) -> None: """Ensures that DSF generates two tracks from measurements in 3 images (H=200,W=400).""" - kps_i0 = Keypoints(coordinates=np.array([[10.,20],[30,40]])) - kps_i1 = Keypoints(coordinates=np.array([[50.,60],[70,80],[90,100]])) - kps_i2 = Keypoints(coordinates=np.array([[110.,120],[130,140]])) + kps_i0 = Keypoints(coordinates=np.array([[10.0, 20], [30, 40]])) + kps_i1 = Keypoints(coordinates=np.array([[50.0, 60], [70, 80], [90, 100]])) + kps_i2 = Keypoints(coordinates=np.array([[110.0, 120], [130, 140]])) keypoints_list = KeypointsList() keypoints_list.append(kps_i0) @@ -28,8 +28,8 @@ class TestDsfTrackGenerator(GtsamTestCase): # For each image pair (i1,i2), we provide a (K,2) matrix of corresponding image indices (k1,k2). matches_dict = MatchIndicesMap() - matches_dict[(0,1)] = np.array([[0,0],[1,1]]) - matches_dict[(1,2)] = np.array([[2,0],[1,1]]) - import pdb; pdb.set_trace() + matches_dict[(0, 1)] = np.array([[0, 0], [1, 1]]) + matches_dict[(1, 2)] = np.array([[2, 0], [1, 1]]) - tracks = DsfTrackGenerator.generate_tracks_from_pairwise_matches(matches_dict, keypoints_list) + tracks = DsfTrackGenerator().generate_tracks_from_pairwise_matches(matches_dict, keypoints_list) + print(tracks[0]) From 6e45be3d98ecc9bbd52ebd7be256e021cf01d7a3 Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Sat, 16 Jul 2022 16:09:21 -0400 Subject: [PATCH 005/118] fix all wrapper issues --- gtsam/sfm/DsfTrackGenerator.h | 16 ++++++++------ gtsam/sfm/sfm.i | 15 ++++++------- python/gtsam/tests/test_DsfTrackGenerator.py | 23 +++++++++++++++++++- 3 files changed, 38 insertions(+), 16 deletions(-) diff --git a/gtsam/sfm/DsfTrackGenerator.h b/gtsam/sfm/DsfTrackGenerator.h index 619ca40ca..6483bae97 100644 --- a/gtsam/sfm/DsfTrackGenerator.h +++ b/gtsam/sfm/DsfTrackGenerator.h @@ -21,6 +21,7 @@ #include #include +#include #include #include @@ -34,9 +35,10 @@ typedef DSFMap DSFMapIndexPair; typedef std::pair ImagePair; typedef Eigen::MatrixX2i CorrespondenceIndices; // N x 2 array - +//struct Keypoints; using KeypointCoordinates = Eigen::MatrixX2d; + struct Keypoints { KeypointCoordinates coordinates; @@ -47,6 +49,10 @@ struct Keypoints Keypoints(const gtsam::KeypointCoordinates& coordinates): coordinates(coordinates) {}; // boost::none }; +using KeypointsList = std::vector; +using KeypointsVector = std::vector; // TODO(johnwlambert): prefer KeypointsSet? +using MatchIndicesMap = std::map; + // @param camera index // @param 2d measurement @@ -54,9 +60,9 @@ struct Keypoints struct NamedSfmMeasurement { size_t i; - Point2 uv; + gtsam::Point2 uv; - NamedSfmMeasurement(size_t i, Point2 uv) : i(i), uv(uv) {} + NamedSfmMeasurement(size_t i, gtsam::Point2 uv) : i(i), uv(uv) {} }; @@ -87,10 +93,6 @@ class SfmTrack2d } }; -using MatchIndicesMap = std::map; -using KeypointsList = std::vector; -using KeypointsVector = std::vector; // TODO(johnwlambert): prefer KeypointsSet? - class DsfTrackGenerator { diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index 89fc2b281..0374d99b3 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -4,14 +4,6 @@ namespace gtsam { -#include -class GtsfmData -{ - GtsfmData(const int numberImages); - void write_points(std::vector& images, const std::string save_dir); - void write_images(std::vector& images, const std::string save_dir); -}; - #include class MatchIndicesMap { @@ -43,6 +35,13 @@ class Keypoints }; // check if this should be a method +class NamedSfmMeasurement +{ + size_t i; + gtsam::Point2 uv; + NamedSfmMeasurement(size_t i, gtsam::Point2 uv); +}; + class SfmTrack2d { void addMeasurement(const gtsam::NamedSfmMeasurement &m); diff --git a/python/gtsam/tests/test_DsfTrackGenerator.py b/python/gtsam/tests/test_DsfTrackGenerator.py index 51665df7a..953167dca 100644 --- a/python/gtsam/tests/test_DsfTrackGenerator.py +++ b/python/gtsam/tests/test_DsfTrackGenerator.py @@ -32,4 +32,25 @@ class TestDsfTrackGenerator(GtsamTestCase): matches_dict[(1, 2)] = np.array([[2, 0], [1, 1]]) tracks = DsfTrackGenerator().generate_tracks_from_pairwise_matches(matches_dict, keypoints_list) - print(tracks[0]) + + assert len(tracks) == 3 + + # Verify track 0. + assert np.allclose(tracks[0].measurements()[0].uv, np.array([10.0, 20.0])) + assert np.allclose(tracks[0].measurements()[1].uv, np.array([50.0, 60.0])) + assert tracks[0].measurements()[0].i, 0 + assert tracks[0].measurements()[1].i, 1 + + # Verify track 1. + assert np.allclose(tracks[1].measurements()[0].uv, np.array([30.0, 40.0])) + assert np.allclose(tracks[1].measurements()[1].uv, np.array([70.0, 80.0])) + assert np.allclose(tracks[1].measurements()[2].uv, np.array([130.0, 140.0])) + assert tracks[1].measurements()[0].i, 0 + assert tracks[1].measurements()[1].i, 1 + assert tracks[1].measurements()[2].i, 2 + + # Verify track 2. + assert np.allclose(tracks[2].measurements()[0].uv, np.array([90.0, 100.0])) + assert np.allclose(tracks[2].measurements()[1].uv, np.array([110.0, 120.0])) + assert tracks[2].measurements()[0].i, 1 + assert tracks[2].measurements()[1].i, 2 From 361e3f601c79c3a467a17392db3e40fb1e9db443 Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Sat, 16 Jul 2022 22:29:01 -0400 Subject: [PATCH 006/118] Remove C++17 decompositions, and address comments --- gtsam/sfm/DsfTrackGenerator.h | 42 +++++++++++++++----- gtsam/sfm/sfm.i | 2 +- python/gtsam/tests/test_DsfTrackGenerator.py | 18 +++++---- 3 files changed, 43 insertions(+), 19 deletions(-) diff --git a/gtsam/sfm/DsfTrackGenerator.h b/gtsam/sfm/DsfTrackGenerator.h index 6483bae97..c67215a91 100644 --- a/gtsam/sfm/DsfTrackGenerator.h +++ b/gtsam/sfm/DsfTrackGenerator.h @@ -13,7 +13,7 @@ * @file DsfTrackGenerator.h * @date July 2022 * @author John Lambert - * @brief Identifies connected components in the keypoint matched graph. + * @brief Identifies connected components in the keypoint matches graph. */ #pragma once @@ -66,6 +66,11 @@ struct NamedSfmMeasurement }; +/** + * @brief Track containing 2D measurements associated with a single 3D point. + * Note: Equivalent to gtsam.SfmTrack, but without the 3d measurement. + * This class holds data temporarily before 3D point is initialized. + */ class SfmTrack2d { private: @@ -80,7 +85,7 @@ class SfmTrack2d // @brief Validates the track by checking that no two measurements are from the same camera. // // returns boolean result of the validation. - bool validate_unique_cameras() + bool has_unique_cameras() { std::vector track_cam_idxs; for (auto & measurement: measurements_) @@ -93,18 +98,25 @@ class SfmTrack2d } }; - +/** + * @brief Generates point tracks from connected components in the keypoint matches graph. + */ class DsfTrackGenerator { public: + /** Default constructor. */ DsfTrackGenerator() {} // Creates a list of tracks from 2d point correspondences. // - // Creates a disjoint-set forest (DSF) and 2d tracks from pairwise matches. We create a singleton for union-find - // set elements from camera index of a detection and the index of that detection in that camera's keypoint list, + // Creates a disjoint-set forest (DSF) and 2d tracks from pairwise matches. + // We create a singleton for union-find set elements from camera index of a + // detection and the index of that detection in that camera's keypoint list, // i.e. (i,k). + // @param Map from (i1,i2) image pair indices to (K,2) matrix, for K + // correspondence indices, from each image. + // @param Length-N list of keypoints, for N images/cameras. std::vector generate_tracks_from_pairwise_matches( const MatchIndicesMap& matches_dict, const KeypointsList& keypoints_list) @@ -115,7 +127,10 @@ class DsfTrackGenerator // Generate the DSF to form tracks. DSFMapIndexPair dsf; - for (const auto& [pair_idxs, corr_idxs]: matches_dict) { + for (const auto& kv: matches_dict) { + const auto pair_idxs = kv.first; + const auto corr_idxs = kv.second; + // Image pair is (i1,i2). size_t i1 = pair_idxs.first; size_t i2 = pair_idxs.second; @@ -132,10 +147,16 @@ class DsfTrackGenerator std::cout << "[SfmTrack2d] Union-Find Complete" << std::endl; const std::map > key_sets = dsf.sets(); + // Return immediately if no sets were found. + if (key_sets.empty()) return track_2d_list; + size_t erroneous_track_count = 0; // Create a list of tracks. // Each track will be represented as a list of (camera_idx, measurements). - for (const auto& [set_id, index_pair_set]: key_sets) { + for (const auto& kv: key_sets) { + const auto set_id = kv.first; + const auto index_pair_set = kv.second; + // Initialize track from measurements. SfmTrack2d track_2d; @@ -150,15 +171,16 @@ class DsfTrackGenerator // Skip erroneous track that had repeated measurements within the same image. // This is an expected result from an incorrect correspondence slipping through. - if (track_2d.validate_unique_cameras()) + if (track_2d.has_unique_cameras()) { track_2d_list.emplace_back(track_2d); } else { - erroneous_track_count += 1; + erroneous_track_count++; } } - double erroneous_track_pct = key_sets.size() > 0 ? erroneous_track_count / key_sets.size() * 100 : std::nan("1"); + double erroneous_track_pct = static_cast(erroneous_track_count) + / static_cast(key_sets.size()) * 100; // TODO(johnwlambert): restrict decimal places to 2 decimals. std::cout << "DSF Union-Find: " << erroneous_track_pct; std::cout << "% of tracks discarded from multiple obs. in a single image." << std::endl; diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index 0374d99b3..2a683a032 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -46,7 +46,7 @@ class SfmTrack2d { void addMeasurement(const gtsam::NamedSfmMeasurement &m); std::vector measurements(); - bool validate_unique_cameras(); + bool has_unique_cameras(); }; diff --git a/python/gtsam/tests/test_DsfTrackGenerator.py b/python/gtsam/tests/test_DsfTrackGenerator.py index 953167dca..cb643bdb9 100644 --- a/python/gtsam/tests/test_DsfTrackGenerator.py +++ b/python/gtsam/tests/test_DsfTrackGenerator.py @@ -16,7 +16,7 @@ class TestDsfTrackGenerator(GtsamTestCase): """Tests for DsfTrackGenerator.""" def test_track_generation(self) -> None: - """Ensures that DSF generates two tracks from measurements in 3 images (H=200,W=400).""" + """Ensures that DSF generates three tracks from measurements in 3 images (H=200,W=400).""" kps_i0 = Keypoints(coordinates=np.array([[10.0, 20], [30, 40]])) kps_i1 = Keypoints(coordinates=np.array([[50.0, 60], [70, 80], [90, 100]])) kps_i2 = Keypoints(coordinates=np.array([[110.0, 120], [130, 140]])) @@ -32,25 +32,27 @@ class TestDsfTrackGenerator(GtsamTestCase): matches_dict[(1, 2)] = np.array([[2, 0], [1, 1]]) tracks = DsfTrackGenerator().generate_tracks_from_pairwise_matches(matches_dict, keypoints_list) + import pdb + pdb.set_trace() assert len(tracks) == 3 # Verify track 0. assert np.allclose(tracks[0].measurements()[0].uv, np.array([10.0, 20.0])) assert np.allclose(tracks[0].measurements()[1].uv, np.array([50.0, 60.0])) - assert tracks[0].measurements()[0].i, 0 - assert tracks[0].measurements()[1].i, 1 + assert tracks[0].measurements()[0].i == 0 + assert tracks[0].measurements()[1].i == 1 # Verify track 1. assert np.allclose(tracks[1].measurements()[0].uv, np.array([30.0, 40.0])) assert np.allclose(tracks[1].measurements()[1].uv, np.array([70.0, 80.0])) assert np.allclose(tracks[1].measurements()[2].uv, np.array([130.0, 140.0])) - assert tracks[1].measurements()[0].i, 0 - assert tracks[1].measurements()[1].i, 1 - assert tracks[1].measurements()[2].i, 2 + assert tracks[1].measurements()[0].i == 0 + assert tracks[1].measurements()[1].i == 1 + assert tracks[1].measurements()[2].i == 2 # Verify track 2. assert np.allclose(tracks[2].measurements()[0].uv, np.array([90.0, 100.0])) assert np.allclose(tracks[2].measurements()[1].uv, np.array([110.0, 120.0])) - assert tracks[2].measurements()[0].i, 1 - assert tracks[2].measurements()[1].i, 2 + assert tracks[2].measurements()[0].i == 1 + assert tracks[2].measurements()[1].i == 2 From 51fb3750e8b82691d0686bddbb1f61934a23238f Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Sun, 17 Jul 2022 00:31:42 -0400 Subject: [PATCH 007/118] remove stray breakpoint --- python/gtsam/tests/test_DsfTrackGenerator.py | 3 --- 1 file changed, 3 deletions(-) diff --git a/python/gtsam/tests/test_DsfTrackGenerator.py b/python/gtsam/tests/test_DsfTrackGenerator.py index cb643bdb9..b9ac22397 100644 --- a/python/gtsam/tests/test_DsfTrackGenerator.py +++ b/python/gtsam/tests/test_DsfTrackGenerator.py @@ -32,9 +32,6 @@ class TestDsfTrackGenerator(GtsamTestCase): matches_dict[(1, 2)] = np.array([[2, 0], [1, 1]]) tracks = DsfTrackGenerator().generate_tracks_from_pairwise_matches(matches_dict, keypoints_list) - import pdb - - pdb.set_trace() assert len(tracks) == 3 # Verify track 0. From fc35096a1d833dd1d4bf08fc475f86b10c7d49a8 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Tue, 19 Jul 2022 00:34:20 -0400 Subject: [PATCH 008/118] Update style to have bracket on same lines, and autoformat --- gtsam/sfm/DsfTrackGenerator.h | 186 ++++++++++++++++------------------ 1 file changed, 90 insertions(+), 96 deletions(-) diff --git a/gtsam/sfm/DsfTrackGenerator.h b/gtsam/sfm/DsfTrackGenerator.h index c67215a91..9da588c55 100644 --- a/gtsam/sfm/DsfTrackGenerator.h +++ b/gtsam/sfm/DsfTrackGenerator.h @@ -17,7 +17,6 @@ */ #pragma once -#include #include #include @@ -39,8 +38,7 @@ typedef Eigen::MatrixX2i CorrespondenceIndices; // N x 2 array using KeypointCoordinates = Eigen::MatrixX2d; -struct Keypoints -{ +struct Keypoints { KeypointCoordinates coordinates; // typedef'd for Eigen::VectorXd boost::optional scales; @@ -57,8 +55,7 @@ using MatchIndicesMap = std::map; // @param camera index // @param 2d measurement // Implemented as named tuple, instead of std::pair (like SfmMeasurement in SfmTrack.h) -struct NamedSfmMeasurement -{ +struct NamedSfmMeasurement { size_t i; gtsam::Point2 uv; @@ -71,16 +68,15 @@ struct NamedSfmMeasurement * Note: Equivalent to gtsam.SfmTrack, but without the 3d measurement. * This class holds data temporarily before 3D point is initialized. */ -class SfmTrack2d -{ - private: - std::vector measurements_; +class SfmTrack2d { + private: + std::vector measurements_; - public: - void addMeasurement(const NamedSfmMeasurement &m) { - measurements_.emplace_back(m); - } - std::vector measurements() {return measurements_; } + public: + void addMeasurement(const NamedSfmMeasurement &m) { + measurements_.emplace_back(m); + } + std::vector measurements() {return measurements_; } // @brief Validates the track by checking that no two measurements are from the same camera. // @@ -88,7 +84,7 @@ class SfmTrack2d bool has_unique_cameras() { std::vector track_cam_idxs; - for (auto & measurement: measurements_) + for (auto & measurement : measurements_) { track_cam_idxs.emplace_back(measurement.i); } @@ -101,93 +97,91 @@ class SfmTrack2d /** * @brief Generates point tracks from connected components in the keypoint matches graph. */ -class DsfTrackGenerator -{ +class DsfTrackGenerator { - public: - /** Default constructor. */ - DsfTrackGenerator() {} + public: + /** Default constructor. */ + DsfTrackGenerator() {} - // Creates a list of tracks from 2d point correspondences. - // - // Creates a disjoint-set forest (DSF) and 2d tracks from pairwise matches. - // We create a singleton for union-find set elements from camera index of a - // detection and the index of that detection in that camera's keypoint list, - // i.e. (i,k). - // @param Map from (i1,i2) image pair indices to (K,2) matrix, for K - // correspondence indices, from each image. - // @param Length-N list of keypoints, for N images/cameras. - std::vector generate_tracks_from_pairwise_matches( - const MatchIndicesMap& matches_dict, - const KeypointsList& keypoints_list) - { - std::vector track_2d_list; + // Creates a list of tracks from 2d point correspondences. + // + // Creates a disjoint-set forest (DSF) and 2d tracks from pairwise matches. + // We create a singleton for union-find set elements from camera index of a + // detection and the index of that detection in that camera's keypoint list, + // i.e. (i,k). + // @param Map from (i1,i2) image pair indices to (K,2) matrix, for K + // correspondence indices, from each image. + // @param Length-N list of keypoints, for N images/cameras. + std::vector generate_tracks_from_pairwise_matches( + const MatchIndicesMap& matches_dict, + const KeypointsList& keypoints_list) { + std::vector track_2d_list; - std::cout << "[SfmTrack2d] Starting Union-Find..." << std::endl; - // Generate the DSF to form tracks. - DSFMapIndexPair dsf; - - for (const auto& kv: matches_dict) { - const auto pair_idxs = kv.first; - const auto corr_idxs = kv.second; + std::cout << "[SfmTrack2d] Starting Union-Find..." << std::endl; + // Generate the DSF to form tracks. + DSFMapIndexPair dsf; - // Image pair is (i1,i2). - size_t i1 = pair_idxs.first; - size_t i2 = pair_idxs.second; - for (size_t k = 0; k < corr_idxs.rows(); k++) - { - // Measurement indices are found in a single matrix row, as (k1,k2). - size_t k1 = corr_idxs(k,0); - size_t k2 = corr_idxs(k,1); - // Unique keys for the DSF are (i,k), representing keypoint index in an image. - dsf.merge(IndexPair(i1, k1), IndexPair(i2, k2)); - } + for (const auto& kv : matches_dict) { + const auto pair_idxs = kv.first; + const auto corr_idxs = kv.second; + + // Image pair is (i1,i2). + size_t i1 = pair_idxs.first; + size_t i2 = pair_idxs.second; + for (size_t k = 0; k < corr_idxs.rows(); k++) + { + // Measurement indices are found in a single matrix row, as (k1,k2). + size_t k1 = corr_idxs(k, 0); + size_t k2 = corr_idxs(k, 1); + // Unique keys for the DSF are (i,k), representing keypoint index in an image. + dsf.merge(IndexPair(i1, k1), IndexPair(i2, k2)); } - - std::cout << "[SfmTrack2d] Union-Find Complete" << std::endl; - const std::map > key_sets = dsf.sets(); - - // Return immediately if no sets were found. - if (key_sets.empty()) return track_2d_list; - - size_t erroneous_track_count = 0; - // Create a list of tracks. - // Each track will be represented as a list of (camera_idx, measurements). - for (const auto& kv: key_sets) { - const auto set_id = kv.first; - const auto index_pair_set = kv.second; - - // Initialize track from measurements. - SfmTrack2d track_2d; - - for (const auto& index_pair: index_pair_set) - { - // Camera index is represented by i, and measurement index is represented by k. - size_t i = index_pair.i(); - size_t k = index_pair.j(); - // Add measurement to this track. - track_2d.addMeasurement(NamedSfmMeasurement(i, keypoints_list[i].coordinates.row(k))); - } - - // Skip erroneous track that had repeated measurements within the same image. - // This is an expected result from an incorrect correspondence slipping through. - if (track_2d.has_unique_cameras()) - { - track_2d_list.emplace_back(track_2d); - } else { - erroneous_track_count++; - } - } - - double erroneous_track_pct = static_cast(erroneous_track_count) - / static_cast(key_sets.size()) * 100; - // TODO(johnwlambert): restrict decimal places to 2 decimals. - std::cout << "DSF Union-Find: " << erroneous_track_pct; - std::cout << "% of tracks discarded from multiple obs. in a single image." << std::endl; - - // TODO(johnwlambert): return the Transitivity failure percentage here. - return track_2d_list; } + + std::cout << "[SfmTrack2d] Union-Find Complete" << std::endl; + const std::map > key_sets = dsf.sets(); + + // Return immediately if no sets were found. + if (key_sets.empty()) return track_2d_list; + + size_t erroneous_track_count = 0; + // Create a list of tracks. + // Each track will be represented as a list of (camera_idx, measurements). + for (const auto& kv : key_sets) { + const auto set_id = kv.first; + const auto index_pair_set = kv.second; + + // Initialize track from measurements. + SfmTrack2d track_2d; + + for (const auto& index_pair : index_pair_set) + { + // Camera index is represented by i, and measurement index is represented by k. + size_t i = index_pair.i(); + size_t k = index_pair.j(); + // Add measurement to this track. + track_2d.addMeasurement(NamedSfmMeasurement(i, keypoints_list[i].coordinates.row(k))); + } + + // Skip erroneous track that had repeated measurements within the same image. + // This is an expected result from an incorrect correspondence slipping through. + if (track_2d.has_unique_cameras()) + { + track_2d_list.emplace_back(track_2d); + } else { + erroneous_track_count++; + } + } + + double erroneous_track_pct = static_cast(erroneous_track_count) + / static_cast(key_sets.size()) * 100; + // TODO(johnwlambert): restrict decimal places to 2 decimals. + std::cout << "DSF Union-Find: " << erroneous_track_pct; + std::cout << "% of tracks discarded from multiple obs. in a single image." << std::endl; + + // TODO(johnwlambert): return the Transitivity failure percentage here. + return track_2d_list; + } }; } // namespace gtsam From e6ccf977125794c0abba8ced4a45751cc5488a86 Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Sun, 24 Jul 2022 02:54:07 -0400 Subject: [PATCH 009/118] hasUniqueCameras to camel case --- gtsam/sfm/DsfTrackGenerator.h | 188 ++++++++++++++++------------------ gtsam/sfm/sfm.i | 16 +-- 2 files changed, 99 insertions(+), 105 deletions(-) diff --git a/gtsam/sfm/DsfTrackGenerator.h b/gtsam/sfm/DsfTrackGenerator.h index c67215a91..18c96653a 100644 --- a/gtsam/sfm/DsfTrackGenerator.h +++ b/gtsam/sfm/DsfTrackGenerator.h @@ -17,7 +17,6 @@ */ #pragma once -#include #include #include @@ -39,8 +38,7 @@ typedef Eigen::MatrixX2i CorrespondenceIndices; // N x 2 array using KeypointCoordinates = Eigen::MatrixX2d; -struct Keypoints -{ +struct Keypoints { KeypointCoordinates coordinates; // typedef'd for Eigen::VectorXd boost::optional scales; @@ -57,8 +55,7 @@ using MatchIndicesMap = std::map; // @param camera index // @param 2d measurement // Implemented as named tuple, instead of std::pair (like SfmMeasurement in SfmTrack.h) -struct NamedSfmMeasurement -{ +struct NamedSfmMeasurement { size_t i; gtsam::Point2 uv; @@ -71,24 +68,23 @@ struct NamedSfmMeasurement * Note: Equivalent to gtsam.SfmTrack, but without the 3d measurement. * This class holds data temporarily before 3D point is initialized. */ -class SfmTrack2d -{ - private: - std::vector measurements_; +class SfmTrack2d { + private: + std::vector measurements_; - public: - void addMeasurement(const NamedSfmMeasurement &m) { - measurements_.emplace_back(m); - } - std::vector measurements() {return measurements_; } + public: + void addMeasurement(const NamedSfmMeasurement &m) { + measurements_.emplace_back(m); + } + std::vector measurements() {return measurements_; } // @brief Validates the track by checking that no two measurements are from the same camera. // // returns boolean result of the validation. - bool has_unique_cameras() + bool hasUniqueCameras() { std::vector track_cam_idxs; - for (auto & measurement: measurements_) + for (auto & measurement : measurements_) { track_cam_idxs.emplace_back(measurement.i); } @@ -101,93 +97,91 @@ class SfmTrack2d /** * @brief Generates point tracks from connected components in the keypoint matches graph. */ -class DsfTrackGenerator -{ +class DsfTrackGenerator { - public: - /** Default constructor. */ - DsfTrackGenerator() {} + public: + /** Default constructor. */ + DsfTrackGenerator() {} - // Creates a list of tracks from 2d point correspondences. - // - // Creates a disjoint-set forest (DSF) and 2d tracks from pairwise matches. - // We create a singleton for union-find set elements from camera index of a - // detection and the index of that detection in that camera's keypoint list, - // i.e. (i,k). - // @param Map from (i1,i2) image pair indices to (K,2) matrix, for K - // correspondence indices, from each image. - // @param Length-N list of keypoints, for N images/cameras. - std::vector generate_tracks_from_pairwise_matches( - const MatchIndicesMap& matches_dict, - const KeypointsList& keypoints_list) - { - std::vector track_2d_list; + // Creates a list of tracks from 2d point correspondences. + // + // Creates a disjoint-set forest (DSF) and 2d tracks from pairwise matches. + // We create a singleton for union-find set elements from camera index of a + // detection and the index of that detection in that camera's keypoint list, + // i.e. (i,k). + // @param Map from (i1,i2) image pair indices to (K,2) matrix, for K + // correspondence indices, from each image. + // @param Length-N list of keypoints, for N images/cameras. + std::vector generate_tracks_from_pairwise_matches( + const MatchIndicesMap& matches_dict, + const KeypointsList& keypoints_list) { + std::vector track_2d_list; - std::cout << "[SfmTrack2d] Starting Union-Find..." << std::endl; - // Generate the DSF to form tracks. - DSFMapIndexPair dsf; - - for (const auto& kv: matches_dict) { - const auto pair_idxs = kv.first; - const auto corr_idxs = kv.second; + std::cout << "[SfmTrack2d] Starting Union-Find..." << std::endl; + // Generate the DSF to form tracks. + DSFMapIndexPair dsf; - // Image pair is (i1,i2). - size_t i1 = pair_idxs.first; - size_t i2 = pair_idxs.second; - for (size_t k = 0; k < corr_idxs.rows(); k++) - { - // Measurement indices are found in a single matrix row, as (k1,k2). - size_t k1 = corr_idxs(k,0); - size_t k2 = corr_idxs(k,1); - // Unique keys for the DSF are (i,k), representing keypoint index in an image. - dsf.merge(IndexPair(i1, k1), IndexPair(i2, k2)); - } + for (const auto& kv : matches_dict) { + const auto pair_idxs = kv.first; + const auto corr_idxs = kv.second; + + // Image pair is (i1,i2). + size_t i1 = pair_idxs.first; + size_t i2 = pair_idxs.second; + for (size_t k = 0; k < corr_idxs.rows(); k++) + { + // Measurement indices are found in a single matrix row, as (k1,k2). + size_t k1 = corr_idxs(k, 0); + size_t k2 = corr_idxs(k, 1); + // Unique keys for the DSF are (i,k), representing keypoint index in an image. + dsf.merge(IndexPair(i1, k1), IndexPair(i2, k2)); } - - std::cout << "[SfmTrack2d] Union-Find Complete" << std::endl; - const std::map > key_sets = dsf.sets(); - - // Return immediately if no sets were found. - if (key_sets.empty()) return track_2d_list; - - size_t erroneous_track_count = 0; - // Create a list of tracks. - // Each track will be represented as a list of (camera_idx, measurements). - for (const auto& kv: key_sets) { - const auto set_id = kv.first; - const auto index_pair_set = kv.second; - - // Initialize track from measurements. - SfmTrack2d track_2d; - - for (const auto& index_pair: index_pair_set) - { - // Camera index is represented by i, and measurement index is represented by k. - size_t i = index_pair.i(); - size_t k = index_pair.j(); - // Add measurement to this track. - track_2d.addMeasurement(NamedSfmMeasurement(i, keypoints_list[i].coordinates.row(k))); - } - - // Skip erroneous track that had repeated measurements within the same image. - // This is an expected result from an incorrect correspondence slipping through. - if (track_2d.has_unique_cameras()) - { - track_2d_list.emplace_back(track_2d); - } else { - erroneous_track_count++; - } - } - - double erroneous_track_pct = static_cast(erroneous_track_count) - / static_cast(key_sets.size()) * 100; - // TODO(johnwlambert): restrict decimal places to 2 decimals. - std::cout << "DSF Union-Find: " << erroneous_track_pct; - std::cout << "% of tracks discarded from multiple obs. in a single image." << std::endl; - - // TODO(johnwlambert): return the Transitivity failure percentage here. - return track_2d_list; } + + std::cout << "[SfmTrack2d] Union-Find Complete" << std::endl; + const std::map > key_sets = dsf.sets(); + + // Return immediately if no sets were found. + if (key_sets.empty()) return track_2d_list; + + size_t erroneous_track_count = 0; + // Create a list of tracks. + // Each track will be represented as a list of (camera_idx, measurements). + for (const auto& kv : key_sets) { + const auto set_id = kv.first; + const auto index_pair_set = kv.second; + + // Initialize track from measurements. + SfmTrack2d track_2d; + + for (const auto& index_pair : index_pair_set) + { + // Camera index is represented by i, and measurement index is represented by k. + size_t i = index_pair.i(); + size_t k = index_pair.j(); + // Add measurement to this track. + track_2d.addMeasurement(NamedSfmMeasurement(i, keypoints_list[i].coordinates.row(k))); + } + + // Skip erroneous track that had repeated measurements within the same image. + // This is an expected result from an incorrect correspondence slipping through. + if (track_2d.hasUniqueCameras()) + { + track_2d_list.emplace_back(track_2d); + } else { + erroneous_track_count++; + } + } + + double erroneous_track_pct = static_cast(erroneous_track_count) + / static_cast(key_sets.size()) * 100; + // TODO(johnwlambert): restrict decimal places to 2 decimals. + std::cout << "DSF Union-Find: " << erroneous_track_pct; + std::cout << "% of tracks discarded from multiple obs. in a single image." << std::endl; + + // TODO(johnwlambert): return the Transitivity failure percentage here. + return track_2d_list; + } }; } // namespace gtsam diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index 2a683a032..b46b308eb 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -17,6 +17,13 @@ class MatchIndicesMap { }; +class Keypoints +{ + Keypoints(const gtsam::KeypointCoordinates& coordinates); + gtsam::KeypointCoordinates coordinates; +}; // check if this should be a method + + class KeypointsList { KeypointsList(); KeypointsList(const gtsam::KeypointsList& other); @@ -28,13 +35,6 @@ class KeypointsList { }; -class Keypoints -{ - Keypoints(const gtsam::KeypointCoordinates& coordinates); - gtsam::KeypointCoordinates coordinates; -}; // check if this should be a method - - class NamedSfmMeasurement { size_t i; @@ -46,7 +46,7 @@ class SfmTrack2d { void addMeasurement(const gtsam::NamedSfmMeasurement &m); std::vector measurements(); - bool has_unique_cameras(); + bool hasUniqueCameras(); }; From 720145ae672274493fbc4f9b1b8718608141adb1 Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Sun, 24 Jul 2022 03:07:04 -0400 Subject: [PATCH 010/118] remove unnecessary extra typedef --- gtsam/sfm/DsfTrackGenerator.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/sfm/DsfTrackGenerator.h b/gtsam/sfm/DsfTrackGenerator.h index 18c96653a..5ab87a7df 100644 --- a/gtsam/sfm/DsfTrackGenerator.h +++ b/gtsam/sfm/DsfTrackGenerator.h @@ -31,7 +31,6 @@ namespace gtsam { typedef DSFMap DSFMapIndexPair; -typedef std::pair ImagePair; typedef Eigen::MatrixX2i CorrespondenceIndices; // N x 2 array //struct Keypoints; @@ -49,7 +48,8 @@ struct Keypoints { using KeypointsList = std::vector; using KeypointsVector = std::vector; // TODO(johnwlambert): prefer KeypointsSet? -using MatchIndicesMap = std::map; +// Mapping from each image pair to (N,2) array representing indices of matching keypoints. +using MatchIndicesMap = std::map; // @param camera index From a6101b2d8f9ef0d512dd4852b0402621eb0d68c5 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 26 Aug 2022 10:36:49 -0400 Subject: [PATCH 011/118] fix filename in docstring --- gtsam/nonlinear/NonlinearISAM.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/nonlinear/NonlinearISAM.cpp b/gtsam/nonlinear/NonlinearISAM.cpp index 512069a8a..840712eb6 100644 --- a/gtsam/nonlinear/NonlinearISAM.cpp +++ b/gtsam/nonlinear/NonlinearISAM.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file NonlinearISAM-inl.h + * @file NonlinearISAM.cpp * @date Jan 19, 2010 * @author Viorela Ila and Richard Roberts */ From f0df82ac040268c83f8c06cd3c4f27dd7c47bf7d Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 26 Aug 2022 11:35:24 -0400 Subject: [PATCH 012/118] HybridBayesNet::optimize --- gtsam/hybrid/HybridBayesNet.cpp | 13 ++++++--- gtsam/hybrid/HybridBayesNet.h | 9 ++++++ gtsam/hybrid/HybridConditional.h | 9 +++--- gtsam/hybrid/tests/testHybridBayesNet.cpp | 34 +++++++++++++++++++++++ 4 files changed, 56 insertions(+), 9 deletions(-) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index 4665a3136..f93d21651 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -16,8 +16,8 @@ */ #include -#include #include +#include namespace gtsam { @@ -112,13 +112,12 @@ HybridBayesNet HybridBayesNet::prune( /* ************************************************************************* */ GaussianMixture::shared_ptr HybridBayesNet::atGaussian(size_t i) const { - return boost::dynamic_pointer_cast(factors_.at(i)->inner()); + return factors_.at(i)->asMixture(); } /* ************************************************************************* */ DiscreteConditional::shared_ptr HybridBayesNet::atDiscrete(size_t i) const { - return boost::dynamic_pointer_cast( - factors_.at(i)->inner()); + return factors_.at(i)->asDiscreteConditional(); } /* ************************************************************************* */ @@ -138,4 +137,10 @@ HybridValues HybridBayesNet::optimize() const { return dag.argmax(); } +/* *******************************************************************************/ +VectorValues HybridBayesNet::optimize(const DiscreteValues &assignment) const { + GaussianBayesNet gbn = this->choose(assignment); + return gbn.optimize(); +} + } // namespace gtsam diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h index 0d2dc3642..a16a4f42c 100644 --- a/gtsam/hybrid/HybridBayesNet.h +++ b/gtsam/hybrid/HybridBayesNet.h @@ -72,6 +72,15 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { /// TODO(Shangjie) do we need to create a HybridGaussianBayesNet class, and /// put this method there? HybridValues optimize() const; + + /** + * @brief Given the discrete assignment, return the optimized estimate for the + * selected Gaussian BayesNet. + * + * @param assignment An assignment of discrete values. + * @return Values + */ + VectorValues optimize(const DiscreteValues &assignment) const; }; } // namespace gtsam diff --git a/gtsam/hybrid/HybridConditional.h b/gtsam/hybrid/HybridConditional.h index 3ba5da393..91c9f8495 100644 --- a/gtsam/hybrid/HybridConditional.h +++ b/gtsam/hybrid/HybridConditional.h @@ -69,7 +69,7 @@ class GTSAM_EXPORT HybridConditional BaseConditional; ///< Typedef to our conditional base class protected: - // Type-erased pointer to the inner type + /// Type-erased pointer to the inner type boost::shared_ptr inner_; public: @@ -127,8 +127,7 @@ class GTSAM_EXPORT HybridConditional * @param gaussianMixture Gaussian Mixture Conditional used to create the * HybridConditional. */ - HybridConditional( - boost::shared_ptr gaussianMixture); + HybridConditional(boost::shared_ptr gaussianMixture); /** * @brief Return HybridConditional as a GaussianMixture @@ -168,10 +167,10 @@ class GTSAM_EXPORT HybridConditional /// Get the type-erased pointer to the inner type boost::shared_ptr inner() { return inner_; } -}; // DiscreteConditional +}; // HybridConditional // traits template <> -struct traits : public Testable {}; +struct traits : public Testable {}; } // namespace gtsam diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp index f3db83955..d447bcce2 100644 --- a/gtsam/hybrid/tests/testHybridBayesNet.cpp +++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp @@ -85,6 +85,40 @@ TEST(HybridBayesNet, Choose) { *gbn.at(3))); } +/* ****************************************************************************/ +// Test bayes net optimize +TEST(HybridBayesNet, Optimize) { + Switching s(4); + + Ordering ordering; + for (auto&& kvp : s.linearizationPoint) { + ordering += kvp.key; + } + + HybridBayesNet::shared_ptr hybridBayesNet; + HybridGaussianFactorGraph::shared_ptr remainingFactorGraph; + std::tie(hybridBayesNet, remainingFactorGraph) = + s.linearizedFactorGraph.eliminatePartialSequential(ordering); + + DiscreteValues assignment; + assignment[M(1)] = 1; + assignment[M(2)] = 1; + assignment[M(3)] = 1; + + VectorValues delta = hybridBayesNet->optimize(assignment); + + // The linearization point has the same value as the key index, + // e.g. X(1) = 1, X(2) = 2, + // but the factors specify X(k) = k-1, so delta should be -1. + VectorValues expected_delta; + expected_delta.insert(make_pair(X(1), -Vector1::Ones())); + expected_delta.insert(make_pair(X(2), -Vector1::Ones())); + expected_delta.insert(make_pair(X(3), -Vector1::Ones())); + expected_delta.insert(make_pair(X(4), -Vector1::Ones())); + + EXPECT(assert_equal(expected_delta, delta)); +} + /* ************************************************************************* */ int main() { TestResult tr; From cb2d2e678d4d0fdd33437895865b43a56af7fa0c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 26 Aug 2022 11:36:13 -0400 Subject: [PATCH 013/118] HybridBayesTree::optimize --- gtsam/hybrid/HybridBayesTree.cpp | 37 +++++++ gtsam/hybrid/HybridBayesTree.h | 9 ++ gtsam/hybrid/tests/testHybridBayesTree.cpp | 106 +++++++++++++++++++++ 3 files changed, 152 insertions(+) create mode 100644 gtsam/hybrid/tests/testHybridBayesTree.cpp diff --git a/gtsam/hybrid/HybridBayesTree.cpp b/gtsam/hybrid/HybridBayesTree.cpp index d65270f91..30fe6f168 100644 --- a/gtsam/hybrid/HybridBayesTree.cpp +++ b/gtsam/hybrid/HybridBayesTree.cpp @@ -35,4 +35,41 @@ bool HybridBayesTree::equals(const This& other, double tol) const { return Base::equals(other, tol); } +/* ************************************************************************* */ +VectorValues HybridBayesTree::optimize(const DiscreteValues& assignment) const { + GaussianBayesNet gbn; + + KeyVector added_keys; + + // Iterate over all the nodes in the BayesTree + for (auto&& node : nodes()) { + // Check if conditional being added is already in the Bayes net. + if (std::find(added_keys.begin(), added_keys.end(), node.first) == + added_keys.end()) { + // Access the clique and get the underlying hybrid conditional + HybridBayesTreeClique::shared_ptr clique = node.second; + HybridConditional::shared_ptr conditional = clique->conditional(); + + KeyVector frontals(conditional->frontals().begin(), + conditional->frontals().end()); + + // Record the key being added + added_keys.insert(added_keys.end(), frontals.begin(), frontals.end()); + + // If conditional is hybrid (and not discrete-only), we get the Gaussian + // Conditional corresponding to the assignment and add it to the Gaussian + // Bayes Net. + if (conditional->isHybrid()) { + auto gm = conditional->asMixture(); + GaussianConditional::shared_ptr gaussian_conditional = + (*gm)(assignment); + + gbn.push_back(gaussian_conditional); + } + } + } + // Return the optimized bayes net. + return gbn.optimize(); +} + } // namespace gtsam diff --git a/gtsam/hybrid/HybridBayesTree.h b/gtsam/hybrid/HybridBayesTree.h index 02a4a11e5..b7950a483 100644 --- a/gtsam/hybrid/HybridBayesTree.h +++ b/gtsam/hybrid/HybridBayesTree.h @@ -70,6 +70,15 @@ class GTSAM_EXPORT HybridBayesTree : public BayesTree { /** Check equality */ bool equals(const This& other, double tol = 1e-9) const; + /** + * @brief Recursively optimize the BayesTree to produce a vector solution. + * + * @param assignment The discrete values assignment to select the Gaussian + * mixtures. + * @return VectorValues + */ + VectorValues optimize(const DiscreteValues& assignment) const; + /// @} }; diff --git a/gtsam/hybrid/tests/testHybridBayesTree.cpp b/gtsam/hybrid/tests/testHybridBayesTree.cpp new file mode 100644 index 000000000..ddc704460 --- /dev/null +++ b/gtsam/hybrid/tests/testHybridBayesTree.cpp @@ -0,0 +1,106 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testHybridBayesTree.cpp + * @brief Unit tests for HybridBayesTree + * @author Varun Agrawal + * @date August 2022 + */ + +#include +#include + +#include "Switching.h" + +// Include for test suite +#include + +using namespace std; +using namespace gtsam; +using noiseModel::Isotropic; +using symbol_shorthand::M; +using symbol_shorthand::X; + +/* ****************************************************************************/ +// Test for optimizing a HybridBayesTree. +TEST(HybridBayesTree, Optimize) { + Switching s(4); + + HybridGaussianISAM isam; + HybridGaussianFactorGraph graph1; + + // Add the 3 hybrid factors, x1-x2, x2-x3, x3-x4 + for (size_t i = 1; i < 4; i++) { + graph1.push_back(s.linearizedFactorGraph.at(i)); + } + + // Add the Gaussian factors, 1 prior on X(1), + // 3 measurements on X(2), X(3), X(4) + graph1.push_back(s.linearizedFactorGraph.at(0)); + for (size_t i = 4; i <= 7; i++) { + graph1.push_back(s.linearizedFactorGraph.at(i)); + } + + isam.update(graph1); + + DiscreteValues assignment; + assignment[M(1)] = 1; + assignment[M(2)] = 1; + assignment[M(3)] = 1; + + VectorValues delta = isam.optimize(assignment); + + // The linearization point has the same value as the key index, + // e.g. X(1) = 1, X(2) = 2, + // but the factors specify X(k) = k-1, so delta should be -1. + VectorValues expected_delta; + expected_delta.insert(make_pair(X(1), -Vector1::Ones())); + expected_delta.insert(make_pair(X(2), -Vector1::Ones())); + expected_delta.insert(make_pair(X(3), -Vector1::Ones())); + expected_delta.insert(make_pair(X(4), -Vector1::Ones())); + + EXPECT(assert_equal(expected_delta, delta)); + + // Create ordering. + Ordering ordering; + for (size_t k = 1; k <= s.K; k++) ordering += X(k); + + HybridBayesNet::shared_ptr hybridBayesNet; + HybridGaussianFactorGraph::shared_ptr remainingFactorGraph; + std::tie(hybridBayesNet, remainingFactorGraph) = + s.linearizedFactorGraph.eliminatePartialSequential(ordering); + + hybridBayesNet->print(); + GaussianBayesNet gbn = hybridBayesNet->choose(assignment); + + // EXPECT_LONGS_EQUAL(4, gbn.size()); + + // EXPECT(assert_equal(*(*boost::dynamic_pointer_cast( + // hybridBayesNet->atGaussian(0)))(assignment), + // *gbn.at(0))); + // EXPECT(assert_equal(*(*boost::dynamic_pointer_cast( + // hybridBayesNet->atGaussian(1)))(assignment), + // *gbn.at(1))); + // EXPECT(assert_equal(*(*boost::dynamic_pointer_cast( + // hybridBayesNet->atGaussian(2)))(assignment), + // *gbn.at(2))); + // EXPECT(assert_equal(*(*boost::dynamic_pointer_cast( + // hybridBayesNet->atGaussian(3)))(assignment), + // *gbn.at(3))); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ From 86320ff3b5082206849883df1d52e205a1a470be Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 26 Aug 2022 16:45:44 -0400 Subject: [PATCH 014/118] extra test for HybridBayesNet optimize --- gtsam/hybrid/HybridBayesNet.cpp | 10 ++++-- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 22 ++++++++++-- gtsam/hybrid/HybridGaussianFactorGraph.h | 9 +++++ gtsam/hybrid/tests/testHybridBayesNet.cpp | 39 +++++++++++++++++++++- 4 files changed, 74 insertions(+), 6 deletions(-) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index f93d21651..f898178c2 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -125,8 +125,14 @@ GaussianBayesNet HybridBayesNet::choose( const DiscreteValues &assignment) const { GaussianBayesNet gbn; for (size_t idx = 0; idx < size(); idx++) { - GaussianMixture gm = *this->atGaussian(idx); - gbn.push_back(gm(assignment)); + try { + GaussianMixture gm = *this->atGaussian(idx); + gbn.push_back(gm(assignment)); + + } catch (std::exception &exc) { + // if factor at `idx` is discrete-only, just continue. + continue; + } } return gbn; } diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index c024c1255..94e33890d 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -135,9 +135,9 @@ continuousElimination(const HybridGaussianFactorGraph &factors, for (auto &fp : factors) { if (auto ptr = boost::dynamic_pointer_cast(fp)) { gfg.push_back(ptr->inner()); - } else if (auto p = - boost::static_pointer_cast(fp)->inner()) { - gfg.push_back(boost::static_pointer_cast(p)); + } else if (auto ptr = boost::static_pointer_cast(fp)) { + gfg.push_back( + boost::static_pointer_cast(ptr->inner())); } else { // It is an orphan wrapped conditional } @@ -401,4 +401,20 @@ void HybridGaussianFactorGraph::add(DecisionTreeFactor::shared_ptr factor) { FactorGraph::add(boost::make_shared(factor)); } +/* ************************************************************************ */ +const Ordering HybridGaussianFactorGraph::getHybridOrdering( + OptionalOrderingType orderingType) const { + KeySet discrete_keys; + for (auto &factor : factors_) { + for (const DiscreteKey &k : factor->discreteKeys()) { + discrete_keys.insert(k.first); + } + } + + const VariableIndex index(factors_); + Ordering ordering = Ordering::ColamdConstrainedLast( + index, KeyVector(discrete_keys.begin(), discrete_keys.end()), true); + return ordering; +} + } // namespace gtsam diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h index 936710330..56f9b7e07 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.h +++ b/gtsam/hybrid/HybridGaussianFactorGraph.h @@ -160,6 +160,15 @@ class GTSAM_EXPORT HybridGaussianFactorGraph Base::push_back(sharedFactor); } } + + /** + * @brief + * + * @param orderingType + * @return const Ordering + */ + const Ordering getHybridOrdering( + OptionalOrderingType orderingType = boost::none) const; }; } // namespace gtsam diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp index d447bcce2..4602e8bac 100644 --- a/gtsam/hybrid/tests/testHybridBayesNet.cpp +++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp @@ -19,6 +19,7 @@ */ #include +#include #include "Switching.h" @@ -87,7 +88,7 @@ TEST(HybridBayesNet, Choose) { /* ****************************************************************************/ // Test bayes net optimize -TEST(HybridBayesNet, Optimize) { +TEST(HybridBayesNet, OptimizeAssignment) { Switching s(4); Ordering ordering; @@ -119,6 +120,42 @@ TEST(HybridBayesNet, Optimize) { EXPECT(assert_equal(expected_delta, delta)); } +/* ****************************************************************************/ +// Test bayes net optimize +TEST(HybridBayesNet, Optimize) { + Switching s(4); + + Ordering ordering; + for (auto&& kvp : s.linearizationPoint) { + ordering += kvp.key; + } + + Ordering hybridOrdering = s.linearizedFactorGraph.getHybridOrdering(); + HybridBayesNet::shared_ptr hybridBayesNet = + s.linearizedFactorGraph.eliminateSequential(hybridOrdering); + + HybridValues delta = hybridBayesNet->optimize(); + + delta.print(); + VectorValues correct; + correct.insert(X(1), 0 * Vector1::Ones()); + correct.insert(X(2), 1 * Vector1::Ones()); + correct.insert(X(3), 2 * Vector1::Ones()); + correct.insert(X(4), 3 * Vector1::Ones()); + + DiscreteValues assignment111; + assignment111[M(1)] = 1; + assignment111[M(2)] = 1; + assignment111[M(3)] = 1; + std::cout << hybridBayesNet->choose(assignment111).error(correct) << std::endl; + + DiscreteValues assignment101; + assignment101[M(1)] = 1; + assignment101[M(2)] = 0; + assignment101[M(3)] = 1; + std::cout << hybridBayesNet->choose(assignment101).error(correct) << std::endl; +} + /* ************************************************************************* */ int main() { TestResult tr; From 74bddd9582d650e7542255ccd86354a5e1918192 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 26 Aug 2022 18:42:08 -0400 Subject: [PATCH 015/118] finish HybridBayesTree test --- gtsam/hybrid/tests/testHybridBayesTree.cpp | 17 ++--------------- 1 file changed, 2 insertions(+), 15 deletions(-) diff --git a/gtsam/hybrid/tests/testHybridBayesTree.cpp b/gtsam/hybrid/tests/testHybridBayesTree.cpp index ddc704460..a693af1b2 100644 --- a/gtsam/hybrid/tests/testHybridBayesTree.cpp +++ b/gtsam/hybrid/tests/testHybridBayesTree.cpp @@ -79,23 +79,10 @@ TEST(HybridBayesTree, Optimize) { std::tie(hybridBayesNet, remainingFactorGraph) = s.linearizedFactorGraph.eliminatePartialSequential(ordering); - hybridBayesNet->print(); GaussianBayesNet gbn = hybridBayesNet->choose(assignment); + VectorValues expected = gbn.optimize(); - // EXPECT_LONGS_EQUAL(4, gbn.size()); - - // EXPECT(assert_equal(*(*boost::dynamic_pointer_cast( - // hybridBayesNet->atGaussian(0)))(assignment), - // *gbn.at(0))); - // EXPECT(assert_equal(*(*boost::dynamic_pointer_cast( - // hybridBayesNet->atGaussian(1)))(assignment), - // *gbn.at(1))); - // EXPECT(assert_equal(*(*boost::dynamic_pointer_cast( - // hybridBayesNet->atGaussian(2)))(assignment), - // *gbn.at(2))); - // EXPECT(assert_equal(*(*boost::dynamic_pointer_cast( - // hybridBayesNet->atGaussian(3)))(assignment), - // *gbn.at(3))); + EXPECT(assert_equal(expected, delta)); } /* ************************************************************************* */ From 129fa68627acbd9dfa9e1275a79c8399cb36cbd0 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 26 Aug 2022 18:42:31 -0400 Subject: [PATCH 016/118] use Switching constructor arguments for noise covariances --- gtsam/hybrid/tests/Switching.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/hybrid/tests/Switching.h b/gtsam/hybrid/tests/Switching.h index 52ef0439e..6f41d06ea 100644 --- a/gtsam/hybrid/tests/Switching.h +++ b/gtsam/hybrid/tests/Switching.h @@ -145,7 +145,7 @@ struct Switching { // Add "motion models". for (size_t k = 1; k < K; k++) { KeyVector keys = {X(k), X(k + 1)}; - auto motion_models = motionModels(k); + auto motion_models = motionModels(k, between_sigma); std::vector components; for (auto &&f : motion_models) { components.push_back(boost::dynamic_pointer_cast(f)); @@ -155,7 +155,7 @@ struct Switching { } // Add measurement factors - auto measurement_noise = noiseModel::Isotropic::Sigma(1, 0.1); + auto measurement_noise = noiseModel::Isotropic::Sigma(1, prior_sigma); for (size_t k = 2; k <= K; k++) { nonlinearFactorGraph.emplace_nonlinear>( X(k), 1.0 * (k - 1), measurement_noise); From 1b5daf9a0e9fe8afd526ffd2d843acd4585658f0 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 26 Aug 2022 19:34:54 -0400 Subject: [PATCH 017/118] remove HybridLookupDAG --- gtsam/hybrid/HybridLookupDAG.cpp | 76 ------ gtsam/hybrid/HybridLookupDAG.h | 119 --------- gtsam/hybrid/tests/testHybridLookupDAG.cpp | 272 --------------------- 3 files changed, 467 deletions(-) delete mode 100644 gtsam/hybrid/HybridLookupDAG.cpp delete mode 100644 gtsam/hybrid/HybridLookupDAG.h delete mode 100644 gtsam/hybrid/tests/testHybridLookupDAG.cpp diff --git a/gtsam/hybrid/HybridLookupDAG.cpp b/gtsam/hybrid/HybridLookupDAG.cpp deleted file mode 100644 index a322a8177..000000000 --- a/gtsam/hybrid/HybridLookupDAG.cpp +++ /dev/null @@ -1,76 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file DiscreteLookupDAG.cpp - * @date Aug, 2022 - * @author Shangjie Xue - */ - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -using std::pair; -using std::vector; - -namespace gtsam { - -/* ************************************************************************** */ -void HybridLookupTable::argmaxInPlace(HybridValues* values) const { - // For discrete conditional, uses argmaxInPlace() method in - // DiscreteLookupTable. - if (isDiscrete()) { - boost::static_pointer_cast(inner_)->argmaxInPlace( - &(values->discrete)); - } else if (isContinuous()) { - // For Gaussian conditional, uses solve() method in GaussianConditional. - values->continuous.insert( - boost::static_pointer_cast(inner_)->solve( - values->continuous)); - } else if (isHybrid()) { - // For hybrid conditional, since children should not contain discrete - // variable, we can condition on the discrete variable in the parents and - // solve the resulting GaussianConditional. - auto conditional = - boost::static_pointer_cast(inner_)->conditionals()( - values->discrete); - values->continuous.insert(conditional->solve(values->continuous)); - } -} - -/* ************************************************************************** */ -HybridLookupDAG HybridLookupDAG::FromBayesNet(const HybridBayesNet& bayesNet) { - HybridLookupDAG dag; - for (auto&& conditional : bayesNet) { - HybridLookupTable hlt(*conditional); - dag.push_back(hlt); - } - return dag; -} - -/* ************************************************************************** */ -HybridValues HybridLookupDAG::argmax(HybridValues result) const { - // Argmax each node in turn in topological sort order (parents first). - for (auto lookupTable : boost::adaptors::reverse(*this)) - lookupTable->argmaxInPlace(&result); - return result; -} - -} // namespace gtsam diff --git a/gtsam/hybrid/HybridLookupDAG.h b/gtsam/hybrid/HybridLookupDAG.h deleted file mode 100644 index cc1c58c58..000000000 --- a/gtsam/hybrid/HybridLookupDAG.h +++ /dev/null @@ -1,119 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file HybridLookupDAG.h - * @date Aug, 2022 - * @author Shangjie Xue - */ - -#pragma once - -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -namespace gtsam { - -/** - * @brief HybridLookupTable table for max-product - * - * Similar to DiscreteLookupTable, inherits from hybrid conditional for - * convenience. Is used in the max-product algorithm. - */ -class GTSAM_EXPORT HybridLookupTable : public HybridConditional { - public: - using Base = HybridConditional; - using This = HybridLookupTable; - using shared_ptr = boost::shared_ptr; - using BaseConditional = Conditional; - - /** - * @brief Construct a new Hybrid Lookup Table object form a HybridConditional. - * - * @param conditional input hybrid conditional - */ - HybridLookupTable(HybridConditional& conditional) : Base(conditional){}; - - /** - * @brief Calculate assignment for frontal variables that maximizes value. - * @param (in/out) parentsValues Known assignments for the parents. - */ - void argmaxInPlace(HybridValues* parentsValues) const; -}; - -/** A DAG made from hybrid lookup tables, as defined above. Similar to - * DiscreteLookupDAG */ -class GTSAM_EXPORT HybridLookupDAG : public BayesNet { - public: - using Base = BayesNet; - using This = HybridLookupDAG; - using shared_ptr = boost::shared_ptr; - - /// @name Standard Constructors - /// @{ - - /// Construct empty DAG. - HybridLookupDAG() {} - - /// Create from BayesNet with LookupTables - static HybridLookupDAG FromBayesNet(const HybridBayesNet& bayesNet); - - /// Destructor - virtual ~HybridLookupDAG() {} - - /// @} - - /// @name Standard Interface - /// @{ - - /** Add a DiscreteLookupTable */ - template - void add(Args&&... args) { - emplace_shared(std::forward(args)...); - } - - /** - * @brief argmax by back-substitution, optionally given certain variables. - * - * Assumes the DAG is reverse topologically sorted, i.e. last - * conditional will be optimized first *and* that the - * DAG does not contain any conditionals for the given variables. If the DAG - * resulted from eliminating a factor graph, this is true for the elimination - * ordering. - * - * @return given assignment extended w. optimal assignment for all variables. - */ - HybridValues argmax(HybridValues given = HybridValues()) const; - /// @} - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE& ar, const unsigned int /*version*/) { - ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - } -}; - -// traits -template <> -struct traits : public Testable {}; - -} // namespace gtsam diff --git a/gtsam/hybrid/tests/testHybridLookupDAG.cpp b/gtsam/hybrid/tests/testHybridLookupDAG.cpp deleted file mode 100644 index 0ab012d10..000000000 --- a/gtsam/hybrid/tests/testHybridLookupDAG.cpp +++ /dev/null @@ -1,272 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file testHybridLookupDAG.cpp - * @date Aug, 2022 - * @author Shangjie Xue - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -// Include for test suite -#include - -#include - -using namespace std; -using namespace gtsam; -using noiseModel::Isotropic; -using symbol_shorthand::M; -using symbol_shorthand::X; - -TEST(HybridLookupTable, basics) { - // create a conditional gaussian node - Matrix S1(2, 2); - S1(0, 0) = 1; - S1(1, 0) = 2; - S1(0, 1) = 3; - S1(1, 1) = 4; - - Matrix S2(2, 2); - S2(0, 0) = 6; - S2(1, 0) = 0.2; - S2(0, 1) = 8; - S2(1, 1) = 0.4; - - Matrix R1(2, 2); - R1(0, 0) = 0.1; - R1(1, 0) = 0.3; - R1(0, 1) = 0.0; - R1(1, 1) = 0.34; - - Matrix R2(2, 2); - R2(0, 0) = 0.1; - R2(1, 0) = 0.3; - R2(0, 1) = 0.0; - R2(1, 1) = 0.34; - - SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector2(1.0, 0.34)); - - Vector2 d1(0.2, 0.5), d2(0.5, 0.2); - - auto conditional0 = boost::make_shared(X(1), d1, R1, - X(2), S1, model), - conditional1 = boost::make_shared(X(1), d2, R2, - X(2), S2, model); - - // Create decision tree - DiscreteKey m1(1, 2); - GaussianMixture::Conditionals conditionals( - {m1}, - vector{conditional0, conditional1}); - // GaussianMixture mixtureFactor2({X(1)}, {X(2)}, {m1}, conditionals); - - boost::shared_ptr mixtureFactor( - new GaussianMixture({X(1)}, {X(2)}, {m1}, conditionals)); - - HybridConditional hc(mixtureFactor); - - GaussianMixture::Conditionals conditional2 = - boost::static_pointer_cast(hc.inner())->conditionals(); - - DiscreteValues dv; - dv[1] = 1; - - VectorValues cv; - cv.insert(X(2), Vector2(0.0, 0.0)); - - HybridValues hv(dv, cv); - - // std::cout << conditional2(values).markdown(); - EXPECT(assert_equal(*conditional2(dv), *conditionals(dv), 1e-6)); - EXPECT(conditional2(dv) == conditionals(dv)); - HybridLookupTable hlt(hc); - - // hlt.argmaxInPlace(&hv); - - HybridLookupDAG dag; - dag.push_back(hlt); - dag.argmax(hv); - - // HybridBayesNet hbn; - // hbn.push_back(hc); - // hbn.optimize(); -} - -TEST(HybridLookupTable, hybrid_argmax) { - Matrix S1(2, 2); - S1(0, 0) = 1; - S1(1, 0) = 0; - S1(0, 1) = 0; - S1(1, 1) = 1; - - Vector2 d1(0.2, 0.5), d2(-0.5, 0.6); - - SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector2(1.0, 0.34)); - - auto conditional0 = - boost::make_shared(X(1), d1, S1, model), - conditional1 = - boost::make_shared(X(1), d2, S1, model); - - DiscreteKey m1(1, 2); - GaussianMixture::Conditionals conditionals( - {m1}, - vector{conditional0, conditional1}); - boost::shared_ptr mixtureFactor( - new GaussianMixture({X(1)}, {}, {m1}, conditionals)); - - HybridConditional hc(mixtureFactor); - - DiscreteValues dv; - dv[1] = 1; - VectorValues cv; - // cv.insert(X(2),Vector2(0.0, 0.0)); - HybridValues hv(dv, cv); - - HybridLookupTable hlt(hc); - - hlt.argmaxInPlace(&hv); - - EXPECT(assert_equal(hv.at(X(1)), d2)); -} - -TEST(HybridLookupTable, discrete_argmax) { - DiscreteKey X(0, 2), Y(1, 2); - - auto conditional = boost::make_shared(X | Y = "0/1 3/2"); - - HybridConditional hc(conditional); - - HybridLookupTable hlt(hc); - - DiscreteValues dv; - dv[1] = 0; - VectorValues cv; - // cv.insert(X(2),Vector2(0.0, 0.0)); - HybridValues hv(dv, cv); - - hlt.argmaxInPlace(&hv); - - EXPECT(assert_equal(hv.atDiscrete(0), 1)); - - DecisionTreeFactor f1(X, "2 3"); - auto conditional2 = boost::make_shared(1, f1); - - HybridConditional hc2(conditional2); - - HybridLookupTable hlt2(hc2); - - HybridValues hv2; - - hlt2.argmaxInPlace(&hv2); - - EXPECT(assert_equal(hv2.atDiscrete(0), 1)); -} - -TEST(HybridLookupTable, gaussian_argmax) { - Matrix S1(2, 2); - S1(0, 0) = 1; - S1(1, 0) = 0; - S1(0, 1) = 0; - S1(1, 1) = 1; - - Vector2 d1(0.2, 0.5), d2(-0.5, 0.6); - - SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector2(1.0, 0.34)); - - auto conditional = - boost::make_shared(X(1), d1, S1, X(2), -S1, model); - - HybridConditional hc(conditional); - - HybridLookupTable hlt(hc); - - DiscreteValues dv; - // dv[1]=0; - VectorValues cv; - cv.insert(X(2), d2); - HybridValues hv(dv, cv); - - hlt.argmaxInPlace(&hv); - - EXPECT(assert_equal(hv.at(X(1)), d1 + d2)); -} - -TEST(HybridLookupDAG, argmax) { - Matrix S1(2, 2); - S1(0, 0) = 1; - S1(1, 0) = 0; - S1(0, 1) = 0; - S1(1, 1) = 1; - - Vector2 d1(0.2, 0.5), d2(-0.5, 0.6); - - SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector2(1.0, 0.34)); - - auto conditional0 = - boost::make_shared(X(2), d1, S1, model), - conditional1 = - boost::make_shared(X(2), d2, S1, model); - - DiscreteKey m1(1, 2); - GaussianMixture::Conditionals conditionals( - {m1}, - vector{conditional0, conditional1}); - boost::shared_ptr mixtureFactor( - new GaussianMixture({X(2)}, {}, {m1}, conditionals)); - HybridConditional hc2(mixtureFactor); - HybridLookupTable hlt2(hc2); - - auto conditional2 = - boost::make_shared(X(1), d1, S1, X(2), -S1, model); - - HybridConditional hc1(conditional2); - HybridLookupTable hlt1(hc1); - - DecisionTreeFactor f1(m1, "2 3"); - auto discrete_conditional = boost::make_shared(1, f1); - - HybridConditional hc3(discrete_conditional); - HybridLookupTable hlt3(hc3); - - HybridLookupDAG dag; - dag.push_back(hlt1); - dag.push_back(hlt2); - dag.push_back(hlt3); - auto hv = dag.argmax(); - - EXPECT(assert_equal(hv.atDiscrete(1), 1)); - EXPECT(assert_equal(hv.at(X(2)), d2)); - EXPECT(assert_equal(hv.at(X(1)), d2 + d1)); -} - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ From 9c7bf36db6e511d2eb9dda39929f42af2a00d288 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 26 Aug 2022 19:35:45 -0400 Subject: [PATCH 018/118] refactor HybridValues --- gtsam/hybrid/HybridValues.h | 54 ++++++++++++++++++++++++------------- 1 file changed, 36 insertions(+), 18 deletions(-) diff --git a/gtsam/hybrid/HybridValues.h b/gtsam/hybrid/HybridValues.h index 5e1bd4164..d5c78f951 100644 --- a/gtsam/hybrid/HybridValues.h +++ b/gtsam/hybrid/HybridValues.h @@ -36,40 +36,58 @@ namespace gtsam { * Optimizing a HybridGaussianBayesNet returns this class. */ class GTSAM_EXPORT HybridValues { - public: + private: // DiscreteValue stored the discrete components of the HybridValues. - DiscreteValues discrete; + DiscreteValues discrete_; // VectorValue stored the continuous components of the HybridValues. - VectorValues continuous; + VectorValues continuous_; + + public: + /// @name Standard Constructors + /// @{ // Default constructor creates an empty HybridValues. - HybridValues() : discrete(), continuous(){}; + HybridValues() = default; // Construct from DiscreteValues and VectorValues. HybridValues(const DiscreteValues& dv, const VectorValues& cv) - : discrete(dv), continuous(cv){}; + : discrete_(dv), continuous_(cv){}; + + /// @} + /// @name Testable + /// @{ // print required by Testable for unit testing void print(const std::string& s = "HybridValues", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { std::cout << s << ": \n"; - discrete.print(" Discrete", keyFormatter); // print discrete components - continuous.print(" Continuous", - keyFormatter); // print continuous components + discrete_.print(" Discrete", keyFormatter); // print discrete components + continuous_.print(" Continuous", + keyFormatter); // print continuous components }; // equals required by Testable for unit testing bool equals(const HybridValues& other, double tol = 1e-9) const { - return discrete.equals(other.discrete, tol) && - continuous.equals(other.continuous, tol); + return discrete_.equals(other.discrete_, tol) && + continuous_.equals(other.continuous_, tol); } + /// @} + /// @name Interface + /// @{ + + /// Return the discrete MPE assignment + DiscreteValues discrete() const { return discrete_; } + + /// Return the delta update for the continuous vectors + VectorValues continuous() const { return continuous_; } + // Check whether a variable with key \c j exists in DiscreteValue. - bool existsDiscrete(Key j) { return (discrete.find(j) != discrete.end()); }; + bool existsDiscrete(Key j) { return (discrete_.find(j) != discrete_.end()); }; // Check whether a variable with key \c j exists in VectorValue. - bool existsVector(Key j) { return continuous.exists(j); }; + bool existsVector(Key j) { return continuous_.exists(j); }; // Check whether a variable with key \c j exists. bool exists(Key j) { return existsDiscrete(j) || existsVector(j); }; @@ -78,13 +96,13 @@ class GTSAM_EXPORT HybridValues { * the key \c j is already used. * @param value The vector to be inserted. * @param j The index with which the value will be associated. */ - void insert(Key j, int value) { discrete[j] = value; }; + void insert(Key j, int value) { discrete_[j] = value; }; /** Insert a vector \c value with key \c j. Throws an invalid_argument * exception if the key \c j is already used. * @param value The vector to be inserted. * @param j The index with which the value will be associated. */ - void insert(Key j, const Vector& value) { continuous.insert(j, value); } + void insert(Key j, const Vector& value) { continuous_.insert(j, value); } // TODO(Shangjie)- update() and insert_or_assign() , similar to Values.h @@ -92,13 +110,13 @@ class GTSAM_EXPORT HybridValues { * Read/write access to the discrete value with key \c j, throws * std::out_of_range if \c j does not exist. */ - size_t& atDiscrete(Key j) { return discrete.at(j); }; + size_t& atDiscrete(Key j) { return discrete_.at(j); }; /** * Read/write access to the vector value with key \c j, throws * std::out_of_range if \c j does not exist. */ - Vector& at(Key j) { return continuous.at(j); }; + Vector& at(Key j) { return continuous_.at(j); }; /// @name Wrapper support /// @{ @@ -112,8 +130,8 @@ class GTSAM_EXPORT HybridValues { std::string html( const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { std::stringstream ss; - ss << this->discrete.html(keyFormatter); - ss << this->continuous.html(keyFormatter); + ss << this->discrete_.html(keyFormatter); + ss << this->continuous_.html(keyFormatter); return ss.str(); }; From 0edcfd4ff88303f8911622de89be4f70db434faf Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 26 Aug 2022 19:36:11 -0400 Subject: [PATCH 019/118] new HybridBayesNet optimize implementation --- gtsam/hybrid/HybridBayesNet.cpp | 18 ++++++++++++--- gtsam/hybrid/tests/testHybridBayesNet.cpp | 27 +++++++++-------------- 2 files changed, 26 insertions(+), 19 deletions(-) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index f898178c2..2238b08ce 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -15,8 +15,9 @@ * @date January 2022 */ +#include +#include #include -#include #include namespace gtsam { @@ -139,8 +140,19 @@ GaussianBayesNet HybridBayesNet::choose( /* *******************************************************************************/ HybridValues HybridBayesNet::optimize() const { - auto dag = HybridLookupDAG::FromBayesNet(*this); - return dag.argmax(); + // Solve for the MPE + DiscreteBayesNet discrete_bn; + for (auto &conditional : factors_) { + if (conditional->isDiscrete()) { + discrete_bn.push_back(conditional->asDiscreteConditional()); + } + } + + DiscreteValues mpe = DiscreteFactorGraph(discrete_bn).optimize(); + + // Given the MPE, compute the optimal continuous values. + GaussianBayesNet gbn = this->choose(mpe); + return HybridValues(mpe, gbn.optimize()); } /* *******************************************************************************/ diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp index 4602e8bac..bcaf7e599 100644 --- a/gtsam/hybrid/tests/testHybridBayesNet.cpp +++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp @@ -136,24 +136,19 @@ TEST(HybridBayesNet, Optimize) { HybridValues delta = hybridBayesNet->optimize(); - delta.print(); - VectorValues correct; - correct.insert(X(1), 0 * Vector1::Ones()); - correct.insert(X(2), 1 * Vector1::Ones()); - correct.insert(X(3), 2 * Vector1::Ones()); - correct.insert(X(4), 3 * Vector1::Ones()); + DiscreteValues expectedAssignment; + expectedAssignment[M(1)] = 1; + expectedAssignment[M(2)] = 0; + expectedAssignment[M(3)] = 1; + EXPECT(assert_equal(expectedAssignment, delta.discrete())); - DiscreteValues assignment111; - assignment111[M(1)] = 1; - assignment111[M(2)] = 1; - assignment111[M(3)] = 1; - std::cout << hybridBayesNet->choose(assignment111).error(correct) << std::endl; + VectorValues expectedValues; + expectedValues.insert(X(1), -0.999904 * Vector1::Ones()); + expectedValues.insert(X(2), -0.99029 * Vector1::Ones()); + expectedValues.insert(X(3), -1.00971 * Vector1::Ones()); + expectedValues.insert(X(4), -1.0001 * Vector1::Ones()); - DiscreteValues assignment101; - assignment101[M(1)] = 1; - assignment101[M(2)] = 0; - assignment101[M(3)] = 1; - std::cout << hybridBayesNet->choose(assignment101).error(correct) << std::endl; + EXPECT(assert_equal(expectedValues, delta.continuous(), 1e-5)); } /* ************************************************************************* */ From 5169b2ec30577a9bb367a6125310251c04b056e2 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 26 Aug 2022 22:13:44 -0400 Subject: [PATCH 020/118] add HybridBayesTree optimize method --- gtsam/hybrid/HybridBayesTree.cpp | 44 +++++++++++++++-- gtsam/hybrid/HybridBayesTree.h | 9 ++++ gtsam/hybrid/tests/testHybridBayesNet.cpp | 5 -- gtsam/hybrid/tests/testHybridBayesTree.cpp | 57 +++++++++++++++++++++- 4 files changed, 104 insertions(+), 11 deletions(-) diff --git a/gtsam/hybrid/HybridBayesTree.cpp b/gtsam/hybrid/HybridBayesTree.cpp index 30fe6f168..e9c15b0ed 100644 --- a/gtsam/hybrid/HybridBayesTree.cpp +++ b/gtsam/hybrid/HybridBayesTree.cpp @@ -18,6 +18,8 @@ */ #include +#include +#include #include #include #include @@ -35,6 +37,42 @@ bool HybridBayesTree::equals(const This& other, double tol) const { return Base::equals(other, tol); } +/* ************************************************************************* */ +HybridValues HybridBayesTree::optimize() const { + HybridBayesNet hbn; + DiscreteBayesNet dbn; + + KeyVector added_keys; + + // Iterate over all the nodes in the BayesTree + for (auto&& node : nodes()) { + // Check if conditional being added is already in the Bayes net. + if (std::find(added_keys.begin(), added_keys.end(), node.first) == + added_keys.end()) { + // Access the clique and get the underlying hybrid conditional + HybridBayesTreeClique::shared_ptr clique = node.second; + HybridConditional::shared_ptr conditional = clique->conditional(); + + // Record the key being added + added_keys.insert(added_keys.end(), conditional->frontals().begin(), + conditional->frontals().end()); + + if (conditional->isHybrid()) { + // If conditional is hybrid, add it to a Hybrid Bayes net. + hbn.push_back(conditional); + } else if (conditional->isDiscrete()) { + // Else if discrete, we use it to compute the MPE + dbn.push_back(conditional->asDiscreteConditional()); + } + } + } + // Get the MPE + DiscreteValues mpe = DiscreteFactorGraph(dbn).optimize(); + // Given the MPE, compute the optimal continuous values. + GaussianBayesNet gbn = hbn.choose(mpe); + return HybridValues(mpe, gbn.optimize()); +} + /* ************************************************************************* */ VectorValues HybridBayesTree::optimize(const DiscreteValues& assignment) const { GaussianBayesNet gbn; @@ -50,11 +88,9 @@ VectorValues HybridBayesTree::optimize(const DiscreteValues& assignment) const { HybridBayesTreeClique::shared_ptr clique = node.second; HybridConditional::shared_ptr conditional = clique->conditional(); - KeyVector frontals(conditional->frontals().begin(), - conditional->frontals().end()); - // Record the key being added - added_keys.insert(added_keys.end(), frontals.begin(), frontals.end()); + added_keys.insert(added_keys.end(), conditional->frontals().begin(), + conditional->frontals().end()); // If conditional is hybrid (and not discrete-only), we get the Gaussian // Conditional corresponding to the assignment and add it to the Gaussian diff --git a/gtsam/hybrid/HybridBayesTree.h b/gtsam/hybrid/HybridBayesTree.h index b7950a483..361fbe86f 100644 --- a/gtsam/hybrid/HybridBayesTree.h +++ b/gtsam/hybrid/HybridBayesTree.h @@ -70,6 +70,15 @@ class GTSAM_EXPORT HybridBayesTree : public BayesTree { /** Check equality */ bool equals(const This& other, double tol = 1e-9) const; + /** + * @brief Optimize the hybrid Bayes tree by computing the MPE for the current + * set of discrete variables and using it to compute the best continuous + * update delta. + * + * @return HybridValues + */ + HybridValues optimize() const; + /** * @brief Recursively optimize the BayesTree to produce a vector solution. * diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp index bcaf7e599..56f3680ae 100644 --- a/gtsam/hybrid/tests/testHybridBayesNet.cpp +++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp @@ -125,11 +125,6 @@ TEST(HybridBayesNet, OptimizeAssignment) { TEST(HybridBayesNet, Optimize) { Switching s(4); - Ordering ordering; - for (auto&& kvp : s.linearizationPoint) { - ordering += kvp.key; - } - Ordering hybridOrdering = s.linearizedFactorGraph.getHybridOrdering(); HybridBayesNet::shared_ptr hybridBayesNet = s.linearizedFactorGraph.eliminateSequential(hybridOrdering); diff --git a/gtsam/hybrid/tests/testHybridBayesTree.cpp b/gtsam/hybrid/tests/testHybridBayesTree.cpp index a693af1b2..3584d7db9 100644 --- a/gtsam/hybrid/tests/testHybridBayesTree.cpp +++ b/gtsam/hybrid/tests/testHybridBayesTree.cpp @@ -16,6 +16,7 @@ * @date August 2022 */ +#include #include #include @@ -31,8 +32,8 @@ using symbol_shorthand::M; using symbol_shorthand::X; /* ****************************************************************************/ -// Test for optimizing a HybridBayesTree. -TEST(HybridBayesTree, Optimize) { +// Test for optimizing a HybridBayesTree with a given assignment. +TEST(HybridBayesTree, OptimizeAssignment) { Switching s(4); HybridGaussianISAM isam; @@ -85,6 +86,58 @@ TEST(HybridBayesTree, Optimize) { EXPECT(assert_equal(expected, delta)); } +/* ****************************************************************************/ +// Test for optimizing a HybridBayesTree. +TEST(HybridBayesTree, Optimize) { + Switching s(4); + + HybridGaussianISAM isam; + HybridGaussianFactorGraph graph1; + + // Add the 3 hybrid factors, x1-x2, x2-x3, x3-x4 + for (size_t i = 1; i < 4; i++) { + graph1.push_back(s.linearizedFactorGraph.at(i)); + } + + // Add the Gaussian factors, 1 prior on X(1), + // 3 measurements on X(2), X(3), X(4) + graph1.push_back(s.linearizedFactorGraph.at(0)); + for (size_t i = 4; i <= 6; i++) { + graph1.push_back(s.linearizedFactorGraph.at(i)); + } + + // Add the discrete factors + for (size_t i = 7; i <= 9; i++) { + graph1.push_back(s.linearizedFactorGraph.at(i)); + } + + isam.update(graph1); + + HybridValues delta = isam.optimize(); + + // Create ordering. + Ordering ordering; + for (size_t k = 1; k <= s.K; k++) ordering += X(k); + + HybridBayesNet::shared_ptr hybridBayesNet; + HybridGaussianFactorGraph::shared_ptr remainingFactorGraph; + std::tie(hybridBayesNet, remainingFactorGraph) = + s.linearizedFactorGraph.eliminatePartialSequential(ordering); + + DiscreteFactorGraph dfg; + for (auto&& f : *remainingFactorGraph) { + auto factor = dynamic_pointer_cast(f); + dfg.push_back( + boost::dynamic_pointer_cast(factor->inner())); + } + + DiscreteValues expectedMPE = dfg.optimize(); + VectorValues expectedValues = hybridBayesNet->optimize(expectedMPE); + + EXPECT(assert_equal(expectedMPE, delta.discrete())); + EXPECT(assert_equal(expectedValues, delta.continuous())); +} + /* ************************************************************************* */ int main() { TestResult tr; From 8645ec0f259240c5ad3f21f5abd9a8c6af11edaa Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 27 Aug 2022 15:43:13 -0400 Subject: [PATCH 021/118] update wrapper for HybridValues --- gtsam/hybrid/hybrid.i | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i index aa63259d9..dfbf3919d 100644 --- a/gtsam/hybrid/hybrid.i +++ b/gtsam/hybrid/hybrid.i @@ -6,8 +6,8 @@ namespace gtsam { #include class HybridValues { - gtsam::DiscreteValues discrete; - gtsam::VectorValues continuous; + gtsam::DiscreteValues discrete() const; + gtsam::VectorValues continuous() const; HybridValues(); HybridValues(const gtsam::DiscreteValues &dv, const gtsam::VectorValues &cv); void print(string s = "HybridValues", From 31bbf513f46cbf0909f5991dbbe5a246e195f6d5 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 27 Aug 2022 19:23:27 -0400 Subject: [PATCH 022/118] add discrete factors to HybridBayesTree test --- gtsam/hybrid/tests/testHybridBayesTree.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/gtsam/hybrid/tests/testHybridBayesTree.cpp b/gtsam/hybrid/tests/testHybridBayesTree.cpp index 3584d7db9..d457e6b74 100644 --- a/gtsam/hybrid/tests/testHybridBayesTree.cpp +++ b/gtsam/hybrid/tests/testHybridBayesTree.cpp @@ -51,6 +51,11 @@ TEST(HybridBayesTree, OptimizeAssignment) { graph1.push_back(s.linearizedFactorGraph.at(i)); } + // Add the discrete factors + for (size_t i = 7; i <= 9; i++) { + graph1.push_back(s.linearizedFactorGraph.at(i)); + } + isam.update(graph1); DiscreteValues assignment; From 72722686bae45d771455692b6b98c17395bcd87b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 27 Aug 2022 19:23:59 -0400 Subject: [PATCH 023/118] fix issue with TBB --- gtsam/hybrid/HybridBayesTree.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/gtsam/hybrid/HybridBayesTree.cpp b/gtsam/hybrid/HybridBayesTree.cpp index 30fe6f168..0a9b9e7f2 100644 --- a/gtsam/hybrid/HybridBayesTree.cpp +++ b/gtsam/hybrid/HybridBayesTree.cpp @@ -68,6 +68,13 @@ VectorValues HybridBayesTree::optimize(const DiscreteValues& assignment) const { } } } + // If TBB is enabled, the bayes net order gets reversed, + // so we pre-reverse it +#ifdef GTSAM_USE_TBB + auto reversed = boost::adaptors::reverse(gbn); + gbn = GaussianBayesNet(reversed.begin(), reversed.end()); +#endif + // Return the optimized bayes net. return gbn.optimize(); } From 496ddf886fcde5dac0053ff5c22daee428c92d2d Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 28 Aug 2022 09:55:44 -0400 Subject: [PATCH 024/118] fix gbn optimize everywhere for HybridBayesTree --- gtsam/hybrid/HybridBayesTree.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/gtsam/hybrid/HybridBayesTree.cpp b/gtsam/hybrid/HybridBayesTree.cpp index 678b4ca34..d0f6f2fe2 100644 --- a/gtsam/hybrid/HybridBayesTree.cpp +++ b/gtsam/hybrid/HybridBayesTree.cpp @@ -70,6 +70,14 @@ HybridValues HybridBayesTree::optimize() const { DiscreteValues mpe = DiscreteFactorGraph(dbn).optimize(); // Given the MPE, compute the optimal continuous values. GaussianBayesNet gbn = hbn.choose(mpe); + + // If TBB is enabled, the bayes net order gets reversed, + // so we pre-reverse it +#ifdef GTSAM_USE_TBB + auto reversed = boost::adaptors::reverse(gbn); + gbn = GaussianBayesNet(reversed.begin(), reversed.end()); +#endif + return HybridValues(mpe, gbn.optimize()); } @@ -104,6 +112,7 @@ VectorValues HybridBayesTree::optimize(const DiscreteValues& assignment) const { } } } + // If TBB is enabled, the bayes net order gets reversed, // so we pre-reverse it #ifdef GTSAM_USE_TBB From 6912d015ef898de8f17eefa834d0d72b7c8933a4 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 29 Aug 2022 09:26:18 -0400 Subject: [PATCH 025/118] assert for discrete only conditional --- gtsam/hybrid/HybridBayesNet.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index 2238b08ce..c108a47c2 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -131,7 +131,9 @@ GaussianBayesNet HybridBayesNet::choose( gbn.push_back(gm(assignment)); } catch (std::exception &exc) { - // if factor at `idx` is discrete-only, just continue. + // factor at `idx` is discrete-only, so we simply continue. + assert(factors_.at(idx)->discreteKeys().size() == + factors_.at(idx)->keys().size()); continue; } } From 4e451d5c0b9642279cb47d3814f2fbb8982a025e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 29 Aug 2022 16:22:55 -0400 Subject: [PATCH 026/118] helper to return HybridConditional as GaussianConditional --- gtsam/hybrid/HybridConditional.h | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/gtsam/hybrid/HybridConditional.h b/gtsam/hybrid/HybridConditional.h index 91c9f8495..96ea6d969 100644 --- a/gtsam/hybrid/HybridConditional.h +++ b/gtsam/hybrid/HybridConditional.h @@ -139,6 +139,17 @@ class GTSAM_EXPORT HybridConditional return boost::static_pointer_cast(inner_); } + /** + * @brief Return HybridConditional as a GaussianConditional + * + * @return GaussianConditional::shared_ptr + */ + GaussianConditional::shared_ptr asGaussian() { + if (!isContinuous()) + throw std::invalid_argument("Not a continuous conditional"); + return boost::static_pointer_cast(inner_); + } + /** * @brief Return conditional as a DiscreteConditional * From 8c41f63167ade58f73131b922b7fd533e605e020 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 29 Aug 2022 16:24:26 -0400 Subject: [PATCH 027/118] rename atGaussian to atMixture and add new atGaussian for continuous conditionals, fix choose method for all types --- gtsam/hybrid/HybridBayesNet.cpp | 24 ++++++++++++++++------- gtsam/hybrid/HybridBayesNet.h | 5 ++++- gtsam/hybrid/tests/testHybridBayesNet.cpp | 8 ++++---- 3 files changed, 25 insertions(+), 12 deletions(-) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index c108a47c2..787c91a0d 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -112,10 +112,15 @@ HybridBayesNet HybridBayesNet::prune( } /* ************************************************************************* */ -GaussianMixture::shared_ptr HybridBayesNet::atGaussian(size_t i) const { +GaussianMixture::shared_ptr HybridBayesNet::atMixture(size_t i) const { return factors_.at(i)->asMixture(); } +/* ************************************************************************* */ +GaussianConditional::shared_ptr HybridBayesNet::atGaussian(size_t i) const { + return factors_.at(i)->asGaussian(); +} + /* ************************************************************************* */ DiscreteConditional::shared_ptr HybridBayesNet::atDiscrete(size_t i) const { return factors_.at(i)->asDiscreteConditional(); @@ -126,17 +131,22 @@ GaussianBayesNet HybridBayesNet::choose( const DiscreteValues &assignment) const { GaussianBayesNet gbn; for (size_t idx = 0; idx < size(); idx++) { - try { - GaussianMixture gm = *this->atGaussian(idx); + if (factors_.at(idx)->isHybrid()) { + // If factor is hybrid, select based on assignment. + GaussianMixture gm = *this->atMixture(idx); gbn.push_back(gm(assignment)); - } catch (std::exception &exc) { - // factor at `idx` is discrete-only, so we simply continue. - assert(factors_.at(idx)->discreteKeys().size() == - factors_.at(idx)->keys().size()); + } else if (factors_.at(idx)->isContinuous()) { + // If continuous only, add gaussian conditional. + factors_.at(idx)->print(); + gbn.push_back((this->atGaussian(idx))); + + } else if (factors_.at(idx)->isDiscrete()) { + // If factor at `idx` is discrete-only, we simply continue. continue; } } + return gbn; } diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h index a16a4f42c..616ea0698 100644 --- a/gtsam/hybrid/HybridBayesNet.h +++ b/gtsam/hybrid/HybridBayesNet.h @@ -54,7 +54,10 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { } /// Get a specific Gaussian mixture by index `i`. - GaussianMixture::shared_ptr atGaussian(size_t i) const; + GaussianMixture::shared_ptr atMixture(size_t i) const; + + /// Get a specific Gaussian conditional by index `i`. + GaussianConditional::shared_ptr atGaussian(size_t i) const; /// Get a specific discrete conditional by index `i`. DiscreteConditional::shared_ptr atDiscrete(size_t i) const; diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp index 56f3680ae..c7516c0f6 100644 --- a/gtsam/hybrid/tests/testHybridBayesNet.cpp +++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp @@ -73,16 +73,16 @@ TEST(HybridBayesNet, Choose) { EXPECT_LONGS_EQUAL(4, gbn.size()); EXPECT(assert_equal(*(*boost::dynamic_pointer_cast( - hybridBayesNet->atGaussian(0)))(assignment), + hybridBayesNet->atMixture(0)))(assignment), *gbn.at(0))); EXPECT(assert_equal(*(*boost::dynamic_pointer_cast( - hybridBayesNet->atGaussian(1)))(assignment), + hybridBayesNet->atMixture(1)))(assignment), *gbn.at(1))); EXPECT(assert_equal(*(*boost::dynamic_pointer_cast( - hybridBayesNet->atGaussian(2)))(assignment), + hybridBayesNet->atMixture(2)))(assignment), *gbn.at(2))); EXPECT(assert_equal(*(*boost::dynamic_pointer_cast( - hybridBayesNet->atGaussian(3)))(assignment), + hybridBayesNet->atMixture(3)))(assignment), *gbn.at(3))); } From ca4293b70ddcdadb2de7c1107856519243a128d6 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 31 Aug 2022 10:36:07 -0400 Subject: [PATCH 028/118] handle Gaussian conditionals in the BayesTree --- gtsam/hybrid/HybridBayesNet.cpp | 1 - gtsam/hybrid/HybridBayesTree.cpp | 16 +++++++++++----- 2 files changed, 11 insertions(+), 6 deletions(-) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index 787c91a0d..bca50a902 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -138,7 +138,6 @@ GaussianBayesNet HybridBayesNet::choose( } else if (factors_.at(idx)->isContinuous()) { // If continuous only, add gaussian conditional. - factors_.at(idx)->print(); gbn.push_back((this->atGaussian(idx))); } else if (factors_.at(idx)->isDiscrete()) { diff --git a/gtsam/hybrid/HybridBayesTree.cpp b/gtsam/hybrid/HybridBayesTree.cpp index d0f6f2fe2..49ae0bd87 100644 --- a/gtsam/hybrid/HybridBayesTree.cpp +++ b/gtsam/hybrid/HybridBayesTree.cpp @@ -57,12 +57,14 @@ HybridValues HybridBayesTree::optimize() const { added_keys.insert(added_keys.end(), conditional->frontals().begin(), conditional->frontals().end()); - if (conditional->isHybrid()) { - // If conditional is hybrid, add it to a Hybrid Bayes net. - hbn.push_back(conditional); - } else if (conditional->isDiscrete()) { - // Else if discrete, we use it to compute the MPE + if (conditional->isDiscrete()) { + // If discrete, we use it to compute the MPE dbn.push_back(conditional->asDiscreteConditional()); + + } else { + // Else conditional is hybrid or continuous-only, + // so we directly add it to the Hybrid Bayes net. + hbn.push_back(conditional); } } } @@ -109,6 +111,10 @@ VectorValues HybridBayesTree::optimize(const DiscreteValues& assignment) const { (*gm)(assignment); gbn.push_back(gaussian_conditional); + + } else if (conditional->isContinuous()) { + // If conditional is Gaussian, we simply add it to the Bayes net. + gbn.push_back(conditional->asGaussian()); } } } From fe595bf745b9b386f1cc0d7012800de58afd024f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 31 Aug 2022 12:25:20 -0400 Subject: [PATCH 029/118] fix doxygen --- gtsam/hybrid/HybridValues.h | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/gtsam/hybrid/HybridValues.h b/gtsam/hybrid/HybridValues.h index d5c78f951..4928f9384 100644 --- a/gtsam/hybrid/HybridValues.h +++ b/gtsam/hybrid/HybridValues.h @@ -31,8 +31,8 @@ namespace gtsam { /** - * HybridValues represents a collection of DiscreteValues and VectorValues. It - * is typically used to store the variables of a HybridGaussianFactorGraph. + * HybridValues represents a collection of DiscreteValues and VectorValues. + * It is typically used to store the variables of a HybridGaussianFactorGraph. * Optimizing a HybridGaussianBayesNet returns this class. */ class GTSAM_EXPORT HybridValues { @@ -47,10 +47,10 @@ class GTSAM_EXPORT HybridValues { /// @name Standard Constructors /// @{ - // Default constructor creates an empty HybridValues. + /// Default constructor creates an empty HybridValues. HybridValues() = default; - // Construct from DiscreteValues and VectorValues. + /// Construct from DiscreteValues and VectorValues. HybridValues(const DiscreteValues& dv, const VectorValues& cv) : discrete_(dv), continuous_(cv){}; @@ -58,7 +58,7 @@ class GTSAM_EXPORT HybridValues { /// @name Testable /// @{ - // print required by Testable for unit testing + /// print required by Testable for unit testing void print(const std::string& s = "HybridValues", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { std::cout << s << ": \n"; @@ -67,7 +67,7 @@ class GTSAM_EXPORT HybridValues { keyFormatter); // print continuous components }; - // equals required by Testable for unit testing + /// equals required by Testable for unit testing bool equals(const HybridValues& other, double tol = 1e-9) const { return discrete_.equals(other.discrete_, tol) && continuous_.equals(other.continuous_, tol); @@ -83,13 +83,13 @@ class GTSAM_EXPORT HybridValues { /// Return the delta update for the continuous vectors VectorValues continuous() const { return continuous_; } - // Check whether a variable with key \c j exists in DiscreteValue. + /// Check whether a variable with key \c j exists in DiscreteValue. bool existsDiscrete(Key j) { return (discrete_.find(j) != discrete_.end()); }; - // Check whether a variable with key \c j exists in VectorValue. + /// Check whether a variable with key \c j exists in VectorValue. bool existsVector(Key j) { return continuous_.exists(j); }; - // Check whether a variable with key \c j exists. + /// Check whether a variable with key \c j exists. bool exists(Key j) { return existsDiscrete(j) || existsVector(j); }; /** Insert a discrete \c value with key \c j. Replaces the existing value if From 4852949b754195c1974110e95ff3003a3f31ac4f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 31 Aug 2022 12:25:57 -0400 Subject: [PATCH 030/118] add helpers to get different types of keys from the hybrid graph --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 25 +++++++++++++++++-- gtsam/hybrid/HybridGaussianFactorGraph.h | 17 ++++++++----- .../tests/testGaussianHybridFactorGraph.cpp | 9 +++---- 3 files changed, 38 insertions(+), 13 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 94e33890d..c031b9729 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -402,14 +402,35 @@ void HybridGaussianFactorGraph::add(DecisionTreeFactor::shared_ptr factor) { } /* ************************************************************************ */ -const Ordering HybridGaussianFactorGraph::getHybridOrdering( - OptionalOrderingType orderingType) const { +const KeySet HybridGaussianFactorGraph::getDiscreteKeys() const { KeySet discrete_keys; for (auto &factor : factors_) { for (const DiscreteKey &k : factor->discreteKeys()) { discrete_keys.insert(k.first); } } + return discrete_keys; +} + +/* ************************************************************************ */ +const KeySet HybridGaussianFactorGraph::getContinuousKeys() const { + KeySet keys; + for (auto &factor : factors_) { + for (const Key &key : factor->continuousKeys()) { + keys.insert(key); + } + } + return keys; +} + +/* ************************************************************************ */ +const Ordering HybridGaussianFactorGraph::getHybridOrdering() const { + KeySet discrete_keys = getDiscreteKeys(); + for (auto &factor : factors_) { + for (const DiscreteKey &k : factor->discreteKeys()) { + discrete_keys.insert(k.first); + } + } const VariableIndex index(factors_); Ordering ordering = Ordering::ColamdConstrainedLast( diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h index 56f9b7e07..ad5cde09b 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.h +++ b/gtsam/hybrid/HybridGaussianFactorGraph.h @@ -161,14 +161,19 @@ class GTSAM_EXPORT HybridGaussianFactorGraph } } + /// Get all the discrete keys in the factor graph. + const KeySet getDiscreteKeys() const; + + /// Get all the continuous keys in the factor graph. + const KeySet getContinuousKeys() const; + /** - * @brief - * - * @param orderingType - * @return const Ordering + * @brief Return a Colamd constrained ordering where the discrete keys are + * eliminated after the continuous keys. + * + * @return const Ordering */ - const Ordering getHybridOrdering( - OptionalOrderingType orderingType = boost::none) const; + const Ordering getHybridOrdering() const; }; } // namespace gtsam diff --git a/gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp b/gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp index e4bd0e084..40da42412 100644 --- a/gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp +++ b/gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp @@ -184,8 +184,8 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalSimple) { hfg.add(DecisionTreeFactor(m1, {2, 8})); hfg.add(DecisionTreeFactor({{M(1), 2}, {M(2), 2}}, "1 2 3 4")); - HybridBayesTree::shared_ptr result = hfg.eliminateMultifrontal( - Ordering::ColamdConstrainedLast(hfg, {M(1), M(2)})); + HybridBayesTree::shared_ptr result = + hfg.eliminateMultifrontal(hfg.getHybridOrdering()); // The bayes tree should have 3 cliques EXPECT_LONGS_EQUAL(3, result->size()); @@ -215,7 +215,7 @@ TEST(HybridGaussianFactorGraph, eliminateFullMultifrontalCLG) { hfg.add(HybridDiscreteFactor(DecisionTreeFactor(m, {2, 8}))); // Get a constrained ordering keeping c1 last - auto ordering_full = Ordering::ColamdConstrainedLast(hfg, {M(1)}); + auto ordering_full = hfg.getHybridOrdering(); // Returns a Hybrid Bayes Tree with distribution P(x0|x1)P(x1|c1)P(c1) HybridBayesTree::shared_ptr hbt = hfg.eliminateMultifrontal(ordering_full); @@ -484,8 +484,7 @@ TEST(HybridGaussianFactorGraph, SwitchingTwoVar) { } HybridBayesNet::shared_ptr hbn; HybridGaussianFactorGraph::shared_ptr remaining; - std::tie(hbn, remaining) = - hfg->eliminatePartialSequential(ordering_partial); + std::tie(hbn, remaining) = hfg->eliminatePartialSequential(ordering_partial); EXPECT_LONGS_EQUAL(14, hbn->size()); EXPECT_LONGS_EQUAL(11, remaining->size()); From 5bc7d3333f21941620514a5298e49e788df495c6 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 31 Aug 2022 12:26:29 -0400 Subject: [PATCH 031/118] wrap BayesTree::optimize --- gtsam/hybrid/hybrid.i | 2 ++ 1 file changed, 2 insertions(+) diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i index dfbf3919d..86029a48a 100644 --- a/gtsam/hybrid/hybrid.i +++ b/gtsam/hybrid/hybrid.i @@ -99,6 +99,8 @@ class HybridBayesTree { bool empty() const; const HybridBayesTreeClique* operator[](size_t j) const; + gtsam::HybridValues optimize() const; + string dot(const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; }; From 2c4866e6e6a6a0ba5dd822624461871df852734e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 31 Aug 2022 12:30:36 -0400 Subject: [PATCH 032/118] DiscreteKeys::print method --- gtsam/discrete/DiscreteKey.h | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/gtsam/discrete/DiscreteKey.h b/gtsam/discrete/DiscreteKey.h index dea00074d..297d5570d 100644 --- a/gtsam/discrete/DiscreteKey.h +++ b/gtsam/discrete/DiscreteKey.h @@ -69,6 +69,16 @@ namespace gtsam { push_back(key); return *this; } + + /// Print the keys and cardinalities. + void print(const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + for (auto&& dkey : *this) { + std::cout << DefaultKeyFormatter(dkey.first) << " " << dkey.second + << std::endl; + } + } + }; // DiscreteKeys /// Create a list from two keys From f42f282438f976640f59910b7bc47ca1a44e8a0f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 31 Aug 2022 16:54:17 -0400 Subject: [PATCH 033/118] rewrap KeyVector for Matlab wrapper --- gtsam/gtsam.i | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 2671f0ef7..00b4d05f8 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -66,6 +66,27 @@ class KeySet { void serialize() const; }; +// Actually a vector, needed for Matlab +class KeyVector { + KeyVector(); + KeyVector(const gtsam::KeyVector& other); + + // Note: no print function + + // common STL methods + size_t size() const; + bool empty() const; + void clear(); + + // structure specific methods + size_t at(size_t i) const; + size_t front() const; + size_t back() const; + void push_back(size_t key) const; + + void serialize() const; +}; + // Actually a FastMap class KeyGroupMap { KeyGroupMap(); From bfb865c12e5ab9855c70d1b0e55dca9a0ff0f68f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 1 Sep 2022 00:00:06 -0400 Subject: [PATCH 034/118] DiscreteKeys serialization --- gtsam/discrete/DiscreteKey.h | 31 ++++++++++++++++++++- gtsam/discrete/tests/testDiscreteFactor.cpp | 20 +++++++++++-- 2 files changed, 47 insertions(+), 4 deletions(-) diff --git a/gtsam/discrete/DiscreteKey.h b/gtsam/discrete/DiscreteKey.h index 297d5570d..ec76f5941 100644 --- a/gtsam/discrete/DiscreteKey.h +++ b/gtsam/discrete/DiscreteKey.h @@ -21,6 +21,7 @@ #include #include +#include #include #include #include @@ -79,8 +80,36 @@ namespace gtsam { } } + bool equals(const DiscreteKeys& other, double tol = 0) const { + if (this->size() != other.size()) { + return false; + } + + for (size_t i = 0; i < this->size(); i++) { + if (this->at(i).first != other.at(i).first || + this->at(i).second != other.at(i).second) { + return false; + } + } + return true; + } + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp( + "DiscreteKeys", + boost::serialization::base_object>(*this)); + } + }; // DiscreteKeys /// Create a list from two keys GTSAM_EXPORT DiscreteKeys operator&(const DiscreteKey& key1, const DiscreteKey& key2); -} + + // traits + template <> + struct traits : public Testable {}; + + } // namespace gtsam diff --git a/gtsam/discrete/tests/testDiscreteFactor.cpp b/gtsam/discrete/tests/testDiscreteFactor.cpp index 8681cf7eb..db0491c9d 100644 --- a/gtsam/discrete/tests/testDiscreteFactor.cpp +++ b/gtsam/discrete/tests/testDiscreteFactor.cpp @@ -16,14 +16,29 @@ * @author Duy-Nguyen Ta */ -#include -#include #include +#include +#include +#include + #include using namespace boost::assign; using namespace std; using namespace gtsam; +using namespace gtsam::serializationTestHelpers; + +/* ************************************************************************* */ +TEST(DisreteKeys, Serialization) { + DiscreteKeys keys; + keys& DiscreteKey(0, 2); + keys& DiscreteKey(1, 3); + keys& DiscreteKey(2, 4); + + EXPECT(equalsObj(keys)); + EXPECT(equalsXML(keys)); + EXPECT(equalsBinary(keys)); +} /* ************************************************************************* */ int main() { @@ -31,4 +46,3 @@ int main() { return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ - From eb5092897b442058d0e37e3eb506bf470807636a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 1 Sep 2022 00:03:31 -0400 Subject: [PATCH 035/118] add serialization for HybridFactor and HybridConditional --- gtsam/hybrid/HybridConditional.h | 9 +++++++++ gtsam/hybrid/HybridFactor.h | 14 ++++++++++++++ 2 files changed, 23 insertions(+) diff --git a/gtsam/hybrid/HybridConditional.h b/gtsam/hybrid/HybridConditional.h index 96ea6d969..b43bb9945 100644 --- a/gtsam/hybrid/HybridConditional.h +++ b/gtsam/hybrid/HybridConditional.h @@ -178,6 +178,15 @@ class GTSAM_EXPORT HybridConditional /// Get the type-erased pointer to the inner type boost::shared_ptr inner() { return inner_; } + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(BaseFactor); + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(BaseConditional); + } + }; // HybridConditional // traits diff --git a/gtsam/hybrid/HybridFactor.h b/gtsam/hybrid/HybridFactor.h index 13dc2e6e6..b3cdc231b 100644 --- a/gtsam/hybrid/HybridFactor.h +++ b/gtsam/hybrid/HybridFactor.h @@ -47,6 +47,7 @@ class GTSAM_EXPORT HybridFactor : public Factor { bool isContinuous_ = false; bool isHybrid_ = false; + // TODO(Varun) remove size_t nrContinuous_ = 0; protected: @@ -129,6 +130,19 @@ class GTSAM_EXPORT HybridFactor : public Factor { const KeyVector &continuousKeys() const { return continuousKeys_; } /// @} + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE &ar, const unsigned int /*version*/) { + ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar &BOOST_SERIALIZATION_NVP(isDiscrete_); + ar &BOOST_SERIALIZATION_NVP(isContinuous_); + ar &BOOST_SERIALIZATION_NVP(isHybrid_); + ar &BOOST_SERIALIZATION_NVP(discreteKeys_); + ar &BOOST_SERIALIZATION_NVP(continuousKeys_); + } }; // HybridFactor From 8692ae63eaa5eafc4465c55f7392e9cf02cdc692 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 1 Sep 2022 00:03:55 -0400 Subject: [PATCH 036/118] Make HybridBayesNet testable and add serialization --- gtsam/hybrid/HybridBayesNet.h | 54 ++++++++++++++++++++--- gtsam/hybrid/tests/testHybridBayesNet.cpp | 15 +++++++ 2 files changed, 63 insertions(+), 6 deletions(-) diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h index 616ea0698..e84103a50 100644 --- a/gtsam/hybrid/HybridBayesNet.h +++ b/gtsam/hybrid/HybridBayesNet.h @@ -18,6 +18,7 @@ #pragma once #include +#include #include #include #include @@ -37,12 +38,31 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { using shared_ptr = boost::shared_ptr; using sharedConditional = boost::shared_ptr; + /// @name Standard Constructors + /// @{ + /** Construct empty bayes net */ HybridBayesNet() = default; - /// Prune the Hybrid Bayes Net given the discrete decision tree. - HybridBayesNet prune( - const DecisionTreeFactor::shared_ptr &discreteFactor) const; + /// @} + /// @name Testable + /// @{ + + /** Check equality */ + bool equals(const This &bn, double tol = 1e-9) const { + return Base::equals(bn, tol); + } + + /// print graph + void print( + const std::string &s = "", + const KeyFormatter &formatter = DefaultKeyFormatter) const override { + Base::print(s, formatter); + } + + /// @} + /// @name Standard Interface + /// @{ /// Add HybridConditional to Bayes Net using Base::add; @@ -71,9 +91,13 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { */ GaussianBayesNet choose(const DiscreteValues &assignment) const; - /// Solve the HybridBayesNet by back-substitution. - /// TODO(Shangjie) do we need to create a HybridGaussianBayesNet class, and - /// put this method there? + /** + * @brief Solve the HybridBayesNet by first computing the MPE of all the + * discrete variables and then optimizing the continuous variables based on + * the MPE assignment. + * + * @return HybridValues + */ HybridValues optimize() const; /** @@ -84,6 +108,24 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { * @return Values */ VectorValues optimize(const DiscreteValues &assignment) const; + + /// Prune the Hybrid Bayes Net given the discrete decision tree. + HybridBayesNet prune( + const DecisionTreeFactor::shared_ptr &discreteFactor) const; + + /// @} + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE &ar, const unsigned int /*version*/) { + ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + } }; +/// traits +template <> +struct traits : public Testable {}; + } // namespace gtsam diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp index c7516c0f6..bf9385bc4 100644 --- a/gtsam/hybrid/tests/testHybridBayesNet.cpp +++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp @@ -18,6 +18,7 @@ * @date December 2021 */ +#include #include #include @@ -28,6 +29,8 @@ using namespace std; using namespace gtsam; +using namespace gtsam::serializationTestHelpers; + using noiseModel::Isotropic; using symbol_shorthand::M; using symbol_shorthand::X; @@ -146,6 +149,18 @@ TEST(HybridBayesNet, Optimize) { EXPECT(assert_equal(expectedValues, delta.continuous(), 1e-5)); } +/* ****************************************************************************/ +// Test HybridBayesNet serialization. +TEST(HybridBayesNet, Serialization) { + Switching s(4); + Ordering ordering = s.linearizedFactorGraph.getHybridOrdering(); + HybridBayesNet hbn = *(s.linearizedFactorGraph.eliminateSequential(ordering)); + + EXPECT(equalsObj(hbn)); + EXPECT(equalsXML(hbn)); + EXPECT(equalsBinary(hbn)); +} + /* ************************************************************************* */ int main() { TestResult tr; From b16b05ea2caef84a69bb5cda82f2472f350811d4 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 1 Sep 2022 00:04:19 -0400 Subject: [PATCH 037/118] Make HybridBayesTree testable and add serialization --- gtsam/hybrid/HybridBayesTree.h | 12 ++++++++++++ gtsam/hybrid/tests/testHybridBayesTree.cpp | 15 +++++++++++++++ 2 files changed, 27 insertions(+) diff --git a/gtsam/hybrid/HybridBayesTree.h b/gtsam/hybrid/HybridBayesTree.h index 361fbe86f..3fa344d4d 100644 --- a/gtsam/hybrid/HybridBayesTree.h +++ b/gtsam/hybrid/HybridBayesTree.h @@ -89,8 +89,20 @@ class GTSAM_EXPORT HybridBayesTree : public BayesTree { VectorValues optimize(const DiscreteValues& assignment) const; /// @} + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + } }; +/// traits +template <> +struct traits : public Testable {}; + /** * @brief Class for Hybrid Bayes tree orphan subtrees. * diff --git a/gtsam/hybrid/tests/testHybridBayesTree.cpp b/gtsam/hybrid/tests/testHybridBayesTree.cpp index d457e6b74..0908b8cb5 100644 --- a/gtsam/hybrid/tests/testHybridBayesTree.cpp +++ b/gtsam/hybrid/tests/testHybridBayesTree.cpp @@ -16,6 +16,7 @@ * @date August 2022 */ +#include #include #include #include @@ -143,6 +144,20 @@ TEST(HybridBayesTree, Optimize) { EXPECT(assert_equal(expectedValues, delta.continuous())); } +/* ****************************************************************************/ +// Test HybridBayesTree serialization. +TEST(HybridBayesTree, Serialization) { + Switching s(4); + Ordering ordering = s.linearizedFactorGraph.getHybridOrdering(); + HybridBayesTree hbt = + *(s.linearizedFactorGraph.eliminateMultifrontal(ordering)); + + using namespace gtsam::serializationTestHelpers; + EXPECT(equalsObj(hbt)); + EXPECT(equalsXML(hbt)); + EXPECT(equalsBinary(hbt)); +} + /* ************************************************************************* */ int main() { TestResult tr; From c6ebbdc70812715d6f578c6f5fa5d6eb72b33cc4 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 1 Sep 2022 00:06:29 -0400 Subject: [PATCH 038/118] add serialization test for GaussianBayesNet --- gtsam/linear/tests/testGaussianBayesNet.cpp | 31 +++++++++++++++++++++ 1 file changed, 31 insertions(+) diff --git a/gtsam/linear/tests/testGaussianBayesNet.cpp b/gtsam/linear/tests/testGaussianBayesNet.cpp index 2b125265f..fc9e52b5c 100644 --- a/gtsam/linear/tests/testGaussianBayesNet.cpp +++ b/gtsam/linear/tests/testGaussianBayesNet.cpp @@ -350,6 +350,37 @@ TEST(GaussianBayesNet, Dot) { "}"); } +#include +using namespace gtsam::serializationTestHelpers; + +/* ****************************************************************************/ +// Test GaussianBayesNet serialization. +TEST(GaussianBayesNet, Serialization) { + // Create an arbitrary Bayes Net + GaussianBayesNet gbn; + gbn += GaussianConditional::shared_ptr(new GaussianConditional( + 0, Vector2(1.0, 2.0), (Matrix2() << 3.0, 4.0, 0.0, 6.0).finished(), 3, + (Matrix2() << 7.0, 8.0, 9.0, 10.0).finished(), 4, + (Matrix2() << 11.0, 12.0, 13.0, 14.0).finished())); + gbn += GaussianConditional::shared_ptr(new GaussianConditional( + 1, Vector2(15.0, 16.0), (Matrix2() << 17.0, 18.0, 0.0, 20.0).finished(), + 2, (Matrix2() << 21.0, 22.0, 23.0, 24.0).finished(), 4, + (Matrix2() << 25.0, 26.0, 27.0, 28.0).finished())); + gbn += GaussianConditional::shared_ptr(new GaussianConditional( + 2, Vector2(29.0, 30.0), (Matrix2() << 31.0, 32.0, 0.0, 34.0).finished(), + 3, (Matrix2() << 35.0, 36.0, 37.0, 38.0).finished())); + gbn += GaussianConditional::shared_ptr(new GaussianConditional( + 3, Vector2(39.0, 40.0), (Matrix2() << 41.0, 42.0, 0.0, 44.0).finished(), + 4, (Matrix2() << 45.0, 46.0, 47.0, 48.0).finished())); + gbn += GaussianConditional::shared_ptr(new GaussianConditional( + 4, Vector2(49.0, 50.0), (Matrix2() << 51.0, 52.0, 0.0, 54.0).finished())); + + EXPECT(equalsObj(gbn)); + EXPECT(equalsXML(gbn)); + EXPECT(equalsBinary(gbn)); +} + + /* ************************************************************************* */ int main() { TestResult tr; From ab017dfd19e8027eedb994d313b890d5118bf0f1 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 1 Sep 2022 10:40:48 -0400 Subject: [PATCH 039/118] move DiscreteKeys code to .cpp --- gtsam/discrete/DiscreteKey.cpp | 21 +++++++++++++++++++++ gtsam/discrete/DiscreteKey.h | 22 +++------------------- 2 files changed, 24 insertions(+), 19 deletions(-) diff --git a/gtsam/discrete/DiscreteKey.cpp b/gtsam/discrete/DiscreteKey.cpp index 121d61103..06ed2ca3b 100644 --- a/gtsam/discrete/DiscreteKey.cpp +++ b/gtsam/discrete/DiscreteKey.cpp @@ -48,4 +48,25 @@ namespace gtsam { return keys & key2; } + void DiscreteKeys::print(const std::string& s, + const KeyFormatter& keyFormatter) const { + for (auto&& dkey : *this) { + std::cout << DefaultKeyFormatter(dkey.first) << " " << dkey.second + << std::endl; + } + } + + bool DiscreteKeys::equals(const DiscreteKeys& other, double tol) const { + if (this->size() != other.size()) { + return false; + } + + for (size_t i = 0; i < this->size(); i++) { + if (this->at(i).first != other.at(i).first || + this->at(i).second != other.at(i).second) { + return false; + } + } + return true; + } } diff --git a/gtsam/discrete/DiscreteKey.h b/gtsam/discrete/DiscreteKey.h index ec76f5941..8e0802d83 100644 --- a/gtsam/discrete/DiscreteKey.h +++ b/gtsam/discrete/DiscreteKey.h @@ -73,26 +73,10 @@ namespace gtsam { /// Print the keys and cardinalities. void print(const std::string& s = "", - const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - for (auto&& dkey : *this) { - std::cout << DefaultKeyFormatter(dkey.first) << " " << dkey.second - << std::endl; - } - } + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; - bool equals(const DiscreteKeys& other, double tol = 0) const { - if (this->size() != other.size()) { - return false; - } - - for (size_t i = 0; i < this->size(); i++) { - if (this->at(i).first != other.at(i).first || - this->at(i).second != other.at(i).second) { - return false; - } - } - return true; - } + /// Check equality to another DiscreteKeys object. + bool equals(const DiscreteKeys& other, double tol = 0) const; /** Serialization function */ friend class boost::serialization::access; From 27a9d566028c694bfd7e8297a999d9c6995e76c8 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 1 Sep 2022 13:47:18 -0400 Subject: [PATCH 040/118] move GaussianBayesNet serialization test to testSerializationLinear --- gtsam/linear/tests/testGaussianBayesNet.cpp | 31 ------------------- .../linear/tests/testSerializationLinear.cpp | 27 ++++++++++++++++ 2 files changed, 27 insertions(+), 31 deletions(-) diff --git a/gtsam/linear/tests/testGaussianBayesNet.cpp b/gtsam/linear/tests/testGaussianBayesNet.cpp index fc9e52b5c..2b125265f 100644 --- a/gtsam/linear/tests/testGaussianBayesNet.cpp +++ b/gtsam/linear/tests/testGaussianBayesNet.cpp @@ -350,37 +350,6 @@ TEST(GaussianBayesNet, Dot) { "}"); } -#include -using namespace gtsam::serializationTestHelpers; - -/* ****************************************************************************/ -// Test GaussianBayesNet serialization. -TEST(GaussianBayesNet, Serialization) { - // Create an arbitrary Bayes Net - GaussianBayesNet gbn; - gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 0, Vector2(1.0, 2.0), (Matrix2() << 3.0, 4.0, 0.0, 6.0).finished(), 3, - (Matrix2() << 7.0, 8.0, 9.0, 10.0).finished(), 4, - (Matrix2() << 11.0, 12.0, 13.0, 14.0).finished())); - gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 1, Vector2(15.0, 16.0), (Matrix2() << 17.0, 18.0, 0.0, 20.0).finished(), - 2, (Matrix2() << 21.0, 22.0, 23.0, 24.0).finished(), 4, - (Matrix2() << 25.0, 26.0, 27.0, 28.0).finished())); - gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 2, Vector2(29.0, 30.0), (Matrix2() << 31.0, 32.0, 0.0, 34.0).finished(), - 3, (Matrix2() << 35.0, 36.0, 37.0, 38.0).finished())); - gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 3, Vector2(39.0, 40.0), (Matrix2() << 41.0, 42.0, 0.0, 44.0).finished(), - 4, (Matrix2() << 45.0, 46.0, 47.0, 48.0).finished())); - gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 4, Vector2(49.0, 50.0), (Matrix2() << 51.0, 52.0, 0.0, 54.0).finished())); - - EXPECT(equalsObj(gbn)); - EXPECT(equalsXML(gbn)); - EXPECT(equalsBinary(gbn)); -} - - /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/linear/tests/testSerializationLinear.cpp b/gtsam/linear/tests/testSerializationLinear.cpp index 881b2830e..ee21de364 100644 --- a/gtsam/linear/tests/testSerializationLinear.cpp +++ b/gtsam/linear/tests/testSerializationLinear.cpp @@ -198,6 +198,33 @@ TEST (Serialization, gaussian_factor_graph) { EXPECT(equalsBinary(graph)); } +/* ****************************************************************************/ +TEST(Serialization, gaussian_bayes_net) { + // Create an arbitrary Bayes Net + GaussianBayesNet gbn; + gbn += GaussianConditional::shared_ptr(new GaussianConditional( + 0, Vector2(1.0, 2.0), (Matrix2() << 3.0, 4.0, 0.0, 6.0).finished(), 3, + (Matrix2() << 7.0, 8.0, 9.0, 10.0).finished(), 4, + (Matrix2() << 11.0, 12.0, 13.0, 14.0).finished())); + gbn += GaussianConditional::shared_ptr(new GaussianConditional( + 1, Vector2(15.0, 16.0), (Matrix2() << 17.0, 18.0, 0.0, 20.0).finished(), + 2, (Matrix2() << 21.0, 22.0, 23.0, 24.0).finished(), 4, + (Matrix2() << 25.0, 26.0, 27.0, 28.0).finished())); + gbn += GaussianConditional::shared_ptr(new GaussianConditional( + 2, Vector2(29.0, 30.0), (Matrix2() << 31.0, 32.0, 0.0, 34.0).finished(), + 3, (Matrix2() << 35.0, 36.0, 37.0, 38.0).finished())); + gbn += GaussianConditional::shared_ptr(new GaussianConditional( + 3, Vector2(39.0, 40.0), (Matrix2() << 41.0, 42.0, 0.0, 44.0).finished(), + 4, (Matrix2() << 45.0, 46.0, 47.0, 48.0).finished())); + gbn += GaussianConditional::shared_ptr(new GaussianConditional( + 4, Vector2(49.0, 50.0), (Matrix2() << 51.0, 52.0, 0.0, 54.0).finished())); + + std::string serialized = serialize(gbn); + GaussianBayesNet actual; + deserialize(serialized, actual); + EXPECT(assert_equal(gbn, actual)); +} + /* ************************************************************************* */ TEST (Serialization, gaussian_bayes_tree) { const Key x1=1, x2=2, x3=3, x4=4; From e16460358fc877a475bf711432ad2a43d63fae8d Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 2 Sep 2022 15:01:59 -0400 Subject: [PATCH 041/118] add multifrontal test to reproduce issue --- gtsam/hybrid/tests/testHybridBayesNet.cpp | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp index bf9385bc4..0c15ee83d 100644 --- a/gtsam/hybrid/tests/testHybridBayesNet.cpp +++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp @@ -20,6 +20,7 @@ #include #include +#include #include #include "Switching.h" @@ -149,6 +150,25 @@ TEST(HybridBayesNet, Optimize) { EXPECT(assert_equal(expectedValues, delta.continuous(), 1e-5)); } +/* ****************************************************************************/ +// Test bayes net multifrontal optimize +TEST(HybridBayesNet, OptimizeMultifrontal) { + Switching s(4); + + Ordering hybridOrdering = s.linearizedFactorGraph.getHybridOrdering(); + HybridBayesTree::shared_ptr hybridBayesTree = + s.linearizedFactorGraph.eliminateMultifrontal(hybridOrdering); + HybridValues delta = hybridBayesTree->optimize(); + + VectorValues expectedValues; + expectedValues.insert(X(1), -0.999904 * Vector1::Ones()); + expectedValues.insert(X(2), -0.99029 * Vector1::Ones()); + expectedValues.insert(X(3), -1.00971 * Vector1::Ones()); + expectedValues.insert(X(4), -1.0001 * Vector1::Ones()); + + EXPECT(assert_equal(expectedValues, delta.continuous(), 1e-5)); +} + /* ****************************************************************************/ // Test HybridBayesNet serialization. TEST(HybridBayesNet, Serialization) { From 773af1ed446fb389a604c80e29783c21403fc113 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 2 Sep 2022 15:04:28 -0400 Subject: [PATCH 042/118] refactor HybridBayesTree::optimize --- gtsam/base/treeTraversal-inst.h | 4 +- gtsam/hybrid/HybridBayesTree.cpp | 164 +++++++++++++++++-------------- 2 files changed, 91 insertions(+), 77 deletions(-) diff --git a/gtsam/base/treeTraversal-inst.h b/gtsam/base/treeTraversal-inst.h index 30cec3b9a..be45a248e 100644 --- a/gtsam/base/treeTraversal-inst.h +++ b/gtsam/base/treeTraversal-inst.h @@ -221,6 +221,6 @@ void PrintForest(const FOREST& forest, std::string str, PrintForestVisitorPre visitor(keyFormatter); DepthFirstForest(forest, str, visitor); } -} +} // namespace treeTraversal -} +} // namespace gtsam diff --git a/gtsam/hybrid/HybridBayesTree.cpp b/gtsam/hybrid/HybridBayesTree.cpp index 49ae0bd87..3fa4d6268 100644 --- a/gtsam/hybrid/HybridBayesTree.cpp +++ b/gtsam/hybrid/HybridBayesTree.cpp @@ -24,6 +24,7 @@ #include #include #include +#include namespace gtsam { @@ -39,95 +40,108 @@ bool HybridBayesTree::equals(const This& other, double tol) const { /* ************************************************************************* */ HybridValues HybridBayesTree::optimize() const { - HybridBayesNet hbn; DiscreteBayesNet dbn; + DiscreteValues mpe; - KeyVector added_keys; + auto root = roots_.at(0); + // Access the clique and get the underlying hybrid conditional + HybridConditional::shared_ptr root_conditional = root->conditional(); - // Iterate over all the nodes in the BayesTree - for (auto&& node : nodes()) { - // Check if conditional being added is already in the Bayes net. - if (std::find(added_keys.begin(), added_keys.end(), node.first) == - added_keys.end()) { - // Access the clique and get the underlying hybrid conditional - HybridBayesTreeClique::shared_ptr clique = node.second; - HybridConditional::shared_ptr conditional = clique->conditional(); - - // Record the key being added - added_keys.insert(added_keys.end(), conditional->frontals().begin(), - conditional->frontals().end()); - - if (conditional->isDiscrete()) { - // If discrete, we use it to compute the MPE - dbn.push_back(conditional->asDiscreteConditional()); - - } else { - // Else conditional is hybrid or continuous-only, - // so we directly add it to the Hybrid Bayes net. - hbn.push_back(conditional); - } - } + // The root should be discrete only, we compute the MPE + if (root_conditional->isDiscrete()) { + dbn.push_back(root_conditional->asDiscreteConditional()); + mpe = DiscreteFactorGraph(dbn).optimize(); + } else { + throw std::runtime_error( + "HybridBayesTree root is not discrete-only. Please check elimination " + "ordering or use continuous factor graph."); } - // Get the MPE - DiscreteValues mpe = DiscreteFactorGraph(dbn).optimize(); - // Given the MPE, compute the optimal continuous values. - GaussianBayesNet gbn = hbn.choose(mpe); - // If TBB is enabled, the bayes net order gets reversed, - // so we pre-reverse it -#ifdef GTSAM_USE_TBB - auto reversed = boost::adaptors::reverse(gbn); - gbn = GaussianBayesNet(reversed.begin(), reversed.end()); -#endif - - return HybridValues(mpe, gbn.optimize()); + VectorValues values = optimize(mpe); + return HybridValues(mpe, values); } /* ************************************************************************* */ -VectorValues HybridBayesTree::optimize(const DiscreteValues& assignment) const { - GaussianBayesNet gbn; +/** + * @brief Helper class for Depth First Forest traversal on the HybridBayesTree. + * + * When traversing the tree, the pre-order visitor will receive an instance of + * this class with the parent clique data. + */ +struct HybridAssignmentData { + const DiscreteValues assignment_; + GaussianBayesTree::sharedNode parentClique_; + // The gaussian bayes tree that will be recursively created. + GaussianBayesTree* gaussianbayesTree_; - KeyVector added_keys; + /** + * @brief Construct a new Hybrid Assignment Data object. + * + * @param assignment The MPE assignment for the optimal Gaussian cliques. + * @param parentClique The clique from the parent node of the current node. + * @param gbt The Gaussian Bayes Tree being generated during tree traversal. + */ + HybridAssignmentData(const DiscreteValues& assignment, + const GaussianBayesTree::sharedNode& parentClique, + GaussianBayesTree* gbt) + : assignment_(assignment), + parentClique_(parentClique), + gaussianbayesTree_(gbt) {} - // Iterate over all the nodes in the BayesTree - for (auto&& node : nodes()) { - // Check if conditional being added is already in the Bayes net. - if (std::find(added_keys.begin(), added_keys.end(), node.first) == - added_keys.end()) { - // Access the clique and get the underlying hybrid conditional - HybridBayesTreeClique::shared_ptr clique = node.second; - HybridConditional::shared_ptr conditional = clique->conditional(); - - // Record the key being added - added_keys.insert(added_keys.end(), conditional->frontals().begin(), - conditional->frontals().end()); - - // If conditional is hybrid (and not discrete-only), we get the Gaussian - // Conditional corresponding to the assignment and add it to the Gaussian - // Bayes Net. - if (conditional->isHybrid()) { - auto gm = conditional->asMixture(); - GaussianConditional::shared_ptr gaussian_conditional = - (*gm)(assignment); - - gbn.push_back(gaussian_conditional); - - } else if (conditional->isContinuous()) { - // If conditional is Gaussian, we simply add it to the Bayes net. - gbn.push_back(conditional->asGaussian()); - } + /** + * @brief A function used during tree traversal that operators on each node + * before visiting the node's children. + * + * @param node The current node being visited. + * @param parentData The HybridAssignmentData from the parent node. + * @return HybridAssignmentData + */ + static HybridAssignmentData AssignmentPreOrderVisitor( + const HybridBayesTree::sharedNode& node, + HybridAssignmentData& parentData) { + // Extract the gaussian conditional from the Hybrid clique + HybridConditional::shared_ptr hybrid_conditional = node->conditional(); + GaussianConditional::shared_ptr conditional; + if (hybrid_conditional->isHybrid()) { + conditional = (*hybrid_conditional->asMixture())(parentData.assignment_); + } else if (hybrid_conditional->isContinuous()) { + conditional = hybrid_conditional->asGaussian(); + } else { + // Discrete only conditional, so we set to empty gaussian conditional + conditional = boost::make_shared(); } + + // Create the GaussianClique for the current node + auto clique = boost::make_shared(conditional); + // Add the current clique to the GaussianBayesTree. + parentData.gaussianbayesTree_->addClique(clique, parentData.parentClique_); + + // Create new HybridAssignmentData where the current node is the parent + // This will be passed down to the children nodes + HybridAssignmentData data(parentData.assignment_, clique, + parentData.gaussianbayesTree_); + return data; + } +}; + +/* ************************************************************************* + */ +VectorValues HybridBayesTree::optimize(const DiscreteValues& assignment) const { + GaussianBayesTree gbt; + HybridAssignmentData rootData(assignment, 0, &gbt); + { + treeTraversal::no_op visitorPost; + // Limits OpenMP threads since we're mixing TBB and OpenMP + TbbOpenMPMixedScope threadLimiter; + treeTraversal::DepthFirstForestParallel( + *this, rootData, HybridAssignmentData::AssignmentPreOrderVisitor, + visitorPost); } - // If TBB is enabled, the bayes net order gets reversed, - // so we pre-reverse it -#ifdef GTSAM_USE_TBB - auto reversed = boost::adaptors::reverse(gbn); - gbn = GaussianBayesNet(reversed.begin(), reversed.end()); -#endif + VectorValues result = gbt.optimize(); - // Return the optimized bayes net. - return gbn.optimize(); + // Return the optimized bayes net result. + return result; } } // namespace gtsam From ba4720709ba718eb0362fa54145d6afa233bed81 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 2 Sep 2022 15:11:49 -0400 Subject: [PATCH 043/118] some name cleaning in the HybridJunctionTree --- gtsam/hybrid/HybridJunctionTree.cpp | 27 +++++++++++++-------------- 1 file changed, 13 insertions(+), 14 deletions(-) diff --git a/gtsam/hybrid/HybridJunctionTree.cpp b/gtsam/hybrid/HybridJunctionTree.cpp index 7725742cf..422c200a4 100644 --- a/gtsam/hybrid/HybridJunctionTree.cpp +++ b/gtsam/hybrid/HybridJunctionTree.cpp @@ -31,9 +31,7 @@ template class EliminatableClusterTree; struct HybridConstructorTraversalData { - typedef - typename JunctionTree::Node - Node; + typedef HybridJunctionTree::Node Node; typedef typename JunctionTree::sharedNode sharedNode; @@ -62,6 +60,7 @@ struct HybridConstructorTraversalData { data.junctionTreeNode = boost::make_shared(node->key, node->factors); parentData.junctionTreeNode->addChild(data.junctionTreeNode); + // Add all the discrete keys in the hybrid factors to the current data for (HybridFactor::shared_ptr& f : node->factors) { for (auto& k : f->discreteKeys()) { data.discreteKeys.insert(k.first); @@ -72,8 +71,8 @@ struct HybridConstructorTraversalData { } // Post-order visitor function - static void ConstructorTraversalVisitorPostAlg2( - const boost::shared_ptr& ETreeNode, + static void ConstructorTraversalVisitorPost( + const boost::shared_ptr& node, const HybridConstructorTraversalData& data) { // In this post-order visitor, we combine the symbolic elimination results // from the elimination tree children and symbolically eliminate the current @@ -86,15 +85,15 @@ struct HybridConstructorTraversalData { // Do symbolic elimination for this node SymbolicFactors symbolicFactors; - symbolicFactors.reserve(ETreeNode->factors.size() + + symbolicFactors.reserve(node->factors.size() + data.childSymbolicFactors.size()); // Add ETree node factors - symbolicFactors += ETreeNode->factors; + symbolicFactors += node->factors; // Add symbolic factors passed up from children symbolicFactors += data.childSymbolicFactors; Ordering keyAsOrdering; - keyAsOrdering.push_back(ETreeNode->key); + keyAsOrdering.push_back(node->key); SymbolicConditional::shared_ptr conditional; SymbolicFactor::shared_ptr separatorFactor; boost::tie(conditional, separatorFactor) = @@ -105,19 +104,19 @@ struct HybridConstructorTraversalData { data.parentData->childSymbolicFactors.push_back(separatorFactor); data.parentData->discreteKeys.merge(data.discreteKeys); - sharedNode node = data.junctionTreeNode; + sharedNode jt_node = data.junctionTreeNode; const FastVector& childConditionals = data.childSymbolicConditionals; - node->problemSize_ = (int)(conditional->size() * symbolicFactors.size()); + jt_node->problemSize_ = (int)(conditional->size() * symbolicFactors.size()); // Merge our children if they are in our clique - if our conditional has // exactly one fewer parent than our child's conditional. const size_t nrParents = conditional->nrParents(); - const size_t nrChildren = node->nrChildren(); + const size_t nrChildren = jt_node->nrChildren(); assert(childConditionals.size() == nrChildren); // decide which children to merge, as index into children - std::vector nrChildrenFrontals = node->nrFrontalsOfChildren(); + std::vector nrChildrenFrontals = jt_node->nrFrontalsOfChildren(); std::vector merge(nrChildren, false); size_t nrFrontals = 1; for (size_t i = 0; i < nrChildren; i++) { @@ -137,7 +136,7 @@ struct HybridConstructorTraversalData { } // now really merge - node->mergeChildren(merge); + jt_node->mergeChildren(merge); } }; @@ -161,7 +160,7 @@ HybridJunctionTree::HybridJunctionTree( // the junction tree roots treeTraversal::DepthFirstForest(eliminationTree, rootData, Data::ConstructorTraversalVisitorPre, - Data::ConstructorTraversalVisitorPostAlg2); + Data::ConstructorTraversalVisitorPost); // Assign roots from the dummy node this->addChildrenAsRoots(rootData.junctionTreeNode); From c7e75d2cd5b69a81b3de15409afcde2a26012089 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 2 Sep 2022 15:12:58 -0400 Subject: [PATCH 044/118] rename myJTNode to junctionTreeNode so we can better subclass ConstructorTraversalData --- gtsam/inference/JunctionTree-inst.h | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/gtsam/inference/JunctionTree-inst.h b/gtsam/inference/JunctionTree-inst.h index 04472f7e3..1c68aa0da 100644 --- a/gtsam/inference/JunctionTree-inst.h +++ b/gtsam/inference/JunctionTree-inst.h @@ -33,7 +33,7 @@ struct ConstructorTraversalData { typedef typename JunctionTree::sharedNode sharedNode; ConstructorTraversalData* const parentData; - sharedNode myJTNode; + sharedNode junctionTreeNode; FastVector childSymbolicConditionals; FastVector childSymbolicFactors; @@ -53,8 +53,9 @@ struct ConstructorTraversalData { // a traversal data structure with its own JT node, and create a child // pointer in its parent. ConstructorTraversalData myData = ConstructorTraversalData(&parentData); - myData.myJTNode = boost::make_shared(node->key, node->factors); - parentData.myJTNode->addChild(myData.myJTNode); + myData.junctionTreeNode = + boost::make_shared(node->key, node->factors); + parentData.junctionTreeNode->addChild(myData.junctionTreeNode); return myData; } @@ -91,7 +92,7 @@ struct ConstructorTraversalData { myData.parentData->childSymbolicConditionals.push_back(myConditional); myData.parentData->childSymbolicFactors.push_back(mySeparatorFactor); - sharedNode node = myData.myJTNode; + sharedNode node = myData.junctionTreeNode; const FastVector& childConditionals = myData.childSymbolicConditionals; node->problemSize_ = (int) (myConditional->size() * symbolicFactors.size()); @@ -138,14 +139,14 @@ JunctionTree::JunctionTree( typedef typename EliminationTree::Node ETreeNode; typedef ConstructorTraversalData Data; Data rootData(0); - rootData.myJTNode = boost::make_shared(); // Make a dummy node to gather - // the junction tree roots + // Make a dummy node to gather the junction tree roots + rootData.junctionTreeNode = boost::make_shared(); treeTraversal::DepthFirstForest(eliminationTree, rootData, Data::ConstructorTraversalVisitorPre, Data::ConstructorTraversalVisitorPostAlg2); // Assign roots from the dummy node - this->addChildrenAsRoots(rootData.myJTNode); + this->addChildrenAsRoots(rootData.junctionTreeNode); // Transfer remaining factors from elimination tree Base::remainingFactors_ = eliminationTree.remainingFactors(); From b0ba35d2bd58e97de51b01dcac09bb729dd94001 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 2 Sep 2022 15:13:24 -0400 Subject: [PATCH 045/118] remove unused variable in testSimilarity2 --- gtsam/geometry/tests/testSimilarity2.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/gtsam/geometry/tests/testSimilarity2.cpp b/gtsam/geometry/tests/testSimilarity2.cpp index dd4fd0efd..ca041fc7b 100644 --- a/gtsam/geometry/tests/testSimilarity2.cpp +++ b/gtsam/geometry/tests/testSimilarity2.cpp @@ -33,8 +33,6 @@ static const Point2 P(0.2, 0.7); static const Rot2 R = Rot2::fromAngle(0.3); static const double s = 4; -const double degree = M_PI / 180; - //****************************************************************************** TEST(Similarity2, Concepts) { BOOST_CONCEPT_ASSERT((IsGroup)); From 9f2229fad57e633ac1066415a927435057f8cb08 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 2 Sep 2022 21:30:10 -0400 Subject: [PATCH 046/118] remove unused blocks --- .../tests/testHybridNonlinearFactorGraph.cpp | 69 +------------------ 1 file changed, 1 insertion(+), 68 deletions(-) diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp index 018b017a9..6c07331ab 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp @@ -257,14 +257,6 @@ TEST(GaussianElimination, Eliminate_x1) { // Add first hybrid factor factors.push_back(self.linearizedFactorGraph[1]); - // TODO(Varun) remove this block since sum is no longer exposed. - // // Check that sum works: - // auto sum = factors.sum(); - // Assignment mode; - // mode[M(1)] = 1; - // auto actual = sum(mode); // Selects one of 2 modes. - // EXPECT_LONGS_EQUAL(2, actual.size()); // Prior and motion model. - // Eliminate x1 Ordering ordering; ordering += X(1); @@ -289,15 +281,6 @@ TEST(HybridsGaussianElimination, Eliminate_x2) { factors.push_back(self.linearizedFactorGraph[1]); // involves m1 factors.push_back(self.linearizedFactorGraph[2]); // involves m2 - // TODO(Varun) remove this block since sum is no longer exposed. - // // Check that sum works: - // auto sum = factors.sum(); - // Assignment mode; - // mode[M(1)] = 0; - // mode[M(2)] = 1; - // auto actual = sum(mode); // Selects one of 4 mode - // combinations. EXPECT_LONGS_EQUAL(2, actual.size()); // 2 motion models. - // Eliminate x2 Ordering ordering; ordering += X(2); @@ -366,49 +349,6 @@ TEST(HybridGaussianElimination, EliminateHybrid_2_Variable) { EXPECT(discreteFactor->root_->isLeaf() == false); } -// /* -// ****************************************************************************/ -// /// Test the toDecisionTreeFactor method -// TEST(HybridFactorGraph, ToDecisionTreeFactor) { -// size_t K = 3; - -// // Provide tight sigma values so that the errors are visibly different. -// double between_sigma = 5e-8, prior_sigma = 1e-7; - -// Switching self(K, between_sigma, prior_sigma); - -// // Clear out discrete factors since sum() cannot hanldle those -// HybridGaussianFactorGraph linearizedFactorGraph( -// self.linearizedFactorGraph.gaussianGraph(), DiscreteFactorGraph(), -// self.linearizedFactorGraph.dcGraph()); - -// auto decisionTreeFactor = linearizedFactorGraph.toDecisionTreeFactor(); - -// auto allAssignments = -// DiscreteValues::CartesianProduct(linearizedFactorGraph.discreteKeys()); - -// // Get the error of the discrete assignment m1=0, m2=1. -// double actual = (*decisionTreeFactor)(allAssignments[1]); - -// /********************************************/ -// // Create equivalent factor graph for m1=0, m2=1 -// GaussianFactorGraph graph = linearizedFactorGraph.gaussianGraph(); - -// for (auto &p : linearizedFactorGraph.dcGraph()) { -// if (auto mixture = -// boost::dynamic_pointer_cast(p)) { -// graph.add((*mixture)(allAssignments[1])); -// } -// } - -// VectorValues values = graph.optimize(); -// double expected = graph.probPrime(values); -// /********************************************/ -// EXPECT_DOUBLES_EQUAL(expected, actual, 1e-12); -// // REGRESSION: -// EXPECT_DOUBLES_EQUAL(0.6125, actual, 1e-4); -// } - /**************************************************************************** * Test partial elimination */ @@ -428,7 +368,6 @@ TEST(HybridFactorGraph, Partial_Elimination) { linearizedFactorGraph.eliminatePartialSequential(ordering); CHECK(hybridBayesNet); - // GTSAM_PRINT(*hybridBayesNet); // HybridBayesNet EXPECT_LONGS_EQUAL(3, hybridBayesNet->size()); EXPECT(hybridBayesNet->at(0)->frontals() == KeyVector{X(1)}); EXPECT(hybridBayesNet->at(0)->parents() == KeyVector({X(2), M(1)})); @@ -438,7 +377,6 @@ TEST(HybridFactorGraph, Partial_Elimination) { EXPECT(hybridBayesNet->at(2)->parents() == KeyVector({M(1), M(2)})); CHECK(remainingFactorGraph); - // GTSAM_PRINT(*remainingFactorGraph); // HybridFactorGraph EXPECT_LONGS_EQUAL(3, remainingFactorGraph->size()); EXPECT(remainingFactorGraph->at(0)->keys() == KeyVector({M(1)})); EXPECT(remainingFactorGraph->at(1)->keys() == KeyVector({M(2), M(1)})); @@ -721,13 +659,8 @@ TEST(HybridFactorGraph, DefaultDecisionTree) { moving = boost::make_shared(X(0), X(1), odometry, noise_model); std::vector motion_models = {still, moving}; - // TODO(Varun) Make a templated constructor for MixtureFactor which does this? - std::vector components; - for (auto&& f : motion_models) { - components.push_back(boost::dynamic_pointer_cast(f)); - } fg.emplace_hybrid( - contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components); + contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, motion_models); // Add Range-Bearing measurements to from X0 to L0 and X1 to L1. // create a noise model for the landmark measurements From e5333fa331908fa2d194b79b22d00608fa8f3190 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 2 Sep 2022 21:30:24 -0400 Subject: [PATCH 047/118] remove nrContinuous since we don't need it --- gtsam/hybrid/HybridFactor.cpp | 2 -- gtsam/hybrid/HybridFactor.h | 3 --- 2 files changed, 5 deletions(-) diff --git a/gtsam/hybrid/HybridFactor.cpp b/gtsam/hybrid/HybridFactor.cpp index a9fe62cf1..e9d64b283 100644 --- a/gtsam/hybrid/HybridFactor.cpp +++ b/gtsam/hybrid/HybridFactor.cpp @@ -52,7 +52,6 @@ DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1, HybridFactor::HybridFactor(const KeyVector &keys) : Base(keys), isContinuous_(true), - nrContinuous_(keys.size()), continuousKeys_(keys) {} /* ************************************************************************ */ @@ -62,7 +61,6 @@ HybridFactor::HybridFactor(const KeyVector &continuousKeys, isDiscrete_((continuousKeys.size() == 0) && (discreteKeys.size() != 0)), isContinuous_((continuousKeys.size() != 0) && (discreteKeys.size() == 0)), isHybrid_((continuousKeys.size() != 0) && (discreteKeys.size() != 0)), - nrContinuous_(continuousKeys.size()), discreteKeys_(discreteKeys), continuousKeys_(continuousKeys) {} diff --git a/gtsam/hybrid/HybridFactor.h b/gtsam/hybrid/HybridFactor.h index b3cdc231b..165dfde29 100644 --- a/gtsam/hybrid/HybridFactor.h +++ b/gtsam/hybrid/HybridFactor.h @@ -47,9 +47,6 @@ class GTSAM_EXPORT HybridFactor : public Factor { bool isContinuous_ = false; bool isHybrid_ = false; - // TODO(Varun) remove - size_t nrContinuous_ = 0; - protected: // Set of DiscreteKeys for this factor. DiscreteKeys discreteKeys_; From 2fe115684130d6d3dc89c5d002039fbcd4866ea8 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 3 Sep 2022 22:36:19 -0400 Subject: [PATCH 048/118] remove redunant statements --- gtsam/hybrid/tests/Switching.h | 6 ------ 1 file changed, 6 deletions(-) diff --git a/gtsam/hybrid/tests/Switching.h b/gtsam/hybrid/tests/Switching.h index 6f41d06ea..ecd90e234 100644 --- a/gtsam/hybrid/tests/Switching.h +++ b/gtsam/hybrid/tests/Switching.h @@ -128,9 +128,6 @@ struct Switching { /// Create with given number of time steps. Switching(size_t K, double between_sigma = 1.0, double prior_sigma = 0.1) : K(K) { - using symbol_shorthand::M; - using symbol_shorthand::X; - // Create DiscreteKeys for binary K modes, modes[0] will not be used. for (size_t k = 0; k <= K; k++) { modes.emplace_back(M(k), 2); @@ -175,9 +172,6 @@ struct Switching { // Create motion models for a given time step static std::vector motionModels(size_t k, double sigma = 1.0) { - using symbol_shorthand::M; - using symbol_shorthand::X; - auto noise_model = noiseModel::Isotropic::Sigma(1, sigma); auto still = boost::make_shared(X(k), X(k + 1), 0.0, noise_model), From 8c10cd554db0c6f53bc72dc00352189409d82e0a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 3 Sep 2022 22:36:43 -0400 Subject: [PATCH 049/118] add todo --- gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp index 6c07331ab..3723e6078 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp @@ -347,6 +347,8 @@ TEST(HybridGaussianElimination, EliminateHybrid_2_Variable) { CHECK(discreteFactor); EXPECT_LONGS_EQUAL(1, discreteFactor->discreteKeys().size()); EXPECT(discreteFactor->root_->isLeaf() == false); + + //TODO(Varun) Test emplace_discrete } /**************************************************************************** From b4c70f2ef94a1a19de68e9f8a1edc13da66a2210 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 3 Sep 2022 22:39:10 -0400 Subject: [PATCH 050/118] add code for simplified hybrid estimation --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 59 ++++- gtsam/hybrid/tests/testHybridEstimation.cpp | 226 ++++++++++++++++++++ 2 files changed, 281 insertions(+), 4 deletions(-) create mode 100644 gtsam/hybrid/tests/testHybridEstimation.cpp diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index c031b9729..9dbbb0468 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -55,6 +55,52 @@ namespace gtsam { template class EliminateableFactorGraph; +std::string GTDKeyFormatter(const Key &key) { + constexpr size_t kMax_uchar_ = std::numeric_limits::max(); + constexpr size_t key_bits = sizeof(gtsam::Key) * 8; + constexpr size_t ch1_bits = sizeof(uint8_t) * 8; + constexpr size_t ch2_bits = sizeof(uint8_t) * 8; + constexpr size_t link_bits = sizeof(uint8_t) * 8; + constexpr size_t joint_bits = sizeof(uint8_t) * 8; + constexpr size_t time_bits = + key_bits - ch1_bits - ch2_bits - link_bits - joint_bits; + + constexpr gtsam::Key ch1_mask = gtsam::Key(kMax_uchar_) + << (key_bits - ch1_bits); + constexpr gtsam::Key ch2_mask = gtsam::Key(kMax_uchar_) + << (key_bits - ch1_bits - ch2_bits); + constexpr gtsam::Key link_mask = gtsam::Key(kMax_uchar_) + << (time_bits + joint_bits); + constexpr gtsam::Key joint_mask = gtsam::Key(kMax_uchar_) << time_bits; + constexpr gtsam::Key time_mask = + ~(ch1_mask | ch2_mask | link_mask | joint_mask); + + uint8_t c1_, c2_, link_idx_, joint_idx_; + uint64_t t_; + + c1_ = (uint8_t)((key & ch1_mask) >> (key_bits - ch1_bits)); + c2_ = (uint8_t)((key & ch2_mask) >> (key_bits - ch1_bits - ch2_bits)); + link_idx_ = (uint8_t)((key & link_mask) >> (time_bits + joint_bits)); + joint_idx_ = (uint8_t)((key & joint_mask) >> time_bits); + t_ = key & time_mask; + + std::string s = ""; + if (c1_ != 0) { + s += c1_; + } + if (c2_ != 0) { + s += c2_; + } + if (link_idx_ != kMax_uchar_) { + s += "[" + std::to_string((int)(link_idx_)) + "]"; + } + if (joint_idx_ != kMax_uchar_) { + s += "(" + std::to_string((int)(joint_idx_)) + ")"; + } + s += std::to_string(t_); + return s; +} + /* ************************************************************************ */ static GaussianMixtureFactor::Sum &addGaussian( GaussianMixtureFactor::Sum &sum, const GaussianFactor::shared_ptr &factor) { @@ -213,10 +259,10 @@ hybridElimination(const HybridGaussianFactorGraph &factors, result = EliminatePreferCholesky(graph, frontalKeys); if (keysOfEliminated.empty()) { - keysOfEliminated = - result.first->keys(); // Initialize the keysOfEliminated to be the + // Initialize the keysOfEliminated to be the keys of the + // eliminated GaussianConditional + keysOfEliminated = result.first->keys(); } - // keysOfEliminated of the GaussianConditional if (keysOfSeparator.empty()) { keysOfSeparator = result.second->keys(); } @@ -237,13 +283,18 @@ hybridElimination(const HybridGaussianFactorGraph &factors, // If there are no more continuous parents, then we should create here a // DiscreteFactor, with the error for each discrete choice. + // frontalKeys.print(); + // separatorFactors.print("", GTDKeyFormatter, [](const GaussianFactor::shared_ptr &factor) { factor->print(); return "";}); + // std::cout << "=====================================" << std::endl; if (keysOfSeparator.empty()) { - VectorValues empty_values; + VectorValues empty_values; //TODO(Varun) Shouldn't this be from the optimized leaf? auto factorError = [&](const GaussianFactor::shared_ptr &factor) { if (!factor) return 0.0; // TODO(fan): does this make sense? return exp(-factor->error(empty_values)); }; DecisionTree fdt(separatorFactors, factorError); + std::cout << "\n\nFactor Decision Tree" << std::endl; + fdt.print("", GTDKeyFormatter, [](double x) { return std::to_string(x); }); auto discreteFactor = boost::make_shared(discreteSeparator, fdt); diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp new file mode 100644 index 000000000..1aa3060ce --- /dev/null +++ b/gtsam/hybrid/tests/testHybridEstimation.cpp @@ -0,0 +1,226 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testHybridEstimation.cpp + * @brief Unit tests for Hybrid Estimation + * @author Varun Agrawal + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// Include for test suite +#include + +using namespace std; +using namespace gtsam; + +using noiseModel::Isotropic; +using symbol_shorthand::M; +using symbol_shorthand::X; + +class Robot { + size_t K_; + DiscreteKeys modes_; + HybridNonlinearFactorGraph nonlinearFactorGraph_; + HybridGaussianFactorGraph linearizedFactorGraph_; + Values linearizationPoint_; + + public: + Robot(size_t K, std::vector measurements) { + // Create DiscreteKeys for binary K modes + for (size_t k = 0; k < K; k++) { + modes_.emplace_back(M(k), 2); + } + + ////// Create hybrid factor graph. + // Add measurement factors + auto measurement_noise = noiseModel::Isotropic::Sigma(1, 1.0); + for (size_t k = 0; k < K; k++) { + nonlinearFactorGraph_.emplace_nonlinear>( + X(k), measurements.at(k), measurement_noise); + } + + // 2 noise models where moving has a higher covariance. + auto still_noise_model = noiseModel::Isotropic::Sigma(1, 1e-2); + auto moving_noise_model = noiseModel::Isotropic::Sigma(1, 1e2); + + // Add "motion models". + // The idea is that the robot has a higher "freedom" (aka higher covariance) + // for movement + using MotionModel = BetweenFactor; + + for (size_t k = 1; k < K; k++) { + KeyVector keys = {X(k - 1), X(k)}; + DiscreteKeys dkeys{modes_[k - 1]}; + auto still = boost::make_shared(X(k - 1), X(k), 0.0, + still_noise_model), + moving = boost::make_shared(X(k - 1), X(k), 0.0, + moving_noise_model); + std::vector> components = {still, moving}; + nonlinearFactorGraph_.emplace_hybrid(keys, dkeys, + components); + } + + // Create the linearization point. + for (size_t k = 0; k < K; k++) { + linearizationPoint_.insert(X(k), static_cast(k + 1)); + } + + linearizedFactorGraph_ = + nonlinearFactorGraph_.linearize(linearizationPoint_); + } + + void print() const { + nonlinearFactorGraph_.print(); + linearizationPoint_.print(); + linearizedFactorGraph_.print(); + } + + HybridValues optimize() const { + Ordering hybridOrdering = linearizedFactorGraph_.getHybridOrdering(); + HybridBayesNet::shared_ptr hybridBayesNet = + linearizedFactorGraph_.eliminateSequential(hybridOrdering); + + HybridValues delta = hybridBayesNet->optimize(); + return delta; + } +}; + +/* ****************************************************************************/ +/** + * I am trying to test if setting the hybrid mixture components to just differ + * in covariance makes sense. This is done by setting the "moving" covariance to + * be 1e2 while the "still" covariance is 1e-2. + */ +TEST(Estimation, StillRobot) { + size_t K = 2; + vector measurements = {0, 0}; + + Robot robot(K, measurements); + // robot.print(); + + HybridValues delta = robot.optimize(); + + delta.continuous().print("delta update:"); + + if (delta.discrete()[M(0)] == 0) { + std::cout << "The robot is stationary!" << std::endl; + } else { + std::cout << "The robot has moved!" << std::endl; + } + EXPECT_LONGS_EQUAL(0, delta.discrete()[M(0)]); +} + +TEST(Estimation, MovingRobot) { + size_t K = 2; + vector measurements = {0, 2}; + + Robot robot(K, measurements); + // robot.print(); + + HybridValues delta = robot.optimize(); + + delta.continuous().print("delta update:"); + + if (delta.discrete()[M(0)] == 0) { + std::cout << "The robot is stationary!" << std::endl; + } else { + std::cout << "The robot has moved!" << std::endl; + } + EXPECT_LONGS_EQUAL(1, delta.discrete()[M(0)]); +} + +/// A robot with a single leg. +class SingleLeg { + size_t K_; + DiscreteKeys modes_; + HybridNonlinearFactorGraph nonlinearFactorGraph_; + HybridGaussianFactorGraph linearizedFactorGraph_; + Values linearizationPoint_; + + public: + SingleLeg(size_t K, std::vector measurements) { + // Create DiscreteKeys for binary K modes + for (size_t k = 0; k < K; k++) { + modes_.emplace_back(M(k), 2); + } + + ////// Create hybrid factor graph. + // Add measurement factors + auto measurement_noise = noiseModel::Isotropic::Sigma(1, 1.0); + for (size_t k = 0; k < K; k++) { + nonlinearFactorGraph_.emplace_nonlinear>( + X(k), measurements.at(k), measurement_noise); + } + + // 2 noise models where moving has a higher covariance. + auto still_noise_model = noiseModel::Isotropic::Sigma(1, 1e-2); + auto moving_noise_model = noiseModel::Isotropic::Sigma(1, 1e2); + + // Add "motion models". + // The idea is that the robot has a higher "freedom" (aka higher covariance) + // for movement + using MotionModel = BetweenFactor; + + for (size_t k = 1; k < K; k++) { + KeyVector keys = {X(k - 1), X(k)}; + DiscreteKeys dkeys{modes_[k - 1]}; + auto still = boost::make_shared(X(k - 1), X(k), 0.0, + still_noise_model), + moving = boost::make_shared(X(k - 1), X(k), 0.0, + moving_noise_model); + std::vector> components = {still, moving}; + nonlinearFactorGraph_.emplace_hybrid(keys, dkeys, + components); + } + + // Create the linearization point. + for (size_t k = 0; k < K; k++) { + linearizationPoint_.insert(X(k), static_cast(k + 1)); + } + + linearizedFactorGraph_ = + nonlinearFactorGraph_.linearize(linearizationPoint_); + } + + void print() const { + nonlinearFactorGraph_.print(); + linearizationPoint_.print(); + linearizedFactorGraph_.print(); + } + + HybridValues optimize() const { + Ordering hybridOrdering = linearizedFactorGraph_.getHybridOrdering(); + HybridBayesNet::shared_ptr hybridBayesNet = + linearizedFactorGraph_.eliminateSequential(hybridOrdering); + + HybridValues delta = hybridBayesNet->optimize(); + return delta; + } +}; + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ From b2ca7476d6b2feab9910feec5ca3ec70066c07ce Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 3 Sep 2022 22:54:39 -0400 Subject: [PATCH 051/118] almost done with single legged robot --- gtsam/hybrid/tests/testHybridEstimation.cpp | 68 +++++++++++++++------ 1 file changed, 49 insertions(+), 19 deletions(-) diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp index 1aa3060ce..086c7cb7d 100644 --- a/gtsam/hybrid/tests/testHybridEstimation.cpp +++ b/gtsam/hybrid/tests/testHybridEstimation.cpp @@ -16,6 +16,7 @@ */ #include +#include #include #include #include @@ -34,6 +35,7 @@ using namespace std; using namespace gtsam; using noiseModel::Isotropic; +using symbol_shorthand::L; using symbol_shorthand::M; using symbol_shorthand::X; @@ -158,37 +160,65 @@ class SingleLeg { Values linearizationPoint_; public: - SingleLeg(size_t K, std::vector measurements) { + /** + * @brief Construct a new Single Leg object. + * + * @param K The number of discrete timesteps + * @param pims std::vector of preintegrated IMU measurements. + * @param contacts std::vector denoting whether the leg was in contact at each + * timestep. + */ + SingleLeg(size_t K, std::vector pims, std::vector contacts) { // Create DiscreteKeys for binary K modes for (size_t k = 0; k < K; k++) { modes_.emplace_back(M(k), 2); } ////// Create hybrid factor graph. - // Add measurement factors - auto measurement_noise = noiseModel::Isotropic::Sigma(1, 1.0); + + // Add measurement factors. + // These are the preintegrated IMU measurements of the base. + auto measurement_noise = noiseModel::Isotropic::Sigma(3, 1.0); for (size_t k = 0; k < K; k++) { - nonlinearFactorGraph_.emplace_nonlinear>( - X(k), measurements.at(k), measurement_noise); + nonlinearFactorGraph_.emplace_nonlinear>( + X(k), X(k + 1), pims.at(k), measurement_noise); + } + + // Forward kinematics from base X to foot L + auto fk_noise = noiseModel::Isotropic::Sigma(3, 1.0); + for (size_t k = 0; k < K; k++) { + if (contacts.at(k) == 1) { + nonlinearFactorGraph_.emplace_nonlinear>( + X(k), L(k), Pose2(), fk_noise) + } else { + nonlinearFactorGraph_.emplace_nonlinear>( + X(k), L(k), Pose2(), fk_noise) + } } // 2 noise models where moving has a higher covariance. - auto still_noise_model = noiseModel::Isotropic::Sigma(1, 1e-2); - auto moving_noise_model = noiseModel::Isotropic::Sigma(1, 1e2); + auto stance_model = noiseModel::Isotropic::Sigma(1, 1e-2); + auto swing_model = noiseModel::Isotropic::Sigma(1, 1e2); - // Add "motion models". - // The idea is that the robot has a higher "freedom" (aka higher covariance) - // for movement - using MotionModel = BetweenFactor; + // Add "contact models" for the foot. + // The idea is that the robot's leg has a tight covariance for stance and + // loose covariance for swing. + using ContactFactor = BetweenFactor; - for (size_t k = 1; k < K; k++) { - KeyVector keys = {X(k - 1), X(k)}; - DiscreteKeys dkeys{modes_[k - 1]}; - auto still = boost::make_shared(X(k - 1), X(k), 0.0, - still_noise_model), - moving = boost::make_shared(X(k - 1), X(k), 0.0, - moving_noise_model); - std::vector> components = {still, moving}; + for (size_t k = 0; k < K; k++) { + KeyVector keys = {L(k), L(k + 1)}; + DiscreteKeys dkeys{modes_[k], modes_[k + 1]}; + auto stance = boost::make_shared(keys.at(0), keys.at(1), + 0.0, stance_model), + lift = boost::make_shared(keys.at(0), keys.at(1), 0.0, + swing_model), + land = boost::make_shared(keys.at(0), keys.at(1), 0.0, + swing_model), + swing = boost::make_shared(keys.at(0), keys.at(1), + 0.0, swing_model); + // 00 - swing, 01 - land, 10 - toe-off, 11 - stance + std::vector> components = {swing, land, + lift, stance}; nonlinearFactorGraph_.emplace_hybrid(keys, dkeys, components); } From 8f94f726a96add1516a57abbe41533ee5f0485c7 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 4 Sep 2022 01:59:18 -0400 Subject: [PATCH 052/118] single leg robot test --- gtsam/hybrid/tests/testHybridEstimation.cpp | 78 ++++++++++++--------- 1 file changed, 46 insertions(+), 32 deletions(-) diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp index 086c7cb7d..c66c8916d 100644 --- a/gtsam/hybrid/tests/testHybridEstimation.cpp +++ b/gtsam/hybrid/tests/testHybridEstimation.cpp @@ -15,7 +15,6 @@ * @author Varun Agrawal */ -#include #include #include #include @@ -40,7 +39,6 @@ using symbol_shorthand::M; using symbol_shorthand::X; class Robot { - size_t K_; DiscreteKeys modes_; HybridNonlinearFactorGraph nonlinearFactorGraph_; HybridGaussianFactorGraph linearizedFactorGraph_; @@ -118,12 +116,10 @@ TEST(Estimation, StillRobot) { vector measurements = {0, 0}; Robot robot(K, measurements); - // robot.print(); HybridValues delta = robot.optimize(); delta.continuous().print("delta update:"); - if (delta.discrete()[M(0)] == 0) { std::cout << "The robot is stationary!" << std::endl; } else { @@ -132,17 +128,16 @@ TEST(Estimation, StillRobot) { EXPECT_LONGS_EQUAL(0, delta.discrete()[M(0)]); } +/* ****************************************************************************/ TEST(Estimation, MovingRobot) { size_t K = 2; vector measurements = {0, 2}; Robot robot(K, measurements); - // robot.print(); HybridValues delta = robot.optimize(); delta.continuous().print("delta update:"); - if (delta.discrete()[M(0)] == 0) { std::cout << "The robot is stationary!" << std::endl; } else { @@ -153,7 +148,6 @@ TEST(Estimation, MovingRobot) { /// A robot with a single leg. class SingleLeg { - size_t K_; DiscreteKeys modes_; HybridNonlinearFactorGraph nonlinearFactorGraph_; HybridGaussianFactorGraph linearizedFactorGraph_; @@ -165,10 +159,9 @@ class SingleLeg { * * @param K The number of discrete timesteps * @param pims std::vector of preintegrated IMU measurements. - * @param contacts std::vector denoting whether the leg was in contact at each - * timestep. + * @param fk std::vector of forward kinematic measurements for the leg. */ - SingleLeg(size_t K, std::vector pims, std::vector contacts) { + SingleLeg(size_t K, std::vector pims, std::vector fk) { // Create DiscreteKeys for binary K modes for (size_t k = 0; k < K; k++) { modes_.emplace_back(M(k), 2); @@ -176,10 +169,15 @@ class SingleLeg { ////// Create hybrid factor graph. + auto measurement_noise = noiseModel::Isotropic::Sigma(3, 1.0); + + // Add prior on the first pose + nonlinearFactorGraph_.emplace_nonlinear>( + X(0), Pose2(0, 2, 0), measurement_noise); + // Add measurement factors. // These are the preintegrated IMU measurements of the base. - auto measurement_noise = noiseModel::Isotropic::Sigma(3, 1.0); - for (size_t k = 0; k < K; k++) { + for (size_t k = 0; k < K - 1; k++) { nonlinearFactorGraph_.emplace_nonlinear>( X(k), X(k + 1), pims.at(k), measurement_noise); } @@ -187,35 +185,30 @@ class SingleLeg { // Forward kinematics from base X to foot L auto fk_noise = noiseModel::Isotropic::Sigma(3, 1.0); for (size_t k = 0; k < K; k++) { - if (contacts.at(k) == 1) { - nonlinearFactorGraph_.emplace_nonlinear>( - X(k), L(k), Pose2(), fk_noise) - } else { - nonlinearFactorGraph_.emplace_nonlinear>( - X(k), L(k), Pose2(), fk_noise) - } + nonlinearFactorGraph_.emplace_nonlinear>( + X(k), L(k), fk.at(k), fk_noise); } // 2 noise models where moving has a higher covariance. - auto stance_model = noiseModel::Isotropic::Sigma(1, 1e-2); - auto swing_model = noiseModel::Isotropic::Sigma(1, 1e2); + auto stance_model = noiseModel::Isotropic::Sigma(3, 1e-2); + auto swing_model = noiseModel::Isotropic::Sigma(3, 1e2); // Add "contact models" for the foot. // The idea is that the robot's leg has a tight covariance for stance and // loose covariance for swing. - using ContactFactor = BetweenFactor; + using ContactFactor = BetweenFactor; - for (size_t k = 0; k < K; k++) { + for (size_t k = 0; k < K - 1; k++) { KeyVector keys = {L(k), L(k + 1)}; DiscreteKeys dkeys{modes_[k], modes_[k + 1]}; - auto stance = boost::make_shared(keys.at(0), keys.at(1), - 0.0, stance_model), - lift = boost::make_shared(keys.at(0), keys.at(1), 0.0, - swing_model), - land = boost::make_shared(keys.at(0), keys.at(1), 0.0, - swing_model), - swing = boost::make_shared(keys.at(0), keys.at(1), - 0.0, swing_model); + auto stance = boost::make_shared( + keys.at(0), keys.at(1), Pose2(0, 0, 0), stance_model), + lift = boost::make_shared( + keys.at(0), keys.at(1), Pose2(0, 0, 0), swing_model), + land = boost::make_shared( + keys.at(0), keys.at(1), Pose2(0, 0, 0), swing_model), + swing = boost::make_shared( + keys.at(0), keys.at(1), Pose2(0, 0, 0), swing_model); // 00 - swing, 01 - land, 10 - toe-off, 11 - stance std::vector> components = {swing, land, lift, stance}; @@ -225,7 +218,8 @@ class SingleLeg { // Create the linearization point. for (size_t k = 0; k < K; k++) { - linearizationPoint_.insert(X(k), static_cast(k + 1)); + linearizationPoint_.insert(X(k), Pose2(k, 2, 0)); + linearizationPoint_.insert(L(k), Pose2(0, 0, 0)); } linearizedFactorGraph_ = @@ -243,11 +237,31 @@ class SingleLeg { HybridBayesNet::shared_ptr hybridBayesNet = linearizedFactorGraph_.eliminateSequential(hybridOrdering); + hybridBayesNet->print(); HybridValues delta = hybridBayesNet->optimize(); return delta; } + + Values linearizationPoint() const { return linearizationPoint_; } }; +/* ****************************************************************************/ +TEST(Estimation, LeggedRobot) { + std::vector pims = {Pose2(1, 0, 0)}; + // Leg is in stance throughout + std::vector fk = {Pose2(0, -2, 0), Pose2(-1, -2, 0)}; + SingleLeg robot(2, pims, fk); + + std::cout << "\n\n\n" << std::endl; + // robot.print(); + Values initial = robot.linearizationPoint(); + // initial.print(); + + HybridValues delta = robot.optimize(); + delta.print(); + + initial.retract(delta.continuous()).print("\n\n========="); +} /* ************************************************************************* */ int main() { TestResult tr; From b5eaaabcb55202139b9b7f47735bc3406699e7d4 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 4 Sep 2022 12:06:56 -0400 Subject: [PATCH 053/118] add more tests to show scheme doesn't work --- gtsam/hybrid/tests/testHybridEstimation.cpp | 173 +++++++++++++++++++- 1 file changed, 168 insertions(+), 5 deletions(-) diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp index c66c8916d..683617a19 100644 --- a/gtsam/hybrid/tests/testHybridEstimation.cpp +++ b/gtsam/hybrid/tests/testHybridEstimation.cpp @@ -21,6 +21,8 @@ #include #include #include +#include +#include #include #include #include @@ -204,11 +206,11 @@ class SingleLeg { auto stance = boost::make_shared( keys.at(0), keys.at(1), Pose2(0, 0, 0), stance_model), lift = boost::make_shared( - keys.at(0), keys.at(1), Pose2(0, 0, 0), swing_model), + keys.at(0), keys.at(1), Pose2(0, -1, 0), swing_model), land = boost::make_shared( - keys.at(0), keys.at(1), Pose2(0, 0, 0), swing_model), + keys.at(0), keys.at(1), Pose2(0, 1, 0), swing_model), swing = boost::make_shared( - keys.at(0), keys.at(1), Pose2(0, 0, 0), swing_model); + keys.at(0), keys.at(1), Pose2(1, 0, 0), swing_model); // 00 - swing, 01 - land, 10 - toe-off, 11 - stance std::vector> components = {swing, land, lift, stance}; @@ -237,7 +239,6 @@ class SingleLeg { HybridBayesNet::shared_ptr hybridBayesNet = linearizedFactorGraph_.eliminateSequential(hybridOrdering); - hybridBayesNet->print(); HybridValues delta = hybridBayesNet->optimize(); return delta; } @@ -258,10 +259,172 @@ TEST(Estimation, LeggedRobot) { // initial.print(); HybridValues delta = robot.optimize(); - delta.print(); + // delta.print(); initial.retract(delta.continuous()).print("\n\n========="); + std::cout << "\n\n\n" << std::endl; } + +/// A robot with a single leg - non-hybrid version. +class SL { + NonlinearFactorGraph nonlinearFactorGraph_; + GaussianFactorGraph linearizedFactorGraph_; + GaussianBayesNet bayesNet_; + Values linearizationPoint_; + + public: + /** + * @brief Construct a new Single Leg object. + * + * @param K The number of discrete timesteps + * @param pims std::vector of preintegrated IMU measurements. + * @param fk std::vector of forward kinematic measurements for the leg. + */ + SL(size_t K, const std::vector& pims, const std::vector& fk, + const std::vector& contacts) { + ////// Create hybrid factor graph. + + auto measurement_noise = noiseModel::Isotropic::Sigma(3, 1.0); + + // Add prior on the first pose + nonlinearFactorGraph_.emplace_shared>( + X(0), Pose2(0, 2, 0), measurement_noise); + + // Add measurement factors. + // These are the preintegrated IMU measurements of the base. + for (size_t k = 0; k < K - 1; k++) { + nonlinearFactorGraph_.emplace_shared>( + X(k), X(k + 1), pims.at(k), measurement_noise); + } + + // Forward kinematics from base X to foot L + auto fk_noise = noiseModel::Isotropic::Sigma(3, 1.0); + for (size_t k = 0; k < K; k++) { + nonlinearFactorGraph_.emplace_shared>( + X(k), L(k), fk.at(k), fk_noise); + } + + // 2 noise models where moving has a higher covariance. + auto stance_model = noiseModel::Isotropic::Sigma(3, 1e-4); + auto swing_model = noiseModel::Isotropic::Sigma(3, 1e8); + + // Add "contact models" for the foot. + // The idea is that the robot's leg has a tight covariance for stance and + // loose covariance for swing. + using ContactFactor = BetweenFactor; + + for (size_t k = 0; k < K - 1; k++) { + KeyVector keys = {L(k), L(k + 1)}; + ContactFactor::shared_ptr factor; + if (contacts[k] && contacts[k + 1]) { + // stance + std::cout << "stance 11" << std::endl; + factor = boost::make_shared( + keys.at(0), keys.at(1), Pose2(0, 0, 0), stance_model); + } else if (contacts[k] && !contacts[k + 1]) { + // toe-off + std::cout << "toe-off 10" << std::endl; + factor = boost::make_shared(keys.at(0), keys.at(1), + Pose2(0, 0, 0), swing_model); + } else if (!contacts[k] && contacts[k + 1]) { + // land + std::cout << "land 01" << std::endl; + factor = boost::make_shared(keys.at(0), keys.at(1), + Pose2(0, 0, 0), swing_model); + } else if (!contacts[k] && !contacts[k + 1]) { + // swing + std::cout << "swing 00" << std::endl; + factor = boost::make_shared(keys.at(0), keys.at(1), + Pose2(0, 0, 0), swing_model); + } + + nonlinearFactorGraph_.push_back(factor); + } + + // Create the linearization point. + for (size_t k = 0; k < K; k++) { + linearizationPoint_.insert(X(k), Pose2(k, 2, 0)); + linearizationPoint_.insert(L(k), Pose2(0, 0, 0)); + } + + linearizedFactorGraph_ = + *nonlinearFactorGraph_.linearize(linearizationPoint_); + } + + void print() const { + nonlinearFactorGraph_.print(); + linearizationPoint_.print(); + linearizedFactorGraph_.print(); + } + + VectorValues optimize() { + bayesNet_ = *linearizedFactorGraph_.eliminateSequential(); + + // bayesNet->print(); + VectorValues delta = bayesNet_.optimize(); + return delta; + } + + Values linearizationPoint() const { return linearizationPoint_; } + NonlinearFactorGraph nonlinearFactorGraph() const { + return nonlinearFactorGraph_; + } + GaussianFactorGraph linearizedFactorGraph() const { + return linearizedFactorGraph_; + } + GaussianBayesNet bayesNet() const { return bayesNet_; } +}; + +/* ****************************************************************************/ +TEST(Estimation, LR) { + std::vector pims = {Pose2(1, 0, 0)}; + // Leg is in stance throughout + // std::vector fk = {Pose2(0, -2, 0), Pose2(-1, -2, 0)}; + // Leg is in swing + // std::vector fk = {Pose2(0, -1, 0), Pose2(0, -1, 0)}; + // Leg is in toe-off + // std::vector fk = {Pose2(0, -2, 0), Pose2(0, -1, 0)}; + // Leg is in land + std::vector fk = {Pose2(0, -1, 0), Pose2(0, -2, 0)}; + + vector contacts; + contacts = {1, 1}; + SL robot11(2, pims, fk, contacts); + VectorValues delta = robot11.optimize(); + // robot11.nonlinearFactorGraph().print(); + std::cout << "Error with optimized delta: " << robot11.bayesNet().error(delta) + << std::endl; + robot11.linearizationPoint().retract(delta).print(); + std::cout << "\n===========================\n\n" << std::endl; + + contacts = {1, 0}; + SL robot10(2, pims, fk, contacts); + delta = robot10.optimize(); + // robot10.nonlinearFactorGraph().print(); + std::cout << "Error with optimized delta: " << robot10.bayesNet().error(delta) + << std::endl; + robot10.linearizationPoint().retract(delta).print(); + std::cout << "\n===========================\n\n" << std::endl; + + contacts = {0, 1}; + SL robot01(2, pims, fk, contacts); + delta = robot01.optimize(); + // robot01.nonlinearFactorGraph().print(); + std::cout << "Error with optimized delta: " << robot01.bayesNet().error(delta) + << std::endl; + robot01.linearizationPoint().retract(delta).print(); + std::cout << "\n===========================\n\n" << std::endl; + + contacts = {0, 0}; + SL robot00(2, pims, fk, contacts); + delta = robot00.optimize(); + // robot00.nonlinearFactorGraph().print(); + std::cout << "Error with optimized delta: " << robot00.bayesNet().error(delta) + << std::endl; + robot00.linearizationPoint().retract(delta).print(); + std::cout << "\n===========================\n\n" << std::endl; +} + /* ************************************************************************* */ int main() { TestResult tr; From 858193d5df7ecf50f7188371fecfafe0c0ab5983 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 11 Sep 2022 16:44:18 -0400 Subject: [PATCH 054/118] remove print statements --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 9dbbb0468..caf7cc080 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -293,8 +293,6 @@ hybridElimination(const HybridGaussianFactorGraph &factors, return exp(-factor->error(empty_values)); }; DecisionTree fdt(separatorFactors, factorError); - std::cout << "\n\nFactor Decision Tree" << std::endl; - fdt.print("", GTDKeyFormatter, [](double x) { return std::to_string(x); }); auto discreteFactor = boost::make_shared(discreteSeparator, fdt); From 8b5b42b6e9eb0ff6c7e8418fd85e6c710e17627a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 12 Sep 2022 18:07:53 -0400 Subject: [PATCH 055/118] Add check for MixtureFactor to ensure the continous keys are the same as in the factors --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 15 ++++++---- gtsam/hybrid/MixtureFactor.h | 12 ++++++++ .../tests/testHybridNonlinearFactorGraph.cpp | 28 ++++++++++++++++++- 3 files changed, 48 insertions(+), 7 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index c031b9729..09b592bd6 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -153,12 +153,14 @@ std::pair discreteElimination(const HybridGaussianFactorGraph &factors, const Ordering &frontalKeys) { DiscreteFactorGraph dfg; - for (auto &fp : factors) { - if (auto ptr = boost::dynamic_pointer_cast(fp)) { - dfg.push_back(ptr->inner()); - } else if (auto p = - boost::static_pointer_cast(fp)->inner()) { - dfg.push_back(boost::static_pointer_cast(p)); + + for (auto &factor : factors) { + if (auto p = boost::dynamic_pointer_cast(factor)) { + dfg.push_back(p->inner()); + } else if (auto p = boost::static_pointer_cast(factor)) { + auto discrete_conditional = + boost::static_pointer_cast(p->inner()); + dfg.push_back(discrete_conditional); } else { // It is an orphan wrapper } @@ -244,6 +246,7 @@ hybridElimination(const HybridGaussianFactorGraph &factors, return exp(-factor->error(empty_values)); }; DecisionTree fdt(separatorFactors, factorError); + auto discreteFactor = boost::make_shared(discreteSeparator, fdt); diff --git a/gtsam/hybrid/MixtureFactor.h b/gtsam/hybrid/MixtureFactor.h index 3cd21e32e..5e7337d0c 100644 --- a/gtsam/hybrid/MixtureFactor.h +++ b/gtsam/hybrid/MixtureFactor.h @@ -100,11 +100,23 @@ class MixtureFactor : public HybridFactor { bool normalized = false) : Base(keys, discreteKeys), normalized_(normalized) { std::vector nonlinear_factors; + KeySet continuous_keys_set(keys.begin(), keys.end()); + KeySet factor_keys_set; for (auto&& f : factors) { + // Insert all factor continuous keys in the continuous keys set. + std::copy(f->keys().begin(), f->keys().end(), + std::inserter(factor_keys_set, factor_keys_set.end())); + nonlinear_factors.push_back( boost::dynamic_pointer_cast(f)); } factors_ = Factors(discreteKeys, nonlinear_factors); + + if (continuous_keys_set != factor_keys_set) { + throw std::runtime_error( + "The specified continuous keys and the keys in the factors don't " + "match!"); + } } ~MixtureFactor() = default; diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp index 3723e6078..6b689b4e3 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp @@ -147,6 +147,32 @@ TEST(HybridGaussianFactorGraph, Resize) { EXPECT_LONGS_EQUAL(gfg.size(), 0); } +/*************************************************************************** + * Test that the MixtureFactor reports correctly if the number of continuous + * keys provided do not match the keys in the factors. + */ +TEST(HybridGaussianFactorGraph, MixtureFactor) { + auto nonlinearFactor = boost::make_shared>( + X(0), X(1), 0.0, Isotropic::Sigma(1, 0.1)); + auto discreteFactor = boost::make_shared(); + + auto noise_model = noiseModel::Isotropic::Sigma(1, 1.0); + auto still = boost::make_shared(X(0), X(1), 0.0, noise_model), + moving = boost::make_shared(X(0), X(1), 1.0, noise_model); + + std::vector components = {still, moving}; + + // Check for exception when number of continuous keys are under-specified. + KeyVector contKeys = {X(0)}; + THROWS_EXCEPTION(boost::make_shared( + contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components)); + + // Check for exception when number of continuous keys are too many. + contKeys = {X(0), X(1), X(2)}; + THROWS_EXCEPTION(boost::make_shared( + contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components)); +} + /***************************************************************************** * Test push_back on HFG makes the correct distinction. */ @@ -348,7 +374,7 @@ TEST(HybridGaussianElimination, EliminateHybrid_2_Variable) { EXPECT_LONGS_EQUAL(1, discreteFactor->discreteKeys().size()); EXPECT(discreteFactor->root_->isLeaf() == false); - //TODO(Varun) Test emplace_discrete + // TODO(Varun) Test emplace_discrete } /**************************************************************************** From 8c10a8089e2417996af9a5119e8488761fa4d3a5 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 12 Sep 2022 19:55:38 -0400 Subject: [PATCH 056/118] return shared pointer for HybridNonlinearFactorGraph::linearize --- gtsam/hybrid/HybridNonlinearFactorGraph.cpp | 21 +++++++++---------- gtsam/hybrid/HybridNonlinearFactorGraph.h | 3 ++- gtsam/hybrid/tests/Switching.h | 2 +- gtsam/hybrid/tests/testHybridIncremental.cpp | 8 +++---- .../tests/testHybridNonlinearFactorGraph.cpp | 8 +++---- 5 files changed, 21 insertions(+), 21 deletions(-) diff --git a/gtsam/hybrid/HybridNonlinearFactorGraph.cpp b/gtsam/hybrid/HybridNonlinearFactorGraph.cpp index a4218593b..3a3bf720b 100644 --- a/gtsam/hybrid/HybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/HybridNonlinearFactorGraph.cpp @@ -27,8 +27,7 @@ void HybridNonlinearFactorGraph::add( } /* ************************************************************************* */ -void HybridNonlinearFactorGraph::add( - boost::shared_ptr factor) { +void HybridNonlinearFactorGraph::add(boost::shared_ptr factor) { FactorGraph::add(boost::make_shared(factor)); } @@ -49,12 +48,12 @@ void HybridNonlinearFactorGraph::print(const std::string& s, } /* ************************************************************************* */ -HybridGaussianFactorGraph HybridNonlinearFactorGraph::linearize( +HybridGaussianFactorGraph::shared_ptr HybridNonlinearFactorGraph::linearize( const Values& continuousValues) const { // create an empty linear FG - HybridGaussianFactorGraph linearFG; + auto linearFG = boost::make_shared(); - linearFG.reserve(size()); + linearFG->reserve(size()); // linearize all hybrid factors for (auto&& factor : factors_) { @@ -66,9 +65,9 @@ HybridGaussianFactorGraph HybridNonlinearFactorGraph::linearize( if (factor->isHybrid()) { // Check if it is a nonlinear mixture factor if (auto nlmf = boost::dynamic_pointer_cast(factor)) { - linearFG.push_back(nlmf->linearize(continuousValues)); + linearFG->push_back(nlmf->linearize(continuousValues)); } else { - linearFG.push_back(factor); + linearFG->push_back(factor); } // Now check if the factor is a continuous only factor. @@ -80,18 +79,18 @@ HybridGaussianFactorGraph HybridNonlinearFactorGraph::linearize( boost::dynamic_pointer_cast(nlhf->inner())) { auto hgf = boost::make_shared( nlf->linearize(continuousValues)); - linearFG.push_back(hgf); + linearFG->push_back(hgf); } else { - linearFG.push_back(factor); + linearFG->push_back(factor); } // Finally if nothing else, we are discrete-only which doesn't need // lineariztion. } else { - linearFG.push_back(factor); + linearFG->push_back(factor); } } else { - linearFG.push_back(GaussianFactor::shared_ptr()); + linearFG->push_back(GaussianFactor::shared_ptr()); } } return linearFG; diff --git a/gtsam/hybrid/HybridNonlinearFactorGraph.h b/gtsam/hybrid/HybridNonlinearFactorGraph.h index 7a19c7755..afa410318 100644 --- a/gtsam/hybrid/HybridNonlinearFactorGraph.h +++ b/gtsam/hybrid/HybridNonlinearFactorGraph.h @@ -127,7 +127,8 @@ class GTSAM_EXPORT HybridNonlinearFactorGraph : public HybridFactorGraph { * @param continuousValues: Dictionary of continuous values. * @return HybridGaussianFactorGraph::shared_ptr */ - HybridGaussianFactorGraph linearize(const Values& continuousValues) const; + HybridGaussianFactorGraph::shared_ptr linearize( + const Values& continuousValues) const; }; template <> diff --git a/gtsam/hybrid/tests/Switching.h b/gtsam/hybrid/tests/Switching.h index ecd90e234..8bcb26c92 100644 --- a/gtsam/hybrid/tests/Switching.h +++ b/gtsam/hybrid/tests/Switching.h @@ -166,7 +166,7 @@ struct Switching { linearizationPoint.insert(X(k), static_cast(k)); } - linearizedFactorGraph = nonlinearFactorGraph.linearize(linearizationPoint); + linearizedFactorGraph = *nonlinearFactorGraph.linearize(linearizationPoint); } // Create motion models for a given time step diff --git a/gtsam/hybrid/tests/testHybridIncremental.cpp b/gtsam/hybrid/tests/testHybridIncremental.cpp index 4449aba0b..8e16d02b9 100644 --- a/gtsam/hybrid/tests/testHybridIncremental.cpp +++ b/gtsam/hybrid/tests/testHybridIncremental.cpp @@ -399,7 +399,7 @@ TEST(HybridGaussianISAM, NonTrivial) { initial.insert(Z(0), Pose2(0.0, 2.0, 0.0)); initial.insert(W(0), Pose2(0.0, 3.0, 0.0)); - HybridGaussianFactorGraph gfg = fg.linearize(initial); + HybridGaussianFactorGraph gfg = *fg.linearize(initial); fg = HybridNonlinearFactorGraph(); HybridGaussianISAM inc; @@ -444,7 +444,7 @@ TEST(HybridGaussianISAM, NonTrivial) { // The leg link did not move so we set the expected pose accordingly. initial.insert(W(1), Pose2(0.0, 3.0, 0.0)); - gfg = fg.linearize(initial); + gfg = *fg.linearize(initial); fg = HybridNonlinearFactorGraph(); // Update without pruning @@ -483,7 +483,7 @@ TEST(HybridGaussianISAM, NonTrivial) { initial.insert(Z(2), Pose2(2.0, 2.0, 0.0)); initial.insert(W(2), Pose2(0.0, 3.0, 0.0)); - gfg = fg.linearize(initial); + gfg = *fg.linearize(initial); fg = HybridNonlinearFactorGraph(); // Now we prune! @@ -526,7 +526,7 @@ TEST(HybridGaussianISAM, NonTrivial) { initial.insert(Z(3), Pose2(3.0, 2.0, 0.0)); initial.insert(W(3), Pose2(0.0, 3.0, 0.0)); - gfg = fg.linearize(initial); + gfg = *fg.linearize(initial); fg = HybridNonlinearFactorGraph(); // Keep pruning! diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp index 6b689b4e3..9e93eaba3 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp @@ -60,7 +60,7 @@ TEST(HybridFactorGraph, GaussianFactorGraph) { Values linearizationPoint; linearizationPoint.insert(X(0), 0); - HybridGaussianFactorGraph ghfg = fg.linearize(linearizationPoint); + HybridGaussianFactorGraph ghfg = *fg.linearize(linearizationPoint); // Add a factor to the GaussianFactorGraph ghfg.add(JacobianFactor(X(0), I_1x1, Vector1(5))); @@ -139,7 +139,7 @@ TEST(HybridGaussianFactorGraph, Resize) { linearizationPoint.insert(X(1), 1); // Generate `HybridGaussianFactorGraph` by linearizing - HybridGaussianFactorGraph gfg = nhfg.linearize(linearizationPoint); + HybridGaussianFactorGraph gfg = *nhfg.linearize(linearizationPoint); EXPECT_LONGS_EQUAL(gfg.size(), 3); @@ -250,7 +250,7 @@ TEST(HybridFactorGraph, Linearization) { // Linearize here: HybridGaussianFactorGraph actualLinearized = - self.nonlinearFactorGraph.linearize(self.linearizationPoint); + *self.nonlinearFactorGraph.linearize(self.linearizationPoint); EXPECT_LONGS_EQUAL(7, actualLinearized.size()); } @@ -718,7 +718,7 @@ TEST(HybridFactorGraph, DefaultDecisionTree) { ordering += X(0); ordering += X(1); - HybridGaussianFactorGraph linearized = fg.linearize(initialEstimate); + HybridGaussianFactorGraph linearized = *fg.linearize(initialEstimate); gtsam::HybridBayesNet::shared_ptr hybridBayesNet; gtsam::HybridGaussianFactorGraph::shared_ptr remainingFactorGraph; From 5a7801583265f0f88e656c7d648d9a21a1d9d8aa Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 12 Sep 2022 19:55:51 -0400 Subject: [PATCH 057/118] add push_back for containers --- gtsam/hybrid/HybridNonlinearFactorGraph.h | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/gtsam/hybrid/HybridNonlinearFactorGraph.h b/gtsam/hybrid/HybridNonlinearFactorGraph.h index afa410318..5f04851d4 100644 --- a/gtsam/hybrid/HybridNonlinearFactorGraph.h +++ b/gtsam/hybrid/HybridNonlinearFactorGraph.h @@ -109,6 +109,23 @@ class GTSAM_EXPORT HybridNonlinearFactorGraph : public HybridFactorGraph { } } + /** + * Push back many factors as shared_ptr's in a container (factors are not + * copied) + */ + template + Base::Base::HasDerivedElementType push_back( + const CONTAINER& container) { + Base::push_back(container.begin(), container.end()); + } + + /// Push back non-pointer objects in a container (factors are copied). + template + Base::Base::HasDerivedValueType push_back( + const CONTAINER& container) { + Base::push_back(container.begin(), container.end()); + } + /// Add a nonlinear factor as a shared ptr. void add(boost::shared_ptr factor); From 83dd813bff53a596128de567b5dd871317e95782 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 13 Sep 2022 13:01:27 -0400 Subject: [PATCH 058/118] HybridNonlinearISAM class --- gtsam/hybrid/HybridGaussianISAM.h | 2 +- gtsam/hybrid/HybridNonlinearISAM.cpp | 111 ++++ gtsam/hybrid/HybridNonlinearISAM.h | 132 ++++ .../hybrid/tests/testHybridNonlinearISAM.cpp | 589 ++++++++++++++++++ 4 files changed, 833 insertions(+), 1 deletion(-) create mode 100644 gtsam/hybrid/HybridNonlinearISAM.cpp create mode 100644 gtsam/hybrid/HybridNonlinearISAM.h create mode 100644 gtsam/hybrid/tests/testHybridNonlinearISAM.cpp diff --git a/gtsam/hybrid/HybridGaussianISAM.h b/gtsam/hybrid/HybridGaussianISAM.h index 4fc206582..ff0978729 100644 --- a/gtsam/hybrid/HybridGaussianISAM.h +++ b/gtsam/hybrid/HybridGaussianISAM.h @@ -65,7 +65,7 @@ class GTSAM_EXPORT HybridGaussianISAM : public ISAM { HybridBayesTree::EliminationTraitsType::DefaultEliminate); /** - * @brief + * @brief Prune the underlying Bayes tree. * * @param root The root key in the discrete conditional decision tree. * @param maxNumberLeaves diff --git a/gtsam/hybrid/HybridNonlinearISAM.cpp b/gtsam/hybrid/HybridNonlinearISAM.cpp new file mode 100644 index 000000000..36cda4e80 --- /dev/null +++ b/gtsam/hybrid/HybridNonlinearISAM.cpp @@ -0,0 +1,111 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file HybridNonlinearISAM.cpp + * @date Sep 12, 2022 + * @author Varun Agrawal + */ + +#include +#include +#include + +#include + +using namespace std; + +namespace gtsam { + +/* ************************************************************************* */ +void HybridNonlinearISAM::saveGraph(const string& s, + const KeyFormatter& keyFormatter) const { + isam_.saveGraph(s, keyFormatter); +} + +/* ************************************************************************* */ +void HybridNonlinearISAM::update(const HybridNonlinearFactorGraph& newFactors, + const Values& initialValues) { + if (newFactors.size() > 0) { + // Reorder and relinearize every reorderInterval updates + if (reorderInterval_ > 0 && ++reorderCounter_ >= reorderInterval_) { + reorder_relinearize(); + reorderCounter_ = 0; + } + + factors_.push_back(newFactors); + + // Linearize new factors and insert them + // TODO: optimize for whole config? + linPoint_.insert(initialValues); + + boost::shared_ptr linearizedNewFactors = + newFactors.linearize(linPoint_); + + // Update ISAM + isam_.update(*linearizedNewFactors, boost::none, eliminationFunction_); + } +} + +/* ************************************************************************* */ +void HybridNonlinearISAM::reorder_relinearize() { + if (factors_.size() > 0) { + // Obtain the new linearization point + const Values newLinPoint = estimate(); + + isam_.clear(); + + // Just recreate the whole BayesTree + // TODO: allow for constrained ordering here + // TODO: decouple relinearization and reordering to avoid + isam_.update(*factors_.linearize(newLinPoint), boost::none, + eliminationFunction_); + + // Update linearization point + linPoint_ = newLinPoint; + } +} + +/* ************************************************************************* */ +Values HybridNonlinearISAM::estimate() { + Values result; + if (isam_.size() > 0) { + HybridValues values = isam_.optimize(); + assignment_ = values.discrete(); + return linPoint_.retract(values.continuous()); + } else { + return linPoint_; + } +} + +// /* ************************************************************************* +// */ Matrix HybridNonlinearISAM::marginalCovariance(Key key) const { +// return isam_.marginalCovariance(key); +// } + +/* ************************************************************************* */ +void HybridNonlinearISAM::print(const string& s, + const KeyFormatter& keyFormatter) const { + cout << s << "ReorderInterval: " << reorderInterval_ + << " Current Count: " << reorderCounter_ << endl; + isam_.print("HybridGaussianISAM:\n"); + linPoint_.print("Linearization Point:\n", keyFormatter); + factors_.print("Nonlinear Graph:\n", keyFormatter); +} + +/* ************************************************************************* */ +void HybridNonlinearISAM::printStats() const { + isam_.getCliqueData().getStats().print(); +} + +/* ************************************************************************* */ + +} // namespace gtsam diff --git a/gtsam/hybrid/HybridNonlinearISAM.h b/gtsam/hybrid/HybridNonlinearISAM.h new file mode 100644 index 000000000..d96485fff --- /dev/null +++ b/gtsam/hybrid/HybridNonlinearISAM.h @@ -0,0 +1,132 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file HybridNonlinearISAM.h + * @date Sep 12, 2022 + * @author Varun Agrawal + */ + +#pragma once + +#include +#include + +namespace gtsam { +/** + * Wrapper class to manage ISAM in a nonlinear context + */ +class GTSAM_EXPORT HybridNonlinearISAM { + protected: + /** The internal iSAM object */ + gtsam::HybridGaussianISAM isam_; + + /** The current linearization point */ + Values linPoint_; + + /// The discrete assignment + DiscreteValues assignment_; + + /** The original factors, used when relinearizing */ + HybridNonlinearFactorGraph factors_; + + /** The reordering interval and counter */ + int reorderInterval_; + int reorderCounter_; + + /** The elimination function */ + HybridGaussianFactorGraph::Eliminate eliminationFunction_; + + public: + /// @name Standard Constructors + /// @{ + + /** + * Periodically reorder and relinearize + * @param reorderInterval is the number of updates between reorderings, + * 0 never reorders (and is dangerous for memory consumption) + * 1 (default) reorders every time, in worse case is batch every update + * typical values are 50 or 100 + */ + HybridNonlinearISAM( + int reorderInterval = 1, + const HybridGaussianFactorGraph::Eliminate& eliminationFunction = + HybridGaussianFactorGraph::EliminationTraitsType::DefaultEliminate) + : reorderInterval_(reorderInterval), + reorderCounter_(0), + eliminationFunction_(eliminationFunction) {} + + /// @} + /// @name Standard Interface + /// @{ + + /** Return the current solution estimate */ + Values estimate(); + + // /** find the marginal covariance for a single variable */ + // Matrix marginalCovariance(Key key) const; + + // access + + /** access the underlying bayes tree */ + const HybridGaussianISAM& bayesTree() const { return isam_; } + + /** + * @brief Prune the underlying Bayes tree. + * + * @param root The root key in the discrete conditional decision tree. + * @param maxNumberLeaves + */ + void prune(const Key& root, const size_t maxNumberLeaves) { + isam_.prune(root, maxNumberLeaves); + } + + /** Return the current linearization point */ + const Values& getLinearizationPoint() const { return linPoint_; } + + /** Return the current discrete assignment */ + const DiscreteValues& getAssignment() const { return assignment_; } + + /** get underlying nonlinear graph */ + const HybridNonlinearFactorGraph& getFactorsUnsafe() const { + return factors_; + } + + /** get counters */ + int reorderInterval() const { return reorderInterval_; } ///< TODO: comment + int reorderCounter() const { return reorderCounter_; } ///< TODO: comment + + /** prints out all contents of the system */ + void print(const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + + /** prints out clique statistics */ + void printStats() const; + + /** saves the Tree to a text file in GraphViz format */ + void saveGraph(const std::string& s, + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + + /// @} + /// @name Advanced Interface + /// @{ + + /** Add new factors along with their initial linearization points */ + void update(const HybridNonlinearFactorGraph& newFactors, + const Values& initialValues); + + /** Relinearization and reordering of variables */ + void reorder_relinearize(); + + /// @} +}; + +} // namespace gtsam diff --git a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp new file mode 100644 index 000000000..bed90fe2c --- /dev/null +++ b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp @@ -0,0 +1,589 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testHybridNonlinearISAM.cpp + * @brief Unit tests for nonlinear incremental inference + * @author Fan Jiang, Varun Agrawal, Frank Dellaert + * @date Jan 2021 + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "Switching.h" + +// Include for test suite +#include + +using namespace std; +using namespace gtsam; +using noiseModel::Isotropic; +using symbol_shorthand::L; +using symbol_shorthand::M; +using symbol_shorthand::W; +using symbol_shorthand::X; +using symbol_shorthand::Y; +using symbol_shorthand::Z; + +/* ****************************************************************************/ +// Test if we can perform elimination incrementally. +TEST(HybridNonlinearISAM, IncrementalElimination) { + Switching switching(3); + HybridNonlinearISAM isam; + HybridNonlinearFactorGraph graph1; + Values initial; + + // Create initial factor graph + // * * * + // | | | + // X1 -*- X2 -*- X3 + // \*-M1-*/ + graph1.push_back(switching.nonlinearFactorGraph.at(0)); // P(X1) + graph1.push_back(switching.nonlinearFactorGraph.at(1)); // P(X1, X2 | M1) + graph1.push_back(switching.nonlinearFactorGraph.at(2)); // P(X2, X3 | M2) + graph1.push_back(switching.nonlinearFactorGraph.at(5)); // P(M1) + + initial.insert(X(1), 1); + initial.insert(X(2), 2); + initial.insert(X(3), 3); + + // Run update step + isam.update(graph1, initial); + + // Check that after update we have 3 hybrid Bayes net nodes: + // P(X1 | X2, M1) and P(X2, X3 | M1, M2), P(M1, M2) + HybridGaussianISAM bayesTree = isam.bayesTree(); + EXPECT_LONGS_EQUAL(3, bayesTree.size()); + EXPECT(bayesTree[X(1)]->conditional()->frontals() == KeyVector{X(1)}); + EXPECT(bayesTree[X(1)]->conditional()->parents() == KeyVector({X(2), M(1)})); + EXPECT(bayesTree[X(2)]->conditional()->frontals() == KeyVector({X(2), X(3)})); + EXPECT(bayesTree[X(2)]->conditional()->parents() == KeyVector({M(1), M(2)})); + + /********************************************************/ + // New factor graph for incremental update. + HybridNonlinearFactorGraph graph2; + initial = Values(); + + graph1.push_back(switching.nonlinearFactorGraph.at(3)); // P(X2) + graph2.push_back(switching.nonlinearFactorGraph.at(4)); // P(X3) + graph2.push_back(switching.nonlinearFactorGraph.at(6)); // P(M1, M2) + + isam.update(graph2, initial); + + bayesTree = isam.bayesTree(); + // Check that after the second update we have + // 1 additional hybrid Bayes net node: + // P(X2, X3 | M1, M2) + EXPECT_LONGS_EQUAL(3, bayesTree.size()); + EXPECT(bayesTree[X(3)]->conditional()->frontals() == KeyVector({X(2), X(3)})); + EXPECT(bayesTree[X(3)]->conditional()->parents() == KeyVector({M(1), M(2)})); +} + +/* ****************************************************************************/ +// Test if we can incrementally do the inference +TEST(HybridNonlinearISAM, IncrementalInference) { + Switching switching(3); + HybridNonlinearISAM isam; + HybridNonlinearFactorGraph graph1; + Values initial; + + // Create initial factor graph + // * * * + // | | | + // X1 -*- X2 -*- X3 + // | | + // *-M1 - * - M2 + graph1.push_back(switching.nonlinearFactorGraph.at(0)); // P(X1) + graph1.push_back(switching.nonlinearFactorGraph.at(1)); // P(X1, X2 | M1) + graph1.push_back(switching.nonlinearFactorGraph.at(3)); // P(X2) + graph1.push_back(switching.nonlinearFactorGraph.at(5)); // P(M1) + + initial.insert(X(1), 1); + initial.insert(X(2), 2); + + // Run update step + isam.update(graph1, initial); + HybridGaussianISAM bayesTree = isam.bayesTree(); + + auto discreteConditional_m1 = + bayesTree[M(1)]->conditional()->asDiscreteConditional(); + EXPECT(discreteConditional_m1->keys() == KeyVector({M(1)})); + + /********************************************************/ + // New factor graph for incremental update. + HybridNonlinearFactorGraph graph2; + initial = Values(); + + initial.insert(X(3), 3); + + graph2.push_back(switching.nonlinearFactorGraph.at(2)); // P(X2, X3 | M2) + graph2.push_back(switching.nonlinearFactorGraph.at(4)); // P(X3) + graph2.push_back(switching.nonlinearFactorGraph.at(6)); // P(M1, M2) + + isam.update(graph2, initial); + bayesTree = isam.bayesTree(); + + /********************************************************/ + // Run batch elimination so we can compare results. + Ordering ordering; + ordering += X(1); + ordering += X(2); + ordering += X(3); + + // Now we calculate the actual factors using full elimination + HybridBayesTree::shared_ptr expectedHybridBayesTree; + HybridGaussianFactorGraph::shared_ptr expectedRemainingGraph; + std::tie(expectedHybridBayesTree, expectedRemainingGraph) = + switching.linearizedFactorGraph.eliminatePartialMultifrontal(ordering); + + // The densities on X(1) should be the same + auto x1_conditional = dynamic_pointer_cast( + bayesTree[X(1)]->conditional()->inner()); + auto actual_x1_conditional = dynamic_pointer_cast( + (*expectedHybridBayesTree)[X(1)]->conditional()->inner()); + EXPECT(assert_equal(*x1_conditional, *actual_x1_conditional)); + + // The densities on X(2) should be the same + auto x2_conditional = dynamic_pointer_cast( + bayesTree[X(2)]->conditional()->inner()); + auto actual_x2_conditional = dynamic_pointer_cast( + (*expectedHybridBayesTree)[X(2)]->conditional()->inner()); + EXPECT(assert_equal(*x2_conditional, *actual_x2_conditional)); + + // The densities on X(3) should be the same + auto x3_conditional = dynamic_pointer_cast( + bayesTree[X(3)]->conditional()->inner()); + auto actual_x3_conditional = dynamic_pointer_cast( + (*expectedHybridBayesTree)[X(2)]->conditional()->inner()); + EXPECT(assert_equal(*x3_conditional, *actual_x3_conditional)); + + // We only perform manual continuous elimination for 0,0. + // The other discrete probabilities on M(2) are calculated the same way + Ordering discrete_ordering; + discrete_ordering += M(1); + discrete_ordering += M(2); + HybridBayesTree::shared_ptr discreteBayesTree = + expectedRemainingGraph->eliminateMultifrontal(discrete_ordering); + + DiscreteValues m00; + m00[M(1)] = 0, m00[M(2)] = 0; + DiscreteConditional decisionTree = + *(*discreteBayesTree)[M(2)]->conditional()->asDiscreteConditional(); + double m00_prob = decisionTree(m00); + + auto discreteConditional = + bayesTree[M(2)]->conditional()->asDiscreteConditional(); + + // Test if the probability values are as expected with regression tests. + DiscreteValues assignment; + EXPECT(assert_equal(m00_prob, 0.0619233, 1e-5)); + assignment[M(1)] = 0; + assignment[M(2)] = 0; + EXPECT(assert_equal(m00_prob, (*discreteConditional)(assignment), 1e-5)); + assignment[M(1)] = 1; + assignment[M(2)] = 0; + EXPECT(assert_equal(0.183743, (*discreteConditional)(assignment), 1e-5)); + assignment[M(1)] = 0; + assignment[M(2)] = 1; + EXPECT(assert_equal(0.204159, (*discreteConditional)(assignment), 1e-5)); + assignment[M(1)] = 1; + assignment[M(2)] = 1; + EXPECT(assert_equal(0.2, (*discreteConditional)(assignment), 1e-5)); + + // Check if the clique conditional generated from incremental elimination + // matches that of batch elimination. + auto expectedChordal = expectedRemainingGraph->eliminateMultifrontal(); + auto expectedConditional = dynamic_pointer_cast( + (*expectedChordal)[M(2)]->conditional()->inner()); + auto actualConditional = dynamic_pointer_cast( + bayesTree[M(2)]->conditional()->inner()); + EXPECT(assert_equal(*actualConditional, *expectedConditional, 1e-6)); +} + +/* ****************************************************************************/ +// Test if we can approximately do the inference +TEST(HybridNonlinearISAM, Approx_inference) { + Switching switching(4); + HybridNonlinearISAM incrementalHybrid; + HybridNonlinearFactorGraph graph1; + Values initial; + + // Add the 3 hybrid factors, x1-x2, x2-x3, x3-x4 + for (size_t i = 1; i < 4; i++) { + graph1.push_back(switching.nonlinearFactorGraph.at(i)); + } + + // Add the Gaussian factors, 1 prior on X(1), + // 3 measurements on X(2), X(3), X(4) + graph1.push_back(switching.nonlinearFactorGraph.at(0)); + for (size_t i = 4; i <= 7; i++) { + graph1.push_back(switching.nonlinearFactorGraph.at(i)); + initial.insert(X(i - 3), i - 3); + } + + // Create ordering. + Ordering ordering; + for (size_t j = 1; j <= 4; j++) { + ordering += X(j); + } + + // Now we calculate the actual factors using full elimination + HybridBayesTree::shared_ptr unprunedHybridBayesTree; + HybridGaussianFactorGraph::shared_ptr unprunedRemainingGraph; + std::tie(unprunedHybridBayesTree, unprunedRemainingGraph) = + switching.linearizedFactorGraph.eliminatePartialMultifrontal(ordering); + + size_t maxNrLeaves = 5; + incrementalHybrid.update(graph1, initial); + HybridGaussianISAM bayesTree = incrementalHybrid.bayesTree(); + + bayesTree.prune(M(3), maxNrLeaves); + + /* + unpruned factor is: + Choice(m3) + 0 Choice(m2) + 0 0 Choice(m1) + 0 0 0 Leaf 0.11267528 + 0 0 1 Leaf 0.18576102 + 0 1 Choice(m1) + 0 1 0 Leaf 0.18754662 + 0 1 1 Leaf 0.30623871 + 1 Choice(m2) + 1 0 Choice(m1) + 1 0 0 Leaf 0.18576102 + 1 0 1 Leaf 0.30622428 + 1 1 Choice(m1) + 1 1 0 Leaf 0.30623871 + 1 1 1 Leaf 0.5 + + pruned factors is: + Choice(m3) + 0 Choice(m2) + 0 0 Leaf 0 + 0 1 Choice(m1) + 0 1 0 Leaf 0.18754662 + 0 1 1 Leaf 0.30623871 + 1 Choice(m2) + 1 0 Choice(m1) + 1 0 0 Leaf 0 + 1 0 1 Leaf 0.30622428 + 1 1 Choice(m1) + 1 1 0 Leaf 0.30623871 + 1 1 1 Leaf 0.5 + */ + + auto discreteConditional_m1 = *dynamic_pointer_cast( + bayesTree[M(1)]->conditional()->inner()); + EXPECT(discreteConditional_m1.keys() == KeyVector({M(1), M(2), M(3)})); + + // Get the number of elements which are greater than 0. + auto count = [](const double &value, int count) { + return value > 0 ? count + 1 : count; + }; + // Check that the number of leaves after pruning is 5. + EXPECT_LONGS_EQUAL(5, discreteConditional_m1.fold(count, 0)); + + // Check that the hybrid nodes of the bayes net match those of the pre-pruning + // bayes net, at the same positions. + auto &unprunedLastDensity = *dynamic_pointer_cast( + unprunedHybridBayesTree->clique(X(4))->conditional()->inner()); + auto &lastDensity = *dynamic_pointer_cast( + bayesTree[X(4)]->conditional()->inner()); + + std::vector> assignments = + discreteConditional_m1.enumerate(); + // Loop over all assignments and check the pruned components + for (auto &&av : assignments) { + const DiscreteValues &assignment = av.first; + const double value = av.second; + + if (value == 0.0) { + EXPECT(lastDensity(assignment) == nullptr); + } else { + CHECK(lastDensity(assignment)); + EXPECT(assert_equal(*unprunedLastDensity(assignment), + *lastDensity(assignment))); + } + } +} + +/* ****************************************************************************/ +// Test approximate inference with an additional pruning step. +TEST(HybridNonlinearISAM, Incremental_approximate) { + Switching switching(5); + HybridNonlinearISAM incrementalHybrid; + HybridNonlinearFactorGraph graph1; + Values initial; + + /***** Run Round 1 *****/ + // Add the 3 hybrid factors, x1-x2, x2-x3, x3-x4 + for (size_t i = 1; i < 4; i++) { + graph1.push_back(switching.nonlinearFactorGraph.at(i)); + } + + // Add the Gaussian factors, 1 prior on X(1), + // 3 measurements on X(2), X(3), X(4) + graph1.push_back(switching.nonlinearFactorGraph.at(0)); + initial.insert(X(1), 1); + for (size_t i = 5; i <= 7; i++) { + graph1.push_back(switching.nonlinearFactorGraph.at(i)); + initial.insert(X(i - 3), i - 3); + } + + // Run update with pruning + size_t maxComponents = 5; + incrementalHybrid.update(graph1, initial); + HybridGaussianISAM bayesTree = incrementalHybrid.bayesTree(); + + bayesTree.prune(M(3), maxComponents); + + // Check if we have a bayes tree with 4 hybrid nodes, + // each with 2, 4, 8, and 5 (pruned) leaves respetively. + EXPECT_LONGS_EQUAL(4, bayesTree.size()); + EXPECT_LONGS_EQUAL( + 2, bayesTree[X(1)]->conditional()->asMixture()->nrComponents()); + EXPECT_LONGS_EQUAL( + 4, bayesTree[X(2)]->conditional()->asMixture()->nrComponents()); + EXPECT_LONGS_EQUAL( + 5, bayesTree[X(3)]->conditional()->asMixture()->nrComponents()); + EXPECT_LONGS_EQUAL( + 5, bayesTree[X(4)]->conditional()->asMixture()->nrComponents()); + + /***** Run Round 2 *****/ + HybridGaussianFactorGraph graph2; + graph2.push_back(switching.nonlinearFactorGraph.at(4)); // x4-x5 + graph2.push_back(switching.nonlinearFactorGraph.at(8)); // x5 measurement + initial = Values(); + initial.insert(X(5), 5); + + // Run update with pruning a second time. + incrementalHybrid.update(graph2, initial); + bayesTree = incrementalHybrid.bayesTree(); + + bayesTree.prune(M(4), maxComponents); + + // Check if we have a bayes tree with pruned hybrid nodes, + // with 5 (pruned) leaves. + CHECK_EQUAL(5, bayesTree.size()); + EXPECT_LONGS_EQUAL( + 5, bayesTree[X(4)]->conditional()->asMixture()->nrComponents()); + EXPECT_LONGS_EQUAL( + 5, bayesTree[X(5)]->conditional()->asMixture()->nrComponents()); +} + +/* ************************************************************************/ +// A GTSAM-only test for running inference on a single-legged robot. +// The leg links are represented by the chain X-Y-Z-W, where X is the base and +// W is the foot. +// We use BetweenFactor as constraints between each of the poses. +TEST(HybridGaussianISAM, NonTrivial) { + /*************** Run Round 1 ***************/ + HybridNonlinearFactorGraph fg; + HybridNonlinearISAM inc; + + // Add a prior on pose x1 at the origin. + // A prior factor consists of a mean and + // a noise model (covariance matrix) + Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin + auto priorNoise = noiseModel::Diagonal::Sigmas( + Vector3(0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta + fg.emplace_nonlinear>(X(0), prior, priorNoise); + + // create a noise model for the landmark measurements + auto poseNoise = noiseModel::Isotropic::Sigma(3, 0.1); + + // We model a robot's single leg as X - Y - Z - W + // where X is the base link and W is the foot link. + + // Add connecting poses similar to PoseFactors in GTD + fg.emplace_nonlinear>(X(0), Y(0), Pose2(0, 1.0, 0), + poseNoise); + fg.emplace_nonlinear>(Y(0), Z(0), Pose2(0, 1.0, 0), + poseNoise); + fg.emplace_nonlinear>(Z(0), W(0), Pose2(0, 1.0, 0), + poseNoise); + + // Create initial estimate + Values initial; + initial.insert(X(0), Pose2(0.0, 0.0, 0.0)); + initial.insert(Y(0), Pose2(0.0, 1.0, 0.0)); + initial.insert(Z(0), Pose2(0.0, 2.0, 0.0)); + initial.insert(W(0), Pose2(0.0, 3.0, 0.0)); + + // Don't run update now since we don't have discrete variables involved. + + /*************** Run Round 2 ***************/ + using PlanarMotionModel = BetweenFactor; + + // Add odometry factor with discrete modes. + Pose2 odometry(1.0, 0.0, 0.0); + KeyVector contKeys = {W(0), W(1)}; + auto noise_model = noiseModel::Isotropic::Sigma(3, 1.0); + auto still = boost::make_shared(W(0), W(1), Pose2(0, 0, 0), + noise_model), + moving = boost::make_shared(W(0), W(1), odometry, + noise_model); + std::vector components = {moving, still}; + auto mixtureFactor = boost::make_shared( + contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, components); + fg.push_back(mixtureFactor); + + // Add equivalent of ImuFactor + fg.emplace_nonlinear>(X(0), X(1), Pose2(1.0, 0.0, 0), + poseNoise); + // PoseFactors-like at k=1 + fg.emplace_nonlinear>(X(1), Y(1), Pose2(0, 1, 0), + poseNoise); + fg.emplace_nonlinear>(Y(1), Z(1), Pose2(0, 1, 0), + poseNoise); + fg.emplace_nonlinear>(Z(1), W(1), Pose2(-1, 1, 0), + poseNoise); + + initial.insert(X(1), Pose2(1.0, 0.0, 0.0)); + initial.insert(Y(1), Pose2(1.0, 1.0, 0.0)); + initial.insert(Z(1), Pose2(1.0, 2.0, 0.0)); + // The leg link did not move so we set the expected pose accordingly. + initial.insert(W(1), Pose2(0.0, 3.0, 0.0)); + + // Update without pruning + // The result is a HybridBayesNet with 1 discrete variable M(1). + // P(X | measurements) = P(W0|Z0, W1, M1) P(Z0|Y0, W1, M1) P(Y0|X0, W1, M1) + // P(X0 | X1, W1, M1) P(W1|Z1, X1, M1) P(Z1|Y1, X1, M1) + // P(Y1 | X1, M1)P(X1 | M1)P(M1) + // The MHS tree is a 1 level tree for time indices (1,) with 2 leaves. + inc.update(fg, initial); + + fg = HybridNonlinearFactorGraph(); + initial = Values(); + + /*************** Run Round 3 ***************/ + // Add odometry factor with discrete modes. + contKeys = {W(1), W(2)}; + still = boost::make_shared(W(1), W(2), Pose2(0, 0, 0), + noise_model); + moving = + boost::make_shared(W(1), W(2), odometry, + noise_model); + components = {moving, still}; + mixtureFactor = boost::make_shared( + contKeys, DiscreteKeys{gtsam::DiscreteKey(M(2), 2)}, components); + fg.push_back(mixtureFactor); + + // Add equivalent of ImuFactor + fg.emplace_nonlinear>(X(1), X(2), Pose2(1.0, 0.0, 0), + poseNoise); + // PoseFactors-like at k=1 + fg.emplace_nonlinear>(X(2), Y(2), Pose2(0, 1, 0), + poseNoise); + fg.emplace_nonlinear>(Y(2), Z(2), Pose2(0, 1, 0), + poseNoise); + fg.emplace_nonlinear>(Z(2), W(2), Pose2(-2, 1, 0), + poseNoise); + + initial.insert(X(2), Pose2(2.0, 0.0, 0.0)); + initial.insert(Y(2), Pose2(2.0, 1.0, 0.0)); + initial.insert(Z(2), Pose2(2.0, 2.0, 0.0)); + initial.insert(W(2), Pose2(0.0, 3.0, 0.0)); + + // Now we prune! + // P(X | measurements) = P(W0|Z0, W1, M1) P(Z0|Y0, W1, M1) P(Y0|X0, W1, M1) + // P(X0 | X1, W1, M1) P(W1|W2, Z1, X1, M1, M2) + // P(Z1| W2, Y1, X1, M1, M2) P(Y1 | W2, X1, M1, M2) + // P(X1 | W2, X2, M1, M2) P(W2|Z2, X2, M1, M2) + // P(Z2|Y2, X2, M1, M2) P(Y2 | X2, M1, M2) + // P(X2 | M1, M2) P(M1, M2) + // The MHS at this point should be a 2 level tree on (1, 2). + // 1 has 2 choices, and 2 has 4 choices. + inc.update(fg, initial); + inc.prune(M(2), 2); + + fg = HybridNonlinearFactorGraph(); + initial = Values(); + + /*************** Run Round 4 ***************/ + // Add odometry factor with discrete modes. + contKeys = {W(2), W(3)}; + still = boost::make_shared(W(2), W(3), Pose2(0, 0, 0), + noise_model); + moving = + boost::make_shared(W(2), W(3), odometry, + noise_model); + components = {moving, still}; + mixtureFactor = boost::make_shared( + contKeys, DiscreteKeys{gtsam::DiscreteKey(M(3), 2)}, components); + fg.push_back(mixtureFactor); + + // Add equivalent of ImuFactor + fg.emplace_nonlinear>(X(2), X(3), Pose2(1.0, 0.0, 0), + poseNoise); + // PoseFactors-like at k=3 + fg.emplace_nonlinear>(X(3), Y(3), Pose2(0, 1, 0), + poseNoise); + fg.emplace_nonlinear>(Y(3), Z(3), Pose2(0, 1, 0), + poseNoise); + fg.emplace_nonlinear>(Z(3), W(3), Pose2(-3, 1, 0), + poseNoise); + + initial.insert(X(3), Pose2(3.0, 0.0, 0.0)); + initial.insert(Y(3), Pose2(3.0, 1.0, 0.0)); + initial.insert(Z(3), Pose2(3.0, 2.0, 0.0)); + initial.insert(W(3), Pose2(0.0, 3.0, 0.0)); + + // Keep pruning! + inc.update(fg, initial); + inc.prune(M(3), 3); + + fg = HybridNonlinearFactorGraph(); + initial = Values(); + + HybridGaussianISAM bayesTree = inc.bayesTree(); + + // The final discrete graph should not be empty since we have eliminated + // all continuous variables. + auto discreteTree = + bayesTree[M(3)]->conditional()->asDiscreteConditional(); + EXPECT_LONGS_EQUAL(3, discreteTree->size()); + + // Test if the optimal discrete mode assignment is (1, 1, 1). + DiscreteFactorGraph discreteGraph; + discreteGraph.push_back(discreteTree); + DiscreteValues optimal_assignment = discreteGraph.optimize(); + + DiscreteValues expected_assignment; + expected_assignment[M(1)] = 1; + expected_assignment[M(2)] = 1; + expected_assignment[M(3)] = 1; + + EXPECT(assert_equal(expected_assignment, optimal_assignment)); + + // Test if pruning worked correctly by checking that + // we only have 3 leaves in the last node. + auto lastConditional = bayesTree[X(3)]->conditional()->asMixture(); + EXPECT_LONGS_EQUAL(3, lastConditional->nrComponents()); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} From 4f22a0651916cd4493ea9fd974001698fe825168 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 14 Sep 2022 14:49:48 -0400 Subject: [PATCH 059/118] add container SFINAE for hybrid nonlinear factor graph push_back --- gtsam/hybrid/HybridNonlinearFactorGraph.h | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/gtsam/hybrid/HybridNonlinearFactorGraph.h b/gtsam/hybrid/HybridNonlinearFactorGraph.h index 5f04851d4..b48e8bb5c 100644 --- a/gtsam/hybrid/HybridNonlinearFactorGraph.h +++ b/gtsam/hybrid/HybridNonlinearFactorGraph.h @@ -42,6 +42,16 @@ class GTSAM_EXPORT HybridNonlinearFactorGraph : public HybridFactorGraph { using IsNonlinear = typename std::enable_if< std::is_base_of::value>::type; + /// Check if T has a value_type derived from FactorType. + template + using HasDerivedValueType = typename std::enable_if< + std::is_base_of::value>::type; + + /// Check if T has a pointer type derived from FactorType. + template + using HasDerivedElementType = typename std::enable_if::value>::type; + public: using Base = HybridFactorGraph; using This = HybridNonlinearFactorGraph; ///< this class @@ -114,15 +124,13 @@ class GTSAM_EXPORT HybridNonlinearFactorGraph : public HybridFactorGraph { * copied) */ template - Base::Base::HasDerivedElementType push_back( - const CONTAINER& container) { + HasDerivedElementType push_back(const CONTAINER& container) { Base::push_back(container.begin(), container.end()); } /// Push back non-pointer objects in a container (factors are copied). template - Base::Base::HasDerivedValueType push_back( - const CONTAINER& container) { + HasDerivedValueType push_back(const CONTAINER& container) { Base::push_back(container.begin(), container.end()); } From 8c0648582eb3bd55bfa1cf0359b02fb7d679ffef Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 14 Sep 2022 15:32:38 -0400 Subject: [PATCH 060/118] fix test name for HybridNonlinearISAM --- gtsam/hybrid/tests/testHybridNonlinearISAM.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp index bed90fe2c..46076b330 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp @@ -396,7 +396,7 @@ TEST(HybridNonlinearISAM, Incremental_approximate) { // The leg links are represented by the chain X-Y-Z-W, where X is the base and // W is the foot. // We use BetweenFactor as constraints between each of the poses. -TEST(HybridGaussianISAM, NonTrivial) { +TEST(HybridNonlinearISAM, NonTrivial) { /*************** Run Round 1 ***************/ HybridNonlinearFactorGraph fg; HybridNonlinearISAM inc; From c4d388990d44294e6ade625917ff20f2076d307f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 15 Sep 2022 13:23:12 -0400 Subject: [PATCH 061/118] prune hybrid gaussian ISAM more efficiently and without the need to specify the root --- gtsam/hybrid/HybridBayesTree.cpp | 4 +- gtsam/hybrid/HybridGaussianISAM.cpp | 72 ++++++++++++------- gtsam/hybrid/HybridGaussianISAM.h | 7 +- gtsam/hybrid/HybridNonlinearISAM.h | 7 +- gtsam/hybrid/tests/testHybridIncremental.cpp | 10 +-- .../hybrid/tests/testHybridNonlinearISAM.cpp | 19 +++-- 6 files changed, 67 insertions(+), 52 deletions(-) diff --git a/gtsam/hybrid/HybridBayesTree.cpp b/gtsam/hybrid/HybridBayesTree.cpp index 3fa4d6268..d600714d4 100644 --- a/gtsam/hybrid/HybridBayesTree.cpp +++ b/gtsam/hybrid/HybridBayesTree.cpp @@ -89,12 +89,12 @@ struct HybridAssignmentData { gaussianbayesTree_(gbt) {} /** - * @brief A function used during tree traversal that operators on each node + * @brief A function used during tree traversal that operates on each node * before visiting the node's children. * * @param node The current node being visited. * @param parentData The HybridAssignmentData from the parent node. - * @return HybridAssignmentData + * @return HybridAssignmentData which is passed to the children. */ static HybridAssignmentData AssignmentPreOrderVisitor( const HybridBayesTree::sharedNode& node, diff --git a/gtsam/hybrid/HybridGaussianISAM.cpp b/gtsam/hybrid/HybridGaussianISAM.cpp index 23a95c021..bf2f60da6 100644 --- a/gtsam/hybrid/HybridGaussianISAM.cpp +++ b/gtsam/hybrid/HybridGaussianISAM.cpp @@ -14,9 +14,10 @@ * @date March 31, 2022 * @author Fan Jiang * @author Frank Dellaert - * @author Richard Roberts + * @author Varun Agrawal */ +#include #include #include #include @@ -121,38 +122,59 @@ bool IsSubset(KeyVector a, KeyVector b) { } /* ************************************************************************* */ -void HybridGaussianISAM::prune(const Key& root, const size_t maxNrLeaves) { +void HybridGaussianISAM::prune(const size_t maxNrLeaves) { auto decisionTree = boost::dynamic_pointer_cast( - this->clique(root)->conditional()->inner()); + this->roots_.at(0)->conditional()->inner()); + DecisionTreeFactor prunedDiscreteFactor = decisionTree->prune(maxNrLeaves); decisionTree->root_ = prunedDiscreteFactor.root_; - std::vector prunedKeys; - for (auto&& clique : nodes()) { - // The cliques can be repeated for each frontal so we record it in - // prunedKeys and check if we have already pruned a particular clique. - if (std::find(prunedKeys.begin(), prunedKeys.end(), clique.first) != - prunedKeys.end()) { - continue; - } + /// Helper struct for pruning the hybrid bayes tree. + struct HybridPrunerData { + /// The discrete decision tree after pruning. + DecisionTreeFactor prunedDiscreteFactor; + HybridPrunerData(const DecisionTreeFactor& prunedDiscreteFactor, + const HybridBayesTree::sharedNode& parentClique) + : prunedDiscreteFactor(prunedDiscreteFactor) {} - // Add all the keys of the current clique to be pruned to prunedKeys - for (auto&& key : clique.second->conditional()->frontals()) { - prunedKeys.push_back(key); - } + /** + * @brief A function used during tree traversal that operates on each node + * before visiting the node's children. + * + * @param node The current node being visited. + * @param parentData The data from the parent node. + * @return HybridPrunerData which is passed to the children. + */ + static HybridPrunerData AssignmentPreOrderVisitor( + const HybridBayesTree::sharedNode& clique, + HybridPrunerData& parentData) { + // Get the conditional + HybridConditional::shared_ptr conditional = clique->conditional(); - // Convert parents() to a KeyVector for comparison - KeyVector parents; - for (auto&& parent : clique.second->conditional()->parents()) { - parents.push_back(parent); - } + // If conditional is hybrid, we prune it. + if (conditional->isHybrid()) { + auto gaussianMixture = conditional->asMixture(); - if (IsSubset(parents, decisionTree->keys())) { - auto gaussianMixture = boost::dynamic_pointer_cast( - clique.second->conditional()->inner()); - - gaussianMixture->prune(prunedDiscreteFactor); + // Check if the number of discrete keys match, + // else we get an assignment error. + // TODO(Varun) Update prune method to handle assignment subset? + if (gaussianMixture->discreteKeys() == + parentData.prunedDiscreteFactor.discreteKeys()) { + gaussianMixture->prune(parentData.prunedDiscreteFactor); + } + } + return parentData; } + }; + + HybridPrunerData rootData(prunedDiscreteFactor, 0); + { + treeTraversal::no_op visitorPost; + // Limits OpenMP threads since we're mixing TBB and OpenMP + TbbOpenMPMixedScope threadLimiter; + treeTraversal::DepthFirstForestParallel( + *this, rootData, HybridPrunerData::AssignmentPreOrderVisitor, + visitorPost); } } diff --git a/gtsam/hybrid/HybridGaussianISAM.h b/gtsam/hybrid/HybridGaussianISAM.h index ff0978729..59e221807 100644 --- a/gtsam/hybrid/HybridGaussianISAM.h +++ b/gtsam/hybrid/HybridGaussianISAM.h @@ -66,11 +66,10 @@ class GTSAM_EXPORT HybridGaussianISAM : public ISAM { /** * @brief Prune the underlying Bayes tree. - * - * @param root The root key in the discrete conditional decision tree. - * @param maxNumberLeaves + * + * @param maxNumberLeaves The max number of leaf nodes to keep. */ - void prune(const Key& root, const size_t maxNumberLeaves); + void prune(const size_t maxNumberLeaves); }; /// traits diff --git a/gtsam/hybrid/HybridNonlinearISAM.h b/gtsam/hybrid/HybridNonlinearISAM.h index d96485fff..b1998fb30 100644 --- a/gtsam/hybrid/HybridNonlinearISAM.h +++ b/gtsam/hybrid/HybridNonlinearISAM.h @@ -82,12 +82,9 @@ class GTSAM_EXPORT HybridNonlinearISAM { /** * @brief Prune the underlying Bayes tree. * - * @param root The root key in the discrete conditional decision tree. - * @param maxNumberLeaves + * @param maxNumberLeaves The max number of leaf nodes to keep. */ - void prune(const Key& root, const size_t maxNumberLeaves) { - isam_.prune(root, maxNumberLeaves); - } + void prune(const size_t maxNumberLeaves) { isam_.prune(maxNumberLeaves); } /** Return the current linearization point */ const Values& getLinearizationPoint() const { return linPoint_; } diff --git a/gtsam/hybrid/tests/testHybridIncremental.cpp b/gtsam/hybrid/tests/testHybridIncremental.cpp index 8e16d02b9..a0a87933f 100644 --- a/gtsam/hybrid/tests/testHybridIncremental.cpp +++ b/gtsam/hybrid/tests/testHybridIncremental.cpp @@ -235,7 +235,7 @@ TEST(HybridGaussianElimination, Approx_inference) { size_t maxNrLeaves = 5; incrementalHybrid.update(graph1); - incrementalHybrid.prune(M(3), maxNrLeaves); + incrementalHybrid.prune(maxNrLeaves); /* unpruned factor is: @@ -329,7 +329,7 @@ TEST(HybridGaussianElimination, Incremental_approximate) { // Run update with pruning size_t maxComponents = 5; incrementalHybrid.update(graph1); - incrementalHybrid.prune(M(3), maxComponents); + incrementalHybrid.prune(maxComponents); // Check if we have a bayes tree with 4 hybrid nodes, // each with 2, 4, 8, and 5 (pruned) leaves respetively. @@ -350,7 +350,7 @@ TEST(HybridGaussianElimination, Incremental_approximate) { // Run update with pruning a second time. incrementalHybrid.update(graph2); - incrementalHybrid.prune(M(4), maxComponents); + incrementalHybrid.prune(maxComponents); // Check if we have a bayes tree with pruned hybrid nodes, // with 5 (pruned) leaves. @@ -496,7 +496,7 @@ TEST(HybridGaussianISAM, NonTrivial) { // The MHS at this point should be a 2 level tree on (1, 2). // 1 has 2 choices, and 2 has 4 choices. inc.update(gfg); - inc.prune(M(2), 2); + inc.prune(2); /*************** Run Round 4 ***************/ // Add odometry factor with discrete modes. @@ -531,7 +531,7 @@ TEST(HybridGaussianISAM, NonTrivial) { // Keep pruning! inc.update(gfg); - inc.prune(M(3), 3); + inc.prune(3); // The final discrete graph should not be empty since we have eliminated // all continuous variables. diff --git a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp index 46076b330..4e1710c42 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp @@ -256,7 +256,7 @@ TEST(HybridNonlinearISAM, Approx_inference) { incrementalHybrid.update(graph1, initial); HybridGaussianISAM bayesTree = incrementalHybrid.bayesTree(); - bayesTree.prune(M(3), maxNrLeaves); + bayesTree.prune(maxNrLeaves); /* unpruned factor is: @@ -355,7 +355,7 @@ TEST(HybridNonlinearISAM, Incremental_approximate) { incrementalHybrid.update(graph1, initial); HybridGaussianISAM bayesTree = incrementalHybrid.bayesTree(); - bayesTree.prune(M(3), maxComponents); + bayesTree.prune(maxComponents); // Check if we have a bayes tree with 4 hybrid nodes, // each with 2, 4, 8, and 5 (pruned) leaves respetively. @@ -380,7 +380,7 @@ TEST(HybridNonlinearISAM, Incremental_approximate) { incrementalHybrid.update(graph2, initial); bayesTree = incrementalHybrid.bayesTree(); - bayesTree.prune(M(4), maxComponents); + bayesTree.prune(maxComponents); // Check if we have a bayes tree with pruned hybrid nodes, // with 5 (pruned) leaves. @@ -482,8 +482,7 @@ TEST(HybridNonlinearISAM, NonTrivial) { still = boost::make_shared(W(1), W(2), Pose2(0, 0, 0), noise_model); moving = - boost::make_shared(W(1), W(2), odometry, - noise_model); + boost::make_shared(W(1), W(2), odometry, noise_model); components = {moving, still}; mixtureFactor = boost::make_shared( contKeys, DiscreteKeys{gtsam::DiscreteKey(M(2), 2)}, components); @@ -515,7 +514,7 @@ TEST(HybridNonlinearISAM, NonTrivial) { // The MHS at this point should be a 2 level tree on (1, 2). // 1 has 2 choices, and 2 has 4 choices. inc.update(fg, initial); - inc.prune(M(2), 2); + inc.prune(2); fg = HybridNonlinearFactorGraph(); initial = Values(); @@ -526,8 +525,7 @@ TEST(HybridNonlinearISAM, NonTrivial) { still = boost::make_shared(W(2), W(3), Pose2(0, 0, 0), noise_model); moving = - boost::make_shared(W(2), W(3), odometry, - noise_model); + boost::make_shared(W(2), W(3), odometry, noise_model); components = {moving, still}; mixtureFactor = boost::make_shared( contKeys, DiscreteKeys{gtsam::DiscreteKey(M(3), 2)}, components); @@ -551,7 +549,7 @@ TEST(HybridNonlinearISAM, NonTrivial) { // Keep pruning! inc.update(fg, initial); - inc.prune(M(3), 3); + inc.prune(3); fg = HybridNonlinearFactorGraph(); initial = Values(); @@ -560,8 +558,7 @@ TEST(HybridNonlinearISAM, NonTrivial) { // The final discrete graph should not be empty since we have eliminated // all continuous variables. - auto discreteTree = - bayesTree[M(3)]->conditional()->asDiscreteConditional(); + auto discreteTree = bayesTree[M(3)]->conditional()->asDiscreteConditional(); EXPECT_LONGS_EQUAL(3, discreteTree->size()); // Test if the optimal discrete mode assignment is (1, 1, 1). From aef1669a5072670d6db8dcdc64a94299c88c8421 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 15 Sep 2022 13:24:33 -0400 Subject: [PATCH 062/118] Add labelformatter to Assignment for convenience --- gtsam/discrete/Assignment.h | 27 +++++++++++++++++++++++---- 1 file changed, 23 insertions(+), 4 deletions(-) diff --git a/gtsam/discrete/Assignment.h b/gtsam/discrete/Assignment.h index 90e2dbdd8..c6e426ca1 100644 --- a/gtsam/discrete/Assignment.h +++ b/gtsam/discrete/Assignment.h @@ -11,15 +11,17 @@ /** * @file Assignment.h - * @brief An assignment from labels to a discrete value index (size_t) + * @brief An assignment from labels to a discrete value index (size_t) * @author Frank Dellaert * @date Feb 5, 2012 */ #pragma once +#include #include #include +#include #include #include @@ -32,13 +34,30 @@ namespace gtsam { */ template class Assignment : public std::map { + /** + * @brief Default method used by `labelFormatter` or `valueFormatter` when + * printing. + * + * @param x The value passed to format. + * @return std::string + */ + static std::string DefaultFormatter(const L& x) { + std::stringstream ss; + ss << x; + return ss.str(); + } + public: using std::map::operator=; - void print(const std::string& s = "Assignment: ") const { + void print(const std::string& s = "Assignment: ", + const std::function& labelFormatter = + &DefaultFormatter) const { std::cout << s << ": "; - for (const typename Assignment::value_type& keyValue : *this) - std::cout << "(" << keyValue.first << ", " << keyValue.second << ")"; + for (const typename Assignment::value_type& keyValue : *this) { + std::cout << "(" << labelFormatter(keyValue.first) << ", " + << keyValue.second << ")"; + } std::cout << std::endl; } From 8b5586f3b61e02e08d3e2621f83dd33915225a71 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 16 Sep 2022 10:23:00 -0400 Subject: [PATCH 063/118] move prune method to HybridBayesTree class --- gtsam/hybrid/HybridBayesTree.cpp | 57 +++++++++++++++++++++++ gtsam/hybrid/HybridBayesTree.h | 7 +++ gtsam/hybrid/HybridGaussianISAM.cpp | 72 ----------------------------- 3 files changed, 64 insertions(+), 72 deletions(-) diff --git a/gtsam/hybrid/HybridBayesTree.cpp b/gtsam/hybrid/HybridBayesTree.cpp index d600714d4..8fb487ae2 100644 --- a/gtsam/hybrid/HybridBayesTree.cpp +++ b/gtsam/hybrid/HybridBayesTree.cpp @@ -144,4 +144,61 @@ VectorValues HybridBayesTree::optimize(const DiscreteValues& assignment) const { return result; } +/* ************************************************************************* */ +void HybridBayesTree::prune(const size_t maxNrLeaves) { + auto decisionTree = boost::dynamic_pointer_cast( + this->roots_.at(0)->conditional()->inner()); + + DecisionTreeFactor prunedDiscreteFactor = decisionTree->prune(maxNrLeaves); + decisionTree->root_ = prunedDiscreteFactor.root_; + + /// Helper struct for pruning the hybrid bayes tree. + struct HybridPrunerData { + /// The discrete decision tree after pruning. + DecisionTreeFactor prunedDiscreteFactor; + HybridPrunerData(const DecisionTreeFactor& prunedDiscreteFactor, + const HybridBayesTree::sharedNode& parentClique) + : prunedDiscreteFactor(prunedDiscreteFactor) {} + + /** + * @brief A function used during tree traversal that operates on each node + * before visiting the node's children. + * + * @param node The current node being visited. + * @param parentData The data from the parent node. + * @return HybridPrunerData which is passed to the children. + */ + static HybridPrunerData AssignmentPreOrderVisitor( + const HybridBayesTree::sharedNode& clique, + HybridPrunerData& parentData) { + // Get the conditional + HybridConditional::shared_ptr conditional = clique->conditional(); + + // If conditional is hybrid, we prune it. + if (conditional->isHybrid()) { + auto gaussianMixture = conditional->asMixture(); + + // Check if the number of discrete keys match, + // else we get an assignment error. + // TODO(Varun) Update prune method to handle assignment subset? + if (gaussianMixture->discreteKeys() == + parentData.prunedDiscreteFactor.discreteKeys()) { + gaussianMixture->prune(parentData.prunedDiscreteFactor); + } + } + return parentData; + } + }; + + HybridPrunerData rootData(prunedDiscreteFactor, 0); + { + treeTraversal::no_op visitorPost; + // Limits OpenMP threads since we're mixing TBB and OpenMP + TbbOpenMPMixedScope threadLimiter; + treeTraversal::DepthFirstForestParallel( + *this, rootData, HybridPrunerData::AssignmentPreOrderVisitor, + visitorPost); + } +} + } // namespace gtsam diff --git a/gtsam/hybrid/HybridBayesTree.h b/gtsam/hybrid/HybridBayesTree.h index 3fa344d4d..d443e33c4 100644 --- a/gtsam/hybrid/HybridBayesTree.h +++ b/gtsam/hybrid/HybridBayesTree.h @@ -88,6 +88,13 @@ class GTSAM_EXPORT HybridBayesTree : public BayesTree { */ VectorValues optimize(const DiscreteValues& assignment) const; + /** + * @brief Prune the underlying Bayes tree. + * + * @param maxNumberLeaves The max number of leaf nodes to keep. + */ + void prune(const size_t maxNumberLeaves); + /// @} private: diff --git a/gtsam/hybrid/HybridGaussianISAM.cpp b/gtsam/hybrid/HybridGaussianISAM.cpp index bf2f60da6..0a9d0b0de 100644 --- a/gtsam/hybrid/HybridGaussianISAM.cpp +++ b/gtsam/hybrid/HybridGaussianISAM.cpp @@ -106,76 +106,4 @@ void HybridGaussianISAM::update(const HybridGaussianFactorGraph& newFactors, this->updateInternal(newFactors, &orphans, ordering, function); } -/* ************************************************************************* */ -/** - * @brief Check if `b` is a subset of `a`. - * Non-const since they need to be sorted. - * - * @param a KeyVector - * @param b KeyVector - * @return True if the keys of b is a subset of a, else false. - */ -bool IsSubset(KeyVector a, KeyVector b) { - std::sort(a.begin(), a.end()); - std::sort(b.begin(), b.end()); - return std::includes(a.begin(), a.end(), b.begin(), b.end()); -} - -/* ************************************************************************* */ -void HybridGaussianISAM::prune(const size_t maxNrLeaves) { - auto decisionTree = boost::dynamic_pointer_cast( - this->roots_.at(0)->conditional()->inner()); - - DecisionTreeFactor prunedDiscreteFactor = decisionTree->prune(maxNrLeaves); - decisionTree->root_ = prunedDiscreteFactor.root_; - - /// Helper struct for pruning the hybrid bayes tree. - struct HybridPrunerData { - /// The discrete decision tree after pruning. - DecisionTreeFactor prunedDiscreteFactor; - HybridPrunerData(const DecisionTreeFactor& prunedDiscreteFactor, - const HybridBayesTree::sharedNode& parentClique) - : prunedDiscreteFactor(prunedDiscreteFactor) {} - - /** - * @brief A function used during tree traversal that operates on each node - * before visiting the node's children. - * - * @param node The current node being visited. - * @param parentData The data from the parent node. - * @return HybridPrunerData which is passed to the children. - */ - static HybridPrunerData AssignmentPreOrderVisitor( - const HybridBayesTree::sharedNode& clique, - HybridPrunerData& parentData) { - // Get the conditional - HybridConditional::shared_ptr conditional = clique->conditional(); - - // If conditional is hybrid, we prune it. - if (conditional->isHybrid()) { - auto gaussianMixture = conditional->asMixture(); - - // Check if the number of discrete keys match, - // else we get an assignment error. - // TODO(Varun) Update prune method to handle assignment subset? - if (gaussianMixture->discreteKeys() == - parentData.prunedDiscreteFactor.discreteKeys()) { - gaussianMixture->prune(parentData.prunedDiscreteFactor); - } - } - return parentData; - } - }; - - HybridPrunerData rootData(prunedDiscreteFactor, 0); - { - treeTraversal::no_op visitorPost; - // Limits OpenMP threads since we're mixing TBB and OpenMP - TbbOpenMPMixedScope threadLimiter; - treeTraversal::DepthFirstForestParallel( - *this, rootData, HybridPrunerData::AssignmentPreOrderVisitor, - visitorPost); - } -} - } // namespace gtsam From dcad55c0322df722d4a58bb683879bc93bea4de6 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 16 Sep 2022 10:26:25 -0400 Subject: [PATCH 064/118] optional maxNrLeaves for HybridGaussianISAM --- gtsam/hybrid/HybridGaussianISAM.cpp | 8 +++++++- gtsam/hybrid/HybridGaussianISAM.h | 11 ++++------- 2 files changed, 11 insertions(+), 8 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianISAM.cpp b/gtsam/hybrid/HybridGaussianISAM.cpp index 0a9d0b0de..6946775b9 100644 --- a/gtsam/hybrid/HybridGaussianISAM.cpp +++ b/gtsam/hybrid/HybridGaussianISAM.cpp @@ -42,6 +42,7 @@ HybridGaussianISAM::HybridGaussianISAM(const HybridBayesTree& bayesTree) void HybridGaussianISAM::updateInternal( const HybridGaussianFactorGraph& newFactors, HybridBayesTree::Cliques* orphans, + const boost::optional& maxNrLeaves, const boost::optional& ordering, const HybridBayesTree::Eliminate& function) { // Remove the contaminated part of the Bayes tree @@ -92,6 +93,10 @@ void HybridGaussianISAM::updateInternal( HybridBayesTree::shared_ptr bayesTree = factors.eliminateMultifrontal(elimination_ordering, function, index); + if (maxNrLeaves) { + bayesTree->prune(*maxNrLeaves); + } + // Re-add into Bayes tree data structures this->roots_.insert(this->roots_.end(), bayesTree->roots().begin(), bayesTree->roots().end()); @@ -100,10 +105,11 @@ void HybridGaussianISAM::updateInternal( /* ************************************************************************* */ void HybridGaussianISAM::update(const HybridGaussianFactorGraph& newFactors, + const boost::optional& maxNrLeaves, const boost::optional& ordering, const HybridBayesTree::Eliminate& function) { Cliques orphans; - this->updateInternal(newFactors, &orphans, ordering, function); + this->updateInternal(newFactors, &orphans, maxNrLeaves, ordering, function); } } // namespace gtsam diff --git a/gtsam/hybrid/HybridGaussianISAM.h b/gtsam/hybrid/HybridGaussianISAM.h index 59e221807..a6a82b3e8 100644 --- a/gtsam/hybrid/HybridGaussianISAM.h +++ b/gtsam/hybrid/HybridGaussianISAM.h @@ -48,6 +48,7 @@ class GTSAM_EXPORT HybridGaussianISAM : public ISAM { void updateInternal( const HybridGaussianFactorGraph& newFactors, HybridBayesTree::Cliques* orphans, + const boost::optional& maxNrLeaves = boost::none, const boost::optional& ordering = boost::none, const HybridBayesTree::Eliminate& function = HybridBayesTree::EliminationTraitsType::DefaultEliminate); @@ -57,19 +58,15 @@ class GTSAM_EXPORT HybridGaussianISAM : public ISAM { * @brief Perform update step with new factors. * * @param newFactors Factor graph of new factors to add and eliminate. + * @param maxNrLeaves The maximum number of leaves to keep after pruning. + * @param ordering Custom elimination ordering. * @param function Elimination function. */ void update(const HybridGaussianFactorGraph& newFactors, + const boost::optional& maxNrLeaves = boost::none, const boost::optional& ordering = boost::none, const HybridBayesTree::Eliminate& function = HybridBayesTree::EliminationTraitsType::DefaultEliminate); - - /** - * @brief Prune the underlying Bayes tree. - * - * @param maxNumberLeaves The max number of leaf nodes to keep. - */ - void prune(const size_t maxNumberLeaves); }; /// traits From 57659261227a07806ab766e88638dd7b0fbd667b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 16 Sep 2022 10:47:01 -0400 Subject: [PATCH 065/118] optional ordering argument for HybridNonlinearISAM::update --- gtsam/hybrid/HybridNonlinearISAM.cpp | 9 ++++++--- gtsam/hybrid/HybridNonlinearISAM.h | 4 +++- 2 files changed, 9 insertions(+), 4 deletions(-) diff --git a/gtsam/hybrid/HybridNonlinearISAM.cpp b/gtsam/hybrid/HybridNonlinearISAM.cpp index 36cda4e80..d05e081dd 100644 --- a/gtsam/hybrid/HybridNonlinearISAM.cpp +++ b/gtsam/hybrid/HybridNonlinearISAM.cpp @@ -33,7 +33,9 @@ void HybridNonlinearISAM::saveGraph(const string& s, /* ************************************************************************* */ void HybridNonlinearISAM::update(const HybridNonlinearFactorGraph& newFactors, - const Values& initialValues) { + const Values& initialValues, + const boost::optional& maxNrLeaves, + const boost::optional& ordering) { if (newFactors.size() > 0) { // Reorder and relinearize every reorderInterval updates if (reorderInterval_ > 0 && ++reorderCounter_ >= reorderInterval_) { @@ -51,7 +53,8 @@ void HybridNonlinearISAM::update(const HybridNonlinearFactorGraph& newFactors, newFactors.linearize(linPoint_); // Update ISAM - isam_.update(*linearizedNewFactors, boost::none, eliminationFunction_); + isam_.update(*linearizedNewFactors, maxNrLeaves, ordering, + eliminationFunction_); } } @@ -66,7 +69,7 @@ void HybridNonlinearISAM::reorder_relinearize() { // Just recreate the whole BayesTree // TODO: allow for constrained ordering here // TODO: decouple relinearization and reordering to avoid - isam_.update(*factors_.linearize(newLinPoint), boost::none, + isam_.update(*factors_.linearize(newLinPoint), boost::none, boost::none, eliminationFunction_); // Update linearization point diff --git a/gtsam/hybrid/HybridNonlinearISAM.h b/gtsam/hybrid/HybridNonlinearISAM.h index b1998fb30..47aa81c55 100644 --- a/gtsam/hybrid/HybridNonlinearISAM.h +++ b/gtsam/hybrid/HybridNonlinearISAM.h @@ -118,7 +118,9 @@ class GTSAM_EXPORT HybridNonlinearISAM { /** Add new factors along with their initial linearization points */ void update(const HybridNonlinearFactorGraph& newFactors, - const Values& initialValues); + const Values& initialValues, + const boost::optional& maxNrLeaves = boost::none, + const boost::optional& ordering = boost::none); /** Relinearization and reordering of variables */ void reorder_relinearize(); From a96c3db29e58041b128f382050ae802fefea2c3e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 16 Sep 2022 10:48:08 -0400 Subject: [PATCH 066/118] minor fix --- gtsam/hybrid/GaussianMixture.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp index 6816dfbf6..3a654ddad 100644 --- a/gtsam/hybrid/GaussianMixture.cpp +++ b/gtsam/hybrid/GaussianMixture.cpp @@ -119,11 +119,12 @@ void GaussianMixture::print(const std::string &s, "", [&](Key k) { return formatter(k); }, [&](const GaussianConditional::shared_ptr &gf) -> std::string { RedirectCout rd; - if (gf && !gf->empty()) + if (gf && !gf->empty()) { gf->print("", formatter); - else - return {"nullptr"}; - return rd.str(); + return rd.str(); + } else { + return "nullptr"; + } }); } From 93528c3d4f8ae5c87a063a3232fb2894e7899504 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 16 Sep 2022 12:19:24 -0400 Subject: [PATCH 067/118] Only eliminate variables that are in newFactors --- gtsam/hybrid/HybridGaussianISAM.cpp | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianISAM.cpp b/gtsam/hybrid/HybridGaussianISAM.cpp index 6946775b9..c7811992e 100644 --- a/gtsam/hybrid/HybridGaussianISAM.cpp +++ b/gtsam/hybrid/HybridGaussianISAM.cpp @@ -62,23 +62,29 @@ void HybridGaussianISAM::updateInternal( for (const sharedClique& orphan : *orphans) factors += boost::make_shared >(orphan); + // Get all the discrete keys from the new factors KeySet allDiscrete; - for (auto& factor : factors) { + for (auto& factor : newFactors) { for (auto& k : factor->discreteKeys()) { allDiscrete.insert(k.first); } } + + // Create KeyVector with continuous keys followed by discrete keys. KeyVector newKeysDiscreteLast; + // Insert continuous keys first. for (auto& k : newFactorKeys) { if (!allDiscrete.exists(k)) { newKeysDiscreteLast.push_back(k); } } + // Insert discrete keys at the end std::copy(allDiscrete.begin(), allDiscrete.end(), std::back_inserter(newKeysDiscreteLast)); // Get an ordering where the new keys are eliminated last - const VariableIndex index(factors); + const VariableIndex index(newFactors); + Ordering elimination_ordering; if (ordering) { elimination_ordering = *ordering; @@ -89,10 +95,14 @@ void HybridGaussianISAM::updateInternal( true); } + GTSAM_PRINT(elimination_ordering); + std::cout << "\n\n\n\neliminateMultifrontal" << std::endl; + GTSAM_PRINT(factors); // eliminate all factors (top, added, orphans) into a new Bayes tree HybridBayesTree::shared_ptr bayesTree = factors.eliminateMultifrontal(elimination_ordering, function, index); + std::cout << "optionally prune" << std::endl; if (maxNrLeaves) { bayesTree->prune(*maxNrLeaves); } From aebcde99e2ae949a1bc20dac3d1fb5853991a576 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 16 Sep 2022 18:13:59 -0400 Subject: [PATCH 068/118] add push_back to HybridBayesNet --- gtsam/hybrid/HybridBayesNet.h | 2 ++ gtsam/hybrid/tests/testHybridBayesNet.cpp | 15 +++++++++++++++ 2 files changed, 17 insertions(+) diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h index e84103a50..f28224d37 100644 --- a/gtsam/hybrid/HybridBayesNet.h +++ b/gtsam/hybrid/HybridBayesNet.h @@ -73,6 +73,8 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { HybridConditional(boost::make_shared(key, table))); } + using Base::push_back; + /// Get a specific Gaussian mixture by index `i`. GaussianMixture::shared_ptr atMixture(size_t i) const; diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp index 0c15ee83d..cc2ab1759 100644 --- a/gtsam/hybrid/tests/testHybridBayesNet.cpp +++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp @@ -52,6 +52,21 @@ TEST(HybridBayesNet, Creation) { EXPECT(df.equals(expected)); } +/* ****************************************************************************/ +// Test adding a bayes net to another one. +TEST(HybridBayesNet, Add) { + HybridBayesNet bayesNet; + + bayesNet.add(Asia, "99/1"); + + DiscreteConditional expected(Asia, "99/1"); + + HybridBayesNet other; + other.push_back(bayesNet); + EXPECT(bayesNet.equals(other)); +} + + /* ****************************************************************************/ // Test choosing an assignment of conditionals TEST(HybridBayesNet, Choose) { From 9ef5c184ec379acb04c1bbd8619bda0e9ca25b5f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 17 Sep 2022 08:04:55 -0400 Subject: [PATCH 069/118] move renamed allDiscreteKeys and allContinuousKeys to HybridFactorGraph --- gtsam/hybrid/HybridFactorGraph.h | 22 ++++++++++++++++++++ gtsam/hybrid/HybridGaussianFactorGraph.cpp | 24 +--------------------- gtsam/hybrid/HybridGaussianFactorGraph.h | 6 ------ 3 files changed, 23 insertions(+), 29 deletions(-) diff --git a/gtsam/hybrid/HybridFactorGraph.h b/gtsam/hybrid/HybridFactorGraph.h index fc730f0c9..ea071a020 100644 --- a/gtsam/hybrid/HybridFactorGraph.h +++ b/gtsam/hybrid/HybridFactorGraph.h @@ -135,6 +135,28 @@ class HybridFactorGraph : public FactorGraph { push_hybrid(p); } } + + /// Get all the discrete keys in the factor graph. + const KeySet allDiscreteKeys() const { + KeySet discrete_keys; + for (auto& factor : factors_) { + for (const DiscreteKey& k : factor->discreteKeys()) { + discrete_keys.insert(k.first); + } + } + return discrete_keys; + } + + /// Get all the continuous keys in the factor graph. + const KeySet allContinuousKeys() const { + KeySet keys; + for (auto& factor : factors_) { + for (const Key& key : factor->continuousKeys()) { + keys.insert(key); + } + } + return keys; + } }; } // namespace gtsam diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 09b592bd6..c08e774f2 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -404,31 +404,9 @@ void HybridGaussianFactorGraph::add(DecisionTreeFactor::shared_ptr factor) { FactorGraph::add(boost::make_shared(factor)); } -/* ************************************************************************ */ -const KeySet HybridGaussianFactorGraph::getDiscreteKeys() const { - KeySet discrete_keys; - for (auto &factor : factors_) { - for (const DiscreteKey &k : factor->discreteKeys()) { - discrete_keys.insert(k.first); - } - } - return discrete_keys; -} - -/* ************************************************************************ */ -const KeySet HybridGaussianFactorGraph::getContinuousKeys() const { - KeySet keys; - for (auto &factor : factors_) { - for (const Key &key : factor->continuousKeys()) { - keys.insert(key); - } - } - return keys; -} - /* ************************************************************************ */ const Ordering HybridGaussianFactorGraph::getHybridOrdering() const { - KeySet discrete_keys = getDiscreteKeys(); + KeySet discrete_keys = allDiscreteKeys(); for (auto &factor : factors_) { for (const DiscreteKey &k : factor->discreteKeys()) { discrete_keys.insert(k.first); diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h index ad5cde09b..bd24cdeaa 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.h +++ b/gtsam/hybrid/HybridGaussianFactorGraph.h @@ -161,12 +161,6 @@ class GTSAM_EXPORT HybridGaussianFactorGraph } } - /// Get all the discrete keys in the factor graph. - const KeySet getDiscreteKeys() const; - - /// Get all the continuous keys in the factor graph. - const KeySet getContinuousKeys() const; - /** * @brief Return a Colamd constrained ordering where the discrete keys are * eliminated after the continuous keys. From 12db5dd947481bc1990dddae5f527ca4724be788 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 17 Sep 2022 08:05:07 -0400 Subject: [PATCH 070/118] undo changes --- gtsam/hybrid/HybridGaussianISAM.cpp | 10 +++------- 1 file changed, 3 insertions(+), 7 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianISAM.cpp b/gtsam/hybrid/HybridGaussianISAM.cpp index c7811992e..1a95c0c93 100644 --- a/gtsam/hybrid/HybridGaussianISAM.cpp +++ b/gtsam/hybrid/HybridGaussianISAM.cpp @@ -62,9 +62,9 @@ void HybridGaussianISAM::updateInternal( for (const sharedClique& orphan : *orphans) factors += boost::make_shared >(orphan); - // Get all the discrete keys from the new factors + // Get all the discrete keys from the factors KeySet allDiscrete; - for (auto& factor : newFactors) { + for (auto& factor : factors) { for (auto& k : factor->discreteKeys()) { allDiscrete.insert(k.first); } @@ -83,7 +83,7 @@ void HybridGaussianISAM::updateInternal( std::back_inserter(newKeysDiscreteLast)); // Get an ordering where the new keys are eliminated last - const VariableIndex index(newFactors); + const VariableIndex index(factors); Ordering elimination_ordering; if (ordering) { @@ -95,14 +95,10 @@ void HybridGaussianISAM::updateInternal( true); } - GTSAM_PRINT(elimination_ordering); - std::cout << "\n\n\n\neliminateMultifrontal" << std::endl; - GTSAM_PRINT(factors); // eliminate all factors (top, added, orphans) into a new Bayes tree HybridBayesTree::shared_ptr bayesTree = factors.eliminateMultifrontal(elimination_ordering, function, index); - std::cout << "optionally prune" << std::endl; if (maxNrLeaves) { bayesTree->prune(*maxNrLeaves); } From 2f8a0f82e0337e80dca1b7d9c608b1f7cbce792c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 19 Sep 2022 18:23:18 -0400 Subject: [PATCH 071/118] rename testHybridIncremental to testHybridGaussianISAM --- .../{testHybridIncremental.cpp => testHybridGaussianISAM.cpp} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename gtsam/hybrid/tests/{testHybridIncremental.cpp => testHybridGaussianISAM.cpp} (100%) diff --git a/gtsam/hybrid/tests/testHybridIncremental.cpp b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp similarity index 100% rename from gtsam/hybrid/tests/testHybridIncremental.cpp rename to gtsam/hybrid/tests/testHybridGaussianISAM.cpp From c2ca426acce0bf60eb14f66267b08b029abc28fb Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 20 Sep 2022 05:16:26 -0400 Subject: [PATCH 072/118] rename allDiscreteKeys and allContinuousKeys to discreteKeys and continuousKeys respectively --- gtsam/hybrid/HybridFactorGraph.h | 4 ++-- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 2 +- gtsam/hybrid/HybridGaussianISAM.cpp | 7 +------ 3 files changed, 4 insertions(+), 9 deletions(-) diff --git a/gtsam/hybrid/HybridFactorGraph.h b/gtsam/hybrid/HybridFactorGraph.h index ea071a020..05a17b000 100644 --- a/gtsam/hybrid/HybridFactorGraph.h +++ b/gtsam/hybrid/HybridFactorGraph.h @@ -137,7 +137,7 @@ class HybridFactorGraph : public FactorGraph { } /// Get all the discrete keys in the factor graph. - const KeySet allDiscreteKeys() const { + const KeySet discreteKeys() const { KeySet discrete_keys; for (auto& factor : factors_) { for (const DiscreteKey& k : factor->discreteKeys()) { @@ -148,7 +148,7 @@ class HybridFactorGraph : public FactorGraph { } /// Get all the continuous keys in the factor graph. - const KeySet allContinuousKeys() const { + const KeySet continuousKeys() const { KeySet keys; for (auto& factor : factors_) { for (const Key& key : factor->continuousKeys()) { diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index c08e774f2..ddb776ff4 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -406,7 +406,7 @@ void HybridGaussianFactorGraph::add(DecisionTreeFactor::shared_ptr factor) { /* ************************************************************************ */ const Ordering HybridGaussianFactorGraph::getHybridOrdering() const { - KeySet discrete_keys = allDiscreteKeys(); + KeySet discrete_keys = discreteKeys(); for (auto &factor : factors_) { for (const DiscreteKey &k : factor->discreteKeys()) { discrete_keys.insert(k.first); diff --git a/gtsam/hybrid/HybridGaussianISAM.cpp b/gtsam/hybrid/HybridGaussianISAM.cpp index 1a95c0c93..57e50104d 100644 --- a/gtsam/hybrid/HybridGaussianISAM.cpp +++ b/gtsam/hybrid/HybridGaussianISAM.cpp @@ -63,12 +63,7 @@ void HybridGaussianISAM::updateInternal( factors += boost::make_shared >(orphan); // Get all the discrete keys from the factors - KeySet allDiscrete; - for (auto& factor : factors) { - for (auto& k : factor->discreteKeys()) { - allDiscrete.insert(k.first); - } - } + KeySet allDiscrete = factors.discreteKeys(); // Create KeyVector with continuous keys followed by discrete keys. KeyVector newKeysDiscreteLast; From a2109a24379bf3cbdebb3d60396eb158d806c22c Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Mon, 26 Sep 2022 10:56:44 -0400 Subject: [PATCH 073/118] pass in IndexPair arguments --- python/gtsam/tests/test_DsfTrackGenerator.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/python/gtsam/tests/test_DsfTrackGenerator.py b/python/gtsam/tests/test_DsfTrackGenerator.py index b9ac22397..74a7fe5dc 100644 --- a/python/gtsam/tests/test_DsfTrackGenerator.py +++ b/python/gtsam/tests/test_DsfTrackGenerator.py @@ -28,8 +28,8 @@ class TestDsfTrackGenerator(GtsamTestCase): # For each image pair (i1,i2), we provide a (K,2) matrix of corresponding image indices (k1,k2). matches_dict = MatchIndicesMap() - matches_dict[(0, 1)] = np.array([[0, 0], [1, 1]]) - matches_dict[(1, 2)] = np.array([[2, 0], [1, 1]]) + matches_dict[gtsam.IndexPair(0, 1)] = np.array([[0, 0], [1, 1]]) + matches_dict[gtsam.IndexPair(1, 2)] = np.array([[2, 0], [1, 1]]) tracks = DsfTrackGenerator().generate_tracks_from_pairwise_matches(matches_dict, keypoints_list) assert len(tracks) == 3 From 2434dc8ab732da5dce37cb7e26c5387ee7d837b0 Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Mon, 26 Sep 2022 11:36:39 -0400 Subject: [PATCH 074/118] add to preamble/specialization --- gtsam/sfm/DsfTrackGenerator.h | 4 ++++ gtsam/sfm/sfm.i | 12 +++++++++++- python/CMakeLists.txt | 3 ++- python/gtsam/preamble/sfm.h | 3 ++- python/gtsam/specializations/sfm.h | 3 +++ python/gtsam/tests/test_DsfTrackGenerator.py | 6 +++--- 6 files changed, 25 insertions(+), 6 deletions(-) diff --git a/gtsam/sfm/DsfTrackGenerator.h b/gtsam/sfm/DsfTrackGenerator.h index 5ab87a7df..16dddcbaa 100644 --- a/gtsam/sfm/DsfTrackGenerator.h +++ b/gtsam/sfm/DsfTrackGenerator.h @@ -94,6 +94,10 @@ class SfmTrack2d { } }; +using SfmTrack2dVector = std::vector; + + + /** * @brief Generates point tracks from connected components in the keypoint matches graph. */ diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index b46b308eb..6c3f1e8eb 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -49,10 +49,20 @@ class SfmTrack2d bool hasUniqueCameras(); }; +class SfmTrack2dVector { + SfmTrack2dVector(); + SfmTrack2dVector(const gtsam::SfmTrack2dVector& other); + void push_back(const gtsam::SfmTrack2d& keypoints); + size_t size() const; + bool empty() const; + void clear(); + gtsam::SfmTrack2d at(const size_t& index) const; +}; + class DsfTrackGenerator { DsfTrackGenerator(); - std::vector generate_tracks_from_pairwise_matches( + const gtsam::SfmTrack2dVector generate_tracks_from_pairwise_matches( const gtsam::MatchIndicesMap& matches_dict, const gtsam::KeypointsList& keypoints_list); }; diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index ffb52ad11..e8cc1f10f 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -152,7 +152,8 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) gtsam::CameraSetCal3Fisheye gtsam::KeyPairDoubleMap gtsam::MatchIndicesMap - gtsam::KeypointsList) + gtsam::KeypointsList + gtsam::SfmTrack2dVector) pybind_wrap(${GTSAM_PYTHON_UNSTABLE_TARGET} # target diff --git a/python/gtsam/preamble/sfm.h b/python/gtsam/preamble/sfm.h index a51e963dd..fa4458c69 100644 --- a/python/gtsam/preamble/sfm.h +++ b/python/gtsam/preamble/sfm.h @@ -26,6 +26,7 @@ PYBIND11_MAKE_OPAQUE( std::vector>); PYBIND11_MAKE_OPAQUE( std::vector); - +PYBIND11_MAKE_OPAQUE( + std::vector); PYBIND11_MAKE_OPAQUE(gtsam::MatchIndicesMap); \ No newline at end of file diff --git a/python/gtsam/specializations/sfm.h b/python/gtsam/specializations/sfm.h index 297aee262..9eaf0f7fd 100644 --- a/python/gtsam/specializations/sfm.h +++ b/python/gtsam/specializations/sfm.h @@ -37,3 +37,6 @@ py::bind_vector< std::vector>>( m_, "SfmMeasurementVector" ); +py::bind_vector< + std::vector >( + m_, "SfmTrack2dVector"); \ No newline at end of file diff --git a/python/gtsam/tests/test_DsfTrackGenerator.py b/python/gtsam/tests/test_DsfTrackGenerator.py index 74a7fe5dc..41394f17a 100644 --- a/python/gtsam/tests/test_DsfTrackGenerator.py +++ b/python/gtsam/tests/test_DsfTrackGenerator.py @@ -8,7 +8,7 @@ import unittest import numpy as np import gtsam -from gtsam import DsfTrackGenerator, Keypoints, KeypointsList, MatchIndicesMap +from gtsam import DsfTrackGenerator, IndexPair, Keypoints, KeypointsList, MatchIndicesMap from gtsam.utils.test_case import GtsamTestCase @@ -28,8 +28,8 @@ class TestDsfTrackGenerator(GtsamTestCase): # For each image pair (i1,i2), we provide a (K,2) matrix of corresponding image indices (k1,k2). matches_dict = MatchIndicesMap() - matches_dict[gtsam.IndexPair(0, 1)] = np.array([[0, 0], [1, 1]]) - matches_dict[gtsam.IndexPair(1, 2)] = np.array([[2, 0], [1, 1]]) + matches_dict[IndexPair(0, 1)] = np.array([[0, 0], [1, 1]]) + matches_dict[IndexPair(1, 2)] = np.array([[2, 0], [1, 1]]) tracks = DsfTrackGenerator().generate_tracks_from_pairwise_matches(matches_dict, keypoints_list) assert len(tracks) == 3 From a3069a3fbc36828bd982128bebff7669fc297ef6 Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Mon, 26 Sep 2022 11:58:07 -0400 Subject: [PATCH 075/118] remove opaque on sfmtrack2d --- python/gtsam/preamble/sfm.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/python/gtsam/preamble/sfm.h b/python/gtsam/preamble/sfm.h index fa4458c69..fe49aadc1 100644 --- a/python/gtsam/preamble/sfm.h +++ b/python/gtsam/preamble/sfm.h @@ -26,7 +26,7 @@ PYBIND11_MAKE_OPAQUE( std::vector>); PYBIND11_MAKE_OPAQUE( std::vector); -PYBIND11_MAKE_OPAQUE( - std::vector); +// PYBIND11_MAKE_OPAQUE( +// std::vector); PYBIND11_MAKE_OPAQUE(gtsam::MatchIndicesMap); \ No newline at end of file From fa17c509107e2ee7a9bee36a3fe12a9a7b55b0e3 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Mon, 26 Sep 2022 18:41:59 -0400 Subject: [PATCH 076/118] Add SfmTrack2dVector to ignore list --- python/CMakeLists.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index e8cc1f10f..3266cf867 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -53,7 +53,8 @@ set(ignore gtsam::DiscreteKey gtsam::KeyPairDoubleMap gtsam::MatchIndicesMap - gtsam::KeypointsList) + gtsam::KeypointsList + gtsam::SfmTrack2dVector) set(interface_headers ${PROJECT_SOURCE_DIR}/gtsam/gtsam.i From 96aaff4bc6b42b824a584c3086311766f8a3daa3 Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Mon, 26 Sep 2022 20:42:03 -0400 Subject: [PATCH 077/118] wrap vector of NamedSfmMeasurement --- gtsam/sfm/DsfTrackGenerator.h | 2 +- gtsam/sfm/sfm.i | 12 +++++++++++- python/CMakeLists.txt | 6 ++++-- python/gtsam/preamble/sfm.h | 5 ++++- python/gtsam/specializations/sfm.h | 6 +++++- 5 files changed, 25 insertions(+), 6 deletions(-) diff --git a/gtsam/sfm/DsfTrackGenerator.h b/gtsam/sfm/DsfTrackGenerator.h index 16dddcbaa..9a0c350bd 100644 --- a/gtsam/sfm/DsfTrackGenerator.h +++ b/gtsam/sfm/DsfTrackGenerator.h @@ -95,7 +95,7 @@ class SfmTrack2d { }; using SfmTrack2dVector = std::vector; - +using NamedSfmMeasurementVector = std::vector; /** diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index 6c3f1e8eb..155a551bd 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -42,6 +42,16 @@ class NamedSfmMeasurement NamedSfmMeasurement(size_t i, gtsam::Point2 uv); }; +class NamedSfmMeasurementVector { + NamedSfmMeasurementVector(); + NamedSfmMeasurementVector(const gtsam::NamedSfmMeasurementVector& other); + void push_back(const gtsam::NamedSfmMeasurement& measurement); + size_t size() const; + bool empty() const; + void clear(); + gtsam::NamedSfmMeasurement at(const size_t& index) const; +}; + class SfmTrack2d { void addMeasurement(const gtsam::NamedSfmMeasurement &m); @@ -52,7 +62,7 @@ class SfmTrack2d class SfmTrack2dVector { SfmTrack2dVector(); SfmTrack2dVector(const gtsam::SfmTrack2dVector& other); - void push_back(const gtsam::SfmTrack2d& keypoints); + void push_back(const gtsam::SfmTrack2d& track); size_t size() const; bool empty() const; void clear(); diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 3266cf867..21f9f951d 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -54,7 +54,8 @@ set(ignore gtsam::KeyPairDoubleMap gtsam::MatchIndicesMap gtsam::KeypointsList - gtsam::SfmTrack2dVector) + gtsam::SfmTrack2dVector + gtsam::NamedSfmMeasurementVector) set(interface_headers ${PROJECT_SOURCE_DIR}/gtsam/gtsam.i @@ -154,7 +155,8 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) gtsam::KeyPairDoubleMap gtsam::MatchIndicesMap gtsam::KeypointsList - gtsam::SfmTrack2dVector) + gtsam::SfmTrack2dVector + gtsam::NamedSfmMeasurementVector) pybind_wrap(${GTSAM_PYTHON_UNSTABLE_TARGET} # target diff --git a/python/gtsam/preamble/sfm.h b/python/gtsam/preamble/sfm.h index fe49aadc1..429b794b8 100644 --- a/python/gtsam/preamble/sfm.h +++ b/python/gtsam/preamble/sfm.h @@ -29,4 +29,7 @@ PYBIND11_MAKE_OPAQUE( // PYBIND11_MAKE_OPAQUE( // std::vector); -PYBIND11_MAKE_OPAQUE(gtsam::MatchIndicesMap); \ No newline at end of file +PYBIND11_MAKE_OPAQUE(gtsam::MatchIndicesMap); + +PYBIND11_MAKE_OPAQUE( + std::vector); \ No newline at end of file diff --git a/python/gtsam/specializations/sfm.h b/python/gtsam/specializations/sfm.h index 9eaf0f7fd..1bbb93570 100644 --- a/python/gtsam/specializations/sfm.h +++ b/python/gtsam/specializations/sfm.h @@ -39,4 +39,8 @@ py::bind_vector< ); py::bind_vector< std::vector >( - m_, "SfmTrack2dVector"); \ No newline at end of file + m_, "SfmTrack2dVector"); + +py::bind_vector< + std::vector >( + m_, "NamedSfmMeasurementVector"); From 6eb4ada2fdcb9c4d2ade763514207d5bb724982b Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Tue, 27 Sep 2022 10:28:57 -0400 Subject: [PATCH 078/118] add more methods for accessing members, getting size --- gtsam/sfm/DsfTrackGenerator.h | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/gtsam/sfm/DsfTrackGenerator.h b/gtsam/sfm/DsfTrackGenerator.h index 9a0c350bd..b65a4fad6 100644 --- a/gtsam/sfm/DsfTrackGenerator.h +++ b/gtsam/sfm/DsfTrackGenerator.h @@ -73,11 +73,26 @@ class SfmTrack2d { std::vector measurements_; public: + // Default constructor. + SfmTrack2d() = default; + + // Constructor from measurements. + SfmTrack2d(std::vector &measurements) : measurements_(measurements) {} + + // Add a measurement to the track. void addMeasurement(const NamedSfmMeasurement &m) { measurements_.emplace_back(m); } + + /// The measurement at index `idx` + NamedSfmMeasurement measurement(size_t idx) const { return measurements_[idx]; } + + // Return all measurements in the track. std::vector measurements() {return measurements_; } + /// Total number of measurements in this track. + size_t numberMeasurements() const { return measurements_.size(); } + // @brief Validates the track by checking that no two measurements are from the same camera. // // returns boolean result of the validation. From cafab61fecb8fcdf14065c8de1fd8a340504b10c Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Tue, 27 Sep 2022 10:29:36 -0400 Subject: [PATCH 079/118] expand tests --- gtsam/sfm/sfm.i | 4 +++ python/gtsam/preamble/sfm.h | 5 ---- python/gtsam/tests/test_DsfTrackGenerator.py | 26 +++++++++++++++++++- 3 files changed, 29 insertions(+), 6 deletions(-) diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index 155a551bd..6fd380b3c 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -54,8 +54,12 @@ class NamedSfmMeasurementVector { class SfmTrack2d { + SfmTrack2d(); + SfmTrack2d(std::vector &measurements); + size_t numberMeasurements() const; void addMeasurement(const gtsam::NamedSfmMeasurement &m); std::vector measurements(); + gtsam::NamedSfmMeasurement measurement(size_t idx) const; bool hasUniqueCameras(); }; diff --git a/python/gtsam/preamble/sfm.h b/python/gtsam/preamble/sfm.h index 429b794b8..91c9fc418 100644 --- a/python/gtsam/preamble/sfm.h +++ b/python/gtsam/preamble/sfm.h @@ -17,7 +17,6 @@ PYBIND11_MAKE_OPAQUE( std::vector); - PYBIND11_MAKE_OPAQUE( std::vector); PYBIND11_MAKE_OPAQUE( @@ -26,10 +25,6 @@ PYBIND11_MAKE_OPAQUE( std::vector>); PYBIND11_MAKE_OPAQUE( std::vector); -// PYBIND11_MAKE_OPAQUE( -// std::vector); - PYBIND11_MAKE_OPAQUE(gtsam::MatchIndicesMap); - PYBIND11_MAKE_OPAQUE( std::vector); \ No newline at end of file diff --git a/python/gtsam/tests/test_DsfTrackGenerator.py b/python/gtsam/tests/test_DsfTrackGenerator.py index 41394f17a..3d4188284 100644 --- a/python/gtsam/tests/test_DsfTrackGenerator.py +++ b/python/gtsam/tests/test_DsfTrackGenerator.py @@ -8,7 +8,16 @@ import unittest import numpy as np import gtsam -from gtsam import DsfTrackGenerator, IndexPair, Keypoints, KeypointsList, MatchIndicesMap +from gtsam import ( + DsfTrackGenerator, + IndexPair, + Keypoints, + KeypointsList, + MatchIndicesMap, + NamedSfmMeasurement, + NamedSfmMeasurementVector, + SfmTrack2d, +) from gtsam.utils.test_case import GtsamTestCase @@ -39,6 +48,7 @@ class TestDsfTrackGenerator(GtsamTestCase): assert np.allclose(tracks[0].measurements()[1].uv, np.array([50.0, 60.0])) assert tracks[0].measurements()[0].i == 0 assert tracks[0].measurements()[1].i == 1 + assert tracks[0].numberMeasurements() == 2 # Verify track 1. assert np.allclose(tracks[1].measurements()[0].uv, np.array([30.0, 40.0])) @@ -47,9 +57,23 @@ class TestDsfTrackGenerator(GtsamTestCase): assert tracks[1].measurements()[0].i == 0 assert tracks[1].measurements()[1].i == 1 assert tracks[1].measurements()[2].i == 2 + assert tracks[1].numberMeasurements() == 3 # Verify track 2. assert np.allclose(tracks[2].measurements()[0].uv, np.array([90.0, 100.0])) assert np.allclose(tracks[2].measurements()[1].uv, np.array([110.0, 120.0])) assert tracks[2].measurements()[0].i == 1 assert tracks[2].measurements()[1].i == 2 + assert tracks[2].numberMeasurements() == 2 + + +class TestSfmTrack2d(GtsamTestCase): + """Tests for SfmTrack2d.""" + + def test_sfm_track_2d_constructor(self) -> None: + """ """ + measurements = NamedSfmMeasurementVector() + measurements.append(NamedSfmMeasurement(i=0, uv=np.array([10.0, 20.0]))) + track = SfmTrack2d(measurements=measurements) + track.measurement(0) + track.numberMeasurements() == 1 From 75b2599e829e4db84777c7e5763e74c0506605f7 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 3 Oct 2022 17:31:53 -0400 Subject: [PATCH 080/118] remove unnecessary comments --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index d8881cc2a..6f48f2176 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -285,11 +285,8 @@ hybridElimination(const HybridGaussianFactorGraph &factors, // If there are no more continuous parents, then we should create here a // DiscreteFactor, with the error for each discrete choice. - // frontalKeys.print(); - // separatorFactors.print("", GTDKeyFormatter, [](const GaussianFactor::shared_ptr &factor) { factor->print(); return "";}); - // std::cout << "=====================================" << std::endl; if (keysOfSeparator.empty()) { - VectorValues empty_values; //TODO(Varun) Shouldn't this be from the optimized leaf? + VectorValues empty_values; auto factorError = [&](const GaussianFactor::shared_ptr &factor) { if (!factor) return 0.0; // TODO(fan): does this make sense? return exp(-factor->error(empty_values)); From ad328756d2d50b1ec83a21c2b5a8734e3e7da393 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 3 Oct 2022 19:14:03 -0400 Subject: [PATCH 081/118] improved hybrid bayes net pruning --- gtsam/hybrid/HybridBayesNet.cpp | 28 +++++++++++++++++++++-- gtsam/hybrid/HybridBayesNet.h | 14 +++++++++--- gtsam/hybrid/tests/testHybridBayesNet.cpp | 19 ++++++++++++++- 3 files changed, 55 insertions(+), 6 deletions(-) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index bca50a902..788970790 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -31,8 +31,32 @@ static std::set DiscreteKeysAsSet(const DiscreteKeys &dkeys) { } /* ************************************************************************* */ -HybridBayesNet HybridBayesNet::prune( - const DecisionTreeFactor::shared_ptr &discreteFactor) const { +DecisionTreeFactor::shared_ptr HybridBayesNet::discreteConditionals() const { + AlgebraicDecisionTree decisionTree; + + // The canonical decision tree factor which will get the discrete conditionals + // added to it. + DecisionTreeFactor dtFactor; + + for (size_t i = 0; i < this->size(); i++) { + HybridConditional::shared_ptr conditional = this->at(i); + if (conditional->isDiscrete()) { + // Convert to a DecisionTreeFactor and add it to the main factor. + DecisionTreeFactor f(*conditional->asDiscreteConditional()); + dtFactor = dtFactor * f; + } + } + return boost::make_shared(dtFactor); +} + +/* ************************************************************************* */ +HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) const { + // Get the decision tree of only the discrete keys + auto discreteConditionals = this->discreteConditionals(); + const DecisionTreeFactor::shared_ptr discreteFactor = + boost::make_shared( + discreteConditionals->prune(maxNrLeaves)); + /* To Prune, we visitWith every leaf in the GaussianMixture. * For each leaf, using the assignment we can check the discrete decision tree * for 0.0 probability, then just set the leaf to a nullptr. diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h index f28224d37..b8234d70a 100644 --- a/gtsam/hybrid/HybridBayesNet.h +++ b/gtsam/hybrid/HybridBayesNet.h @@ -111,9 +111,17 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { */ VectorValues optimize(const DiscreteValues &assignment) const; - /// Prune the Hybrid Bayes Net given the discrete decision tree. - HybridBayesNet prune( - const DecisionTreeFactor::shared_ptr &discreteFactor) const; + protected: + /** + * @brief Get all the discrete conditionals as a decision tree factor. + * + * @return DecisionTreeFactor::shared_ptr + */ + DecisionTreeFactor::shared_ptr discreteConditionals() const; + + public: + /// Prune the Hybrid Bayes Net such that we have at most maxNrLeaves leaves. + HybridBayesNet prune(size_t maxNrLeaves) const; /// @} diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp index cc2ab1759..5885fdcdc 100644 --- a/gtsam/hybrid/tests/testHybridBayesNet.cpp +++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp @@ -66,7 +66,6 @@ TEST(HybridBayesNet, Add) { EXPECT(bayesNet.equals(other)); } - /* ****************************************************************************/ // Test choosing an assignment of conditionals TEST(HybridBayesNet, Choose) { @@ -184,6 +183,24 @@ TEST(HybridBayesNet, OptimizeMultifrontal) { EXPECT(assert_equal(expectedValues, delta.continuous(), 1e-5)); } +/* ****************************************************************************/ +// Test bayes net pruning +TEST(HybridBayesNet, Prune) { + Switching s(4); + + Ordering hybridOrdering = s.linearizedFactorGraph.getHybridOrdering(); + HybridBayesNet::shared_ptr hybridBayesNet = + s.linearizedFactorGraph.eliminateSequential(hybridOrdering); + + HybridValues delta = hybridBayesNet->optimize(); + + auto prunedBayesNet = hybridBayesNet->prune(2); + HybridValues pruned_delta = prunedBayesNet.optimize(); + + EXPECT(assert_equal(delta.discrete(), pruned_delta.discrete())); + EXPECT(assert_equal(delta.continuous(), pruned_delta.continuous())); +} + /* ****************************************************************************/ // Test HybridBayesNet serialization. TEST(HybridBayesNet, Serialization) { From d6d44fc3b422051bab6d0858cc6190fbb3121610 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 3 Oct 2022 19:14:13 -0400 Subject: [PATCH 082/118] minor cleanup --- gtsam/hybrid/HybridConditional.h | 2 -- gtsam/inference/BayesTree.h | 1 - 2 files changed, 3 deletions(-) diff --git a/gtsam/hybrid/HybridConditional.h b/gtsam/hybrid/HybridConditional.h index b43bb9945..93ce33bea 100644 --- a/gtsam/hybrid/HybridConditional.h +++ b/gtsam/hybrid/HybridConditional.h @@ -34,8 +34,6 @@ namespace gtsam { -class HybridGaussianFactorGraph; - /** * Hybrid Conditional Density * diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index 924a505a2..b4b07a357 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -33,7 +33,6 @@ namespace gtsam { // Forward declarations template class FactorGraph; template class EliminatableClusterTree; - class HybridBayesTreeClique; /* ************************************************************************* */ /** clique statistics */ From bc8c77c54d36e55732dc60e0349e1815b607aa15 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 4 Oct 2022 10:12:02 -0400 Subject: [PATCH 083/118] rename test file to correct form --- ...ianHybridFactorGraph.cpp => testHybridGaussianFactorGraph.cpp} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename gtsam/hybrid/tests/{testGaussianHybridFactorGraph.cpp => testHybridGaussianFactorGraph.cpp} (100%) diff --git a/gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp similarity index 100% rename from gtsam/hybrid/tests/testGaussianHybridFactorGraph.cpp rename to gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp From 8820bf272c268ae298d99d8e13fe81052af80fc2 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 4 Oct 2022 12:33:28 -0400 Subject: [PATCH 084/118] Add test to expose bug in elimination with gaussian conditionals --- .../tests/testHybridGaussianFactorGraph.cpp | 41 +++++++++++++++++++ 1 file changed, 41 insertions(+) diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp index 40da42412..d199d7611 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp @@ -500,6 +500,7 @@ TEST(HybridGaussianFactorGraph, SwitchingTwoVar) { } } +/* ************************************************************************* */ TEST(HybridGaussianFactorGraph, optimize) { HybridGaussianFactorGraph hfg; @@ -521,6 +522,46 @@ TEST(HybridGaussianFactorGraph, optimize) { EXPECT(assert_equal(hv.atDiscrete(C(1)), int(0))); } + +/* ************************************************************************* */ +// Test adding of gaussian conditional and re-elimination. +TEST(HybridGaussianFactorGraph, Conditionals) { + Switching switching(4); + HybridGaussianFactorGraph hfg; + + hfg.push_back(switching.linearizedFactorGraph.at(0)); // P(X1) + Ordering ordering; + ordering.push_back(X(1)); + HybridBayesNet::shared_ptr bayes_net = hfg.eliminateSequential(ordering); + + hfg.push_back(switching.linearizedFactorGraph.at(1)); // P(X1, X2 | M1) + hfg.push_back(*bayes_net); + hfg.push_back(switching.linearizedFactorGraph.at(2)); // P(X2, X3 | M2) + hfg.push_back(switching.linearizedFactorGraph.at(5)); // P(M1) + ordering.push_back(X(2)); + ordering.push_back(X(3)); + ordering.push_back(M(1)); + ordering.push_back(M(2)); + + bayes_net = hfg.eliminateSequential(ordering); + + HybridValues result = bayes_net->optimize(); + + Values expected_continuous; + expected_continuous.insert(X(1), 0); + expected_continuous.insert(X(2), 1); + expected_continuous.insert(X(3), 2); + expected_continuous.insert(X(4), 4); + Values result_continuous = + switching.linearizationPoint.retract(result.continuous()); + EXPECT(assert_equal(expected_continuous, result_continuous)); + + DiscreteValues expected_discrete; + expected_discrete[M(1)] = 1; + expected_discrete[M(2)] = 1; + EXPECT(assert_equal(expected_discrete, result.discrete())); +} + /* ************************************************************************* */ int main() { TestResult tr; From 9002b6829183fdebb1ce65eb61bb261956c64307 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 4 Oct 2022 12:33:37 -0400 Subject: [PATCH 085/118] fix the bug --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index ddb776ff4..0faf0e86e 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -96,8 +96,12 @@ GaussianMixtureFactor::Sum sumFrontals( } } else if (f->isContinuous()) { - deferredFactors.push_back( - boost::dynamic_pointer_cast(f)->inner()); + if (auto gf = boost::dynamic_pointer_cast(f)) { + deferredFactors.push_back(gf->inner()); + } + if (auto cg = boost::dynamic_pointer_cast(f)) { + deferredFactors.push_back(cg->asGaussian()); + } } else if (f->isDiscrete()) { // Don't do anything for discrete-only factors From 6238a1f9017017c7d21ca2708b4dab46d3d3af2b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 4 Oct 2022 12:34:53 -0400 Subject: [PATCH 086/118] more docs for Switching example --- gtsam/hybrid/tests/Switching.h | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/gtsam/hybrid/tests/Switching.h b/gtsam/hybrid/tests/Switching.h index 8bcb26c92..3ae8f0bb1 100644 --- a/gtsam/hybrid/tests/Switching.h +++ b/gtsam/hybrid/tests/Switching.h @@ -115,7 +115,6 @@ inline std::pair> makeBinaryOrdering( /* *************************************************************************** */ using MotionModel = BetweenFactor; -// using MotionMixture = MixtureFactor; // Test fixture with switching network. struct Switching { @@ -125,7 +124,13 @@ struct Switching { HybridGaussianFactorGraph linearizedFactorGraph; Values linearizationPoint; - /// Create with given number of time steps. + /** + * @brief Create with given number of time steps. + * + * @param K The total number of timesteps. + * @param between_sigma The stddev between poses. + * @param prior_sigma The stddev on priors (also used for measurements). + */ Switching(size_t K, double between_sigma = 1.0, double prior_sigma = 0.1) : K(K) { // Create DiscreteKeys for binary K modes, modes[0] will not be used. @@ -166,6 +171,8 @@ struct Switching { linearizationPoint.insert(X(k), static_cast(k)); } + // The ground truth is robot moving forward + // and one less than the linearization point linearizedFactorGraph = *nonlinearFactorGraph.linearize(linearizationPoint); } From ec3d6b36da7edc2718d7509b82e48c0746888ccd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Gonzalve?= Date: Wed, 5 Oct 2022 20:15:39 +0200 Subject: [PATCH 087/118] Use cannonical library name for eigen --- CMakeLists.txt | 2 -- cmake/HandleEigen.cmake | 26 +++++++++++++++++++++----- gtsam/CMakeLists.txt | 7 ++----- 3 files changed, 23 insertions(+), 12 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 74433f333..39d1e4307 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -101,8 +101,6 @@ if(GTSAM_BUILD_PYTHON OR GTSAM_INSTALL_MATLAB_TOOLBOX) # Copy matlab.h to the correct folder. configure_file(${PROJECT_SOURCE_DIR}/wrap/matlab.h ${PROJECT_BINARY_DIR}/wrap/matlab.h COPYONLY) - # Add the include directories so that matlab.h can be found - include_directories("${PROJECT_BINARY_DIR}" "${GTSAM_EIGEN_INCLUDE_FOR_BUILD}") add_subdirectory(wrap) list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/wrap/cmake") diff --git a/cmake/HandleEigen.cmake b/cmake/HandleEigen.cmake index c49eb4f8e..887ae1748 100644 --- a/cmake/HandleEigen.cmake +++ b/cmake/HandleEigen.cmake @@ -16,8 +16,14 @@ endif() if(GTSAM_USE_SYSTEM_EIGEN) find_package(Eigen3 REQUIRED) # need to find again as REQUIRED - # Use generic Eigen include paths e.g. - set(GTSAM_EIGEN_INCLUDE_FOR_INSTALL "${EIGEN3_INCLUDE_DIR}") + add_library(Eigen3::Eigen INTERFACE IMPORTED) + + set_target_properties(Eigen3::Eigen PROPERTIES + INTERFACE_INCLUDE_DIRECTORIES ${EIGEN3_INCLUDE_DIR} + ) + + # The actual include directory (for BUILD cmake target interface): + set(GTSAM_EIGEN_INCLUDE_FOR_BUILD "${EIGEN3_INCLUDE_DIR}/") # check if MKL is also enabled - can have one or the other, but not both! # Note: Eigen >= v3.2.5 includes our patches @@ -30,9 +36,6 @@ if(GTSAM_USE_SYSTEM_EIGEN) if(EIGEN_USE_MKL_ALL AND (EIGEN3_VERSION VERSION_EQUAL 3.3.4)) message(FATAL_ERROR "MKL does not work with Eigen 3.3.4 because of a bug in Eigen. See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=1527. Disable GTSAM_USE_SYSTEM_EIGEN to use GTSAM's copy of Eigen, disable GTSAM_WITH_EIGEN_MKL, or upgrade/patch your installation of Eigen.") endif() - - # The actual include directory (for BUILD cmake target interface): - set(GTSAM_EIGEN_INCLUDE_FOR_BUILD "${EIGEN3_INCLUDE_DIR}") else() # Use bundled Eigen include path. # Clear any variables set by FindEigen3 @@ -46,6 +49,19 @@ else() # The actual include directory (for BUILD cmake target interface): set(GTSAM_EIGEN_INCLUDE_FOR_BUILD "${GTSAM_SOURCE_DIR}/gtsam/3rdparty/Eigen/") + + add_library(gtsam_eigen3 INTERFACE) + + target_include_directories(gtsam_eigen3 INTERFACE + $ + $ + ) + add_library(Eigen3::Eigen ALIAS gtsam_eigen3) + + install(TARGETS gtsam_eigen3 EXPORT GTSAM-exports PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}) + + list(APPEND GTSAM_EXPORTED_TARGETS gtsam_eigen3) + set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE) endif() # Detect Eigen version: diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index 09f1ea806..d3408ee7f 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -117,12 +117,9 @@ set_target_properties(gtsam PROPERTIES VERSION ${gtsam_version} SOVERSION ${gtsam_soversion}) -# Append Eigen include path, set in top-level CMakeLists.txt to either # system-eigen, or GTSAM eigen path -target_include_directories(gtsam PUBLIC - $ - $ -) +target_link_libraries(gtsam PUBLIC Eigen3::Eigen) + # MKL include dir: if (GTSAM_USE_EIGEN_MKL) target_include_directories(gtsam PUBLIC ${MKL_INCLUDE_DIR}) From 4d690efdebead9dd9449feecb4aa11318bf5fba6 Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Wed, 5 Oct 2022 22:45:03 -0400 Subject: [PATCH 088/118] add docimentation for Keypoints class --- gtsam/sfm/DsfTrackGenerator.h | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/gtsam/sfm/DsfTrackGenerator.h b/gtsam/sfm/DsfTrackGenerator.h index b65a4fad6..05ad61390 100644 --- a/gtsam/sfm/DsfTrackGenerator.h +++ b/gtsam/sfm/DsfTrackGenerator.h @@ -37,17 +37,27 @@ typedef Eigen::MatrixX2i CorrespondenceIndices; // N x 2 array using KeypointCoordinates = Eigen::MatrixX2d; +// Output of detections in an image. +// Coordinate system convention: +// 1. The x coordinate denotes the horizontal direction (+ve direction towards the right). +// 2. The y coordinate denotes the vertical direction (+ve direction downwards). +// 3. Origin is at the top left corner of the image. struct Keypoints { + + // The (x, y) coordinates of the features, of shape Nx2. KeypointCoordinates coordinates; - // typedef'd for Eigen::VectorXd + + // Optional scale of the detections, of shape N. + // Note: gtsam::Vector is typedef'd for Eigen::VectorXd. boost::optional scales; + + /// Optional confidences/responses for each detection, of shape N. boost::optional responses; Keypoints(const gtsam::KeypointCoordinates& coordinates): coordinates(coordinates) {}; // boost::none }; using KeypointsList = std::vector; -using KeypointsVector = std::vector; // TODO(johnwlambert): prefer KeypointsSet? // Mapping from each image pair to (N,2) array representing indices of matching keypoints. using MatchIndicesMap = std::map; From 6bd16d995e15e027d58eecbc9d150a6ee2a62401 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 7 Oct 2022 15:29:20 -0400 Subject: [PATCH 089/118] move GTDKeyFormatter to types.h --- gtsam/base/types.cpp | 46 ++++++++++++++++++++++ gtsam/base/types.h | 2 + gtsam/hybrid/HybridGaussianFactorGraph.cpp | 46 ---------------------- 3 files changed, 48 insertions(+), 46 deletions(-) diff --git a/gtsam/base/types.cpp b/gtsam/base/types.cpp index edc449b12..f96e037c1 100644 --- a/gtsam/base/types.cpp +++ b/gtsam/base/types.cpp @@ -64,4 +64,50 @@ std::string demangle(const char* name) { return demangled_name; } +std::string GTDKeyFormatter(const Key &key) { + constexpr size_t kMax_uchar_ = std::numeric_limits::max(); + constexpr size_t key_bits = sizeof(gtsam::Key) * 8; + constexpr size_t ch1_bits = sizeof(uint8_t) * 8; + constexpr size_t ch2_bits = sizeof(uint8_t) * 8; + constexpr size_t link_bits = sizeof(uint8_t) * 8; + constexpr size_t joint_bits = sizeof(uint8_t) * 8; + constexpr size_t time_bits = + key_bits - ch1_bits - ch2_bits - link_bits - joint_bits; + + constexpr gtsam::Key ch1_mask = gtsam::Key(kMax_uchar_) + << (key_bits - ch1_bits); + constexpr gtsam::Key ch2_mask = gtsam::Key(kMax_uchar_) + << (key_bits - ch1_bits - ch2_bits); + constexpr gtsam::Key link_mask = gtsam::Key(kMax_uchar_) + << (time_bits + joint_bits); + constexpr gtsam::Key joint_mask = gtsam::Key(kMax_uchar_) << time_bits; + constexpr gtsam::Key time_mask = + ~(ch1_mask | ch2_mask | link_mask | joint_mask); + + uint8_t c1_, c2_, link_idx_, joint_idx_; + uint64_t t_; + + c1_ = (uint8_t)((key & ch1_mask) >> (key_bits - ch1_bits)); + c2_ = (uint8_t)((key & ch2_mask) >> (key_bits - ch1_bits - ch2_bits)); + link_idx_ = (uint8_t)((key & link_mask) >> (time_bits + joint_bits)); + joint_idx_ = (uint8_t)((key & joint_mask) >> time_bits); + t_ = key & time_mask; + + std::string s = ""; + if (c1_ != 0) { + s += c1_; + } + if (c2_ != 0) { + s += c2_; + } + if (link_idx_ != kMax_uchar_) { + s += "[" + std::to_string((int)(link_idx_)) + "]"; + } + if (joint_idx_ != kMax_uchar_) { + s += "(" + std::to_string((int)(joint_idx_)) + ")"; + } + s += std::to_string(t_); + return s; +} + } /* namespace gtsam */ diff --git a/gtsam/base/types.h b/gtsam/base/types.h index a0d24f1a6..dcb389c9b 100644 --- a/gtsam/base/types.h +++ b/gtsam/base/types.h @@ -74,6 +74,8 @@ namespace gtsam { /// The index type for Eigen objects typedef ptrdiff_t DenseIndex; + std::string GTDKeyFormatter(const Key &key); + /* ************************************************************************* */ /** * Helper class that uses templates to select between two types based on diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 286129f8b..041603fbd 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -55,52 +55,6 @@ namespace gtsam { template class EliminateableFactorGraph; -std::string GTDKeyFormatter(const Key &key) { - constexpr size_t kMax_uchar_ = std::numeric_limits::max(); - constexpr size_t key_bits = sizeof(gtsam::Key) * 8; - constexpr size_t ch1_bits = sizeof(uint8_t) * 8; - constexpr size_t ch2_bits = sizeof(uint8_t) * 8; - constexpr size_t link_bits = sizeof(uint8_t) * 8; - constexpr size_t joint_bits = sizeof(uint8_t) * 8; - constexpr size_t time_bits = - key_bits - ch1_bits - ch2_bits - link_bits - joint_bits; - - constexpr gtsam::Key ch1_mask = gtsam::Key(kMax_uchar_) - << (key_bits - ch1_bits); - constexpr gtsam::Key ch2_mask = gtsam::Key(kMax_uchar_) - << (key_bits - ch1_bits - ch2_bits); - constexpr gtsam::Key link_mask = gtsam::Key(kMax_uchar_) - << (time_bits + joint_bits); - constexpr gtsam::Key joint_mask = gtsam::Key(kMax_uchar_) << time_bits; - constexpr gtsam::Key time_mask = - ~(ch1_mask | ch2_mask | link_mask | joint_mask); - - uint8_t c1_, c2_, link_idx_, joint_idx_; - uint64_t t_; - - c1_ = (uint8_t)((key & ch1_mask) >> (key_bits - ch1_bits)); - c2_ = (uint8_t)((key & ch2_mask) >> (key_bits - ch1_bits - ch2_bits)); - link_idx_ = (uint8_t)((key & link_mask) >> (time_bits + joint_bits)); - joint_idx_ = (uint8_t)((key & joint_mask) >> time_bits); - t_ = key & time_mask; - - std::string s = ""; - if (c1_ != 0) { - s += c1_; - } - if (c2_ != 0) { - s += c2_; - } - if (link_idx_ != kMax_uchar_) { - s += "[" + std::to_string((int)(link_idx_)) + "]"; - } - if (joint_idx_ != kMax_uchar_) { - s += "(" + std::to_string((int)(joint_idx_)) + ")"; - } - s += std::to_string(t_); - return s; -} - /* ************************************************************************ */ static GaussianMixtureFactor::Sum &addGaussian( GaussianMixtureFactor::Sum &sum, const GaussianFactor::shared_ptr &factor) { From 774cfd25b880e1deef5696c5b0ac1b4f2e548cf0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Gonzalve?= Date: Fri, 7 Oct 2022 18:52:10 +0200 Subject: [PATCH 090/118] Use Eigen Config file for cmake detection Since Eigen 3.3.0, a Config.cmake file is provided, thus no need to rely on a custom one. Moreover, the FindEigen3.cmake used in gtsam was erroneously forcing an include directory when using system version of eigen. This fixes bug #1297 --- cmake/Config.cmake.in | 4 ++ cmake/FindEigen3.cmake | 81 ----------------------------------------- cmake/HandleEigen.cmake | 16 ++++---- 3 files changed, 11 insertions(+), 90 deletions(-) delete mode 100644 cmake/FindEigen3.cmake diff --git a/cmake/Config.cmake.in b/cmake/Config.cmake.in index 89627a172..cc2a7df8f 100644 --- a/cmake/Config.cmake.in +++ b/cmake/Config.cmake.in @@ -21,6 +21,10 @@ else() find_dependency(Boost @BOOST_FIND_MINIMUM_VERSION@ COMPONENTS @BOOST_FIND_MINIMUM_COMPONENTS@) endif() +if(@GTSAM_USE_SYSTEM_EIGEN@) +find_dependency(Eigen3 REQUIRED) +endif() + # Load exports include(${OUR_CMAKE_DIR}/@PACKAGE_NAME@-exports.cmake) diff --git a/cmake/FindEigen3.cmake b/cmake/FindEigen3.cmake deleted file mode 100644 index 9c546a05d..000000000 --- a/cmake/FindEigen3.cmake +++ /dev/null @@ -1,81 +0,0 @@ -# - Try to find Eigen3 lib -# -# This module supports requiring a minimum version, e.g. you can do -# find_package(Eigen3 3.1.2) -# to require version 3.1.2 or newer of Eigen3. -# -# Once done this will define -# -# EIGEN3_FOUND - system has eigen lib with correct version -# EIGEN3_INCLUDE_DIR - the eigen include directory -# EIGEN3_VERSION - eigen version - -# Copyright (c) 2006, 2007 Montel Laurent, -# Copyright (c) 2008, 2009 Gael Guennebaud, -# Copyright (c) 2009 Benoit Jacob -# Redistribution and use is allowed according to the terms of the 2-clause BSD license. - -if(NOT Eigen3_FIND_VERSION) - if(NOT Eigen3_FIND_VERSION_MAJOR) - set(Eigen3_FIND_VERSION_MAJOR 2) - endif(NOT Eigen3_FIND_VERSION_MAJOR) - if(NOT Eigen3_FIND_VERSION_MINOR) - set(Eigen3_FIND_VERSION_MINOR 91) - endif(NOT Eigen3_FIND_VERSION_MINOR) - if(NOT Eigen3_FIND_VERSION_PATCH) - set(Eigen3_FIND_VERSION_PATCH 0) - endif(NOT Eigen3_FIND_VERSION_PATCH) - - set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") -endif(NOT Eigen3_FIND_VERSION) - -macro(_eigen3_check_version) - file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) - - string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") - set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") - string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") - set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") - string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") - set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") - - set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) - if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) - set(EIGEN3_VERSION_OK FALSE) - else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) - set(EIGEN3_VERSION_OK TRUE) - endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) - - if(NOT EIGEN3_VERSION_OK) - - message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " - "but at least version ${Eigen3_FIND_VERSION} is required") - endif(NOT EIGEN3_VERSION_OK) -endmacro(_eigen3_check_version) - -if (EIGEN3_INCLUDE_DIR) - - # in cache already - _eigen3_check_version() - set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) - -else (EIGEN3_INCLUDE_DIR) - - find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library - PATHS - ${CMAKE_INSTALL_PREFIX}/include - ${KDE4_INCLUDE_DIR} - PATH_SUFFIXES eigen3 eigen - ) - - if(EIGEN3_INCLUDE_DIR) - _eigen3_check_version() - endif(EIGEN3_INCLUDE_DIR) - - include(FindPackageHandleStandardArgs) - find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) - - mark_as_advanced(EIGEN3_INCLUDE_DIR) - -endif(EIGEN3_INCLUDE_DIR) - diff --git a/cmake/HandleEigen.cmake b/cmake/HandleEigen.cmake index 887ae1748..48941b85b 100644 --- a/cmake/HandleEigen.cmake +++ b/cmake/HandleEigen.cmake @@ -1,7 +1,7 @@ ############################################################################### # Option for using system Eigen or GTSAM-bundled Eigen # Default: Use system's Eigen if found automatically: -find_package(Eigen3 QUIET) +find_package(Eigen3 CONFIG QUIET) set(USE_SYSTEM_EIGEN_INITIAL_VALUE ${Eigen3_FOUND}) option(GTSAM_USE_SYSTEM_EIGEN "Find and use system-installed Eigen. If 'off', use the one bundled with GTSAM" ${USE_SYSTEM_EIGEN_INITIAL_VALUE}) unset(USE_SYSTEM_EIGEN_INITIAL_VALUE) @@ -14,16 +14,14 @@ endif() # Switch for using system Eigen or GTSAM-bundled Eigen if(GTSAM_USE_SYSTEM_EIGEN) - find_package(Eigen3 REQUIRED) # need to find again as REQUIRED - - add_library(Eigen3::Eigen INTERFACE IMPORTED) - - set_target_properties(Eigen3::Eigen PROPERTIES - INTERFACE_INCLUDE_DIRECTORIES ${EIGEN3_INCLUDE_DIR} - ) + # Since Eigen 3.3.0 a Eigen3Config.cmake is available so use it. + find_package(Eigen3 CONFIG REQUIRED) # need to find again as REQUIRED # The actual include directory (for BUILD cmake target interface): - set(GTSAM_EIGEN_INCLUDE_FOR_BUILD "${EIGEN3_INCLUDE_DIR}/") + # Note: EIGEN3_INCLUDE_DIR points to some random location on some eigen + # versions. So here I use the target itself to get the proper include + # directory (it is generated by cmake, thus has the correct path) + get_target_property(GTSAM_EIGEN_INCLUDE_FOR_BUILD Eigen3::Eigen INTERFACE_INCLUDE_DIRECTORIES) # check if MKL is also enabled - can have one or the other, but not both! # Note: Eigen >= v3.2.5 includes our patches From 9c77c661dcb496aba70a3962ca2cc697b387ff71 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 8 Oct 2022 13:56:46 -0400 Subject: [PATCH 091/118] fix tests --- gtsam/hybrid/tests/testHybridEstimation.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp index 683617a19..5d47a22dd 100644 --- a/gtsam/hybrid/tests/testHybridEstimation.cpp +++ b/gtsam/hybrid/tests/testHybridEstimation.cpp @@ -88,7 +88,7 @@ class Robot { } linearizedFactorGraph_ = - nonlinearFactorGraph_.linearize(linearizationPoint_); + *nonlinearFactorGraph_.linearize(linearizationPoint_); } void print() const { @@ -225,7 +225,7 @@ class SingleLeg { } linearizedFactorGraph_ = - nonlinearFactorGraph_.linearize(linearizationPoint_); + *nonlinearFactorGraph_.linearize(linearizationPoint_); } void print() const { From 1a17a8117399286977777ca618d9bd39b15c4eb9 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 9 Oct 2022 04:25:45 -0400 Subject: [PATCH 092/118] formatting --- gtsam/hybrid/HybridFactor.cpp | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/gtsam/hybrid/HybridFactor.cpp b/gtsam/hybrid/HybridFactor.cpp index e9d64b283..1216fd922 100644 --- a/gtsam/hybrid/HybridFactor.cpp +++ b/gtsam/hybrid/HybridFactor.cpp @@ -50,9 +50,7 @@ DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1, /* ************************************************************************ */ HybridFactor::HybridFactor(const KeyVector &keys) - : Base(keys), - isContinuous_(true), - continuousKeys_(keys) {} + : Base(keys), isContinuous_(true), continuousKeys_(keys) {} /* ************************************************************************ */ HybridFactor::HybridFactor(const KeyVector &continuousKeys, @@ -101,7 +99,6 @@ void HybridFactor::print(const std::string &s, if (d < discreteKeys_.size() - 1) { std::cout << " "; } - } std::cout << "]"; } From 055df81dd37c28aabe7d4bab8403847eba586fd3 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 9 Oct 2022 04:26:34 -0400 Subject: [PATCH 093/118] apply keyformatter to Gaussian iSAM in HybridNonlinearISAM --- gtsam/hybrid/HybridNonlinearISAM.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/hybrid/HybridNonlinearISAM.cpp b/gtsam/hybrid/HybridNonlinearISAM.cpp index d05e081dd..57e0daf8d 100644 --- a/gtsam/hybrid/HybridNonlinearISAM.cpp +++ b/gtsam/hybrid/HybridNonlinearISAM.cpp @@ -99,7 +99,7 @@ void HybridNonlinearISAM::print(const string& s, const KeyFormatter& keyFormatter) const { cout << s << "ReorderInterval: " << reorderInterval_ << " Current Count: " << reorderCounter_ << endl; - isam_.print("HybridGaussianISAM:\n"); + isam_.print("HybridGaussianISAM:\n", keyFormatter); linPoint_.print("Linearization Point:\n", keyFormatter); factors_.print("Nonlinear Graph:\n", keyFormatter); } From 3e151846ca30d8249d72530669bca03d0be3f731 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 10 Oct 2022 14:55:02 -0400 Subject: [PATCH 094/118] slight improvement to GaussianMixtureFactor print --- gtsam/hybrid/GaussianMixtureFactor.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/gtsam/hybrid/GaussianMixtureFactor.cpp b/gtsam/hybrid/GaussianMixtureFactor.cpp index 9b5be188a..181b1e6a5 100644 --- a/gtsam/hybrid/GaussianMixtureFactor.cpp +++ b/gtsam/hybrid/GaussianMixtureFactor.cpp @@ -57,11 +57,12 @@ void GaussianMixtureFactor::print(const std::string &s, [&](const GaussianFactor::shared_ptr &gf) -> std::string { RedirectCout rd; std::cout << ":\n"; - if (gf) + if (gf && !gf->empty()) { gf->print("", formatter); - else - return {"nullptr"}; - return rd.str(); + return rd.str(); + } else { + return "nullptr"; + } }); std::cout << "}" << std::endl; } From a00bcbcac92d45a5dcc06d7c74d7f25ef9feb32d Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 10 Oct 2022 16:03:36 -0400 Subject: [PATCH 095/118] PrunerFunc helper function --- gtsam/hybrid/HybridBayesNet.cpp | 65 +++++++++++++++++++++------------ 1 file changed, 42 insertions(+), 23 deletions(-) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index 788970790..163e77e47 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -49,6 +49,38 @@ DecisionTreeFactor::shared_ptr HybridBayesNet::discreteConditionals() const { return boost::make_shared(dtFactor); } +/** + * @brief Helper function to get the pruner functional. + * + * @param probDecisionTree The probability decision tree of only discrete keys. + * @param discreteFactorKeySet Set of DiscreteKeys in probDecisionTree. + * Pre-computed for efficiency. + * @param gaussianMixtureKeySet Set of DiscreteKeys in the GaussianMixture. + * @return std::function &, const GaussianConditional::shared_ptr &)> + */ +std::function &, const GaussianConditional::shared_ptr &)> +PrunerFunc(const DecisionTreeFactor::shared_ptr &probDecisionTree, + const std::set &discreteFactorKeySet, + const std::set &gaussianMixtureKeySet) { + auto pruner = [&](const Assignment &choices, + const GaussianConditional::shared_ptr &conditional) + -> GaussianConditional::shared_ptr { + // typecast so we can use this to get probability value + DiscreteValues values(choices); + + if ((*probDecisionTree)(values) == 0.0) { + // empty aka null pointer + boost::shared_ptr null; + return null; + } else { + return conditional; + } + }; + return pruner; +} + /* ************************************************************************* */ HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) const { // Get the decision tree of only the discrete keys @@ -57,6 +89,8 @@ HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) const { boost::make_shared( discreteConditionals->prune(maxNrLeaves)); + auto discreteFactorKeySet = DiscreteKeysAsSet(discreteFactor->discreteKeys()); + /* To Prune, we visitWith every leaf in the GaussianMixture. * For each leaf, using the assignment we can check the discrete decision tree * for 0.0 probability, then just set the leaf to a nullptr. @@ -66,23 +100,6 @@ HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) const { HybridBayesNet prunedBayesNetFragment; - // Functional which loops over all assignments and create a set of - // GaussianConditionals - auto pruner = [&](const Assignment &choices, - const GaussianConditional::shared_ptr &conditional) - -> GaussianConditional::shared_ptr { - // typecast so we can use this to get probability value - DiscreteValues values(choices); - - if ((*discreteFactor)(values) == 0.0) { - // empty aka null pointer - boost::shared_ptr null; - return null; - } else { - return conditional; - } - }; - // Go through all the conditionals in the // Bayes Net and prune them as per discreteFactor. for (size_t i = 0; i < this->size(); i++) { @@ -92,17 +109,19 @@ HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) const { boost::dynamic_pointer_cast(conditional->inner()); if (gaussianMixture) { - // We may have mixtures with less discrete keys than discreteFactor so we - // skip those since the label assignment does not exist. + // We may have mixtures with less discrete keys than discreteFactor so + // we skip those since the label assignment does not exist. auto gmKeySet = DiscreteKeysAsSet(gaussianMixture->discreteKeys()); - auto dfKeySet = DiscreteKeysAsSet(discreteFactor->discreteKeys()); - if (gmKeySet != dfKeySet) { + if (gmKeySet != discreteFactorKeySet) { // Add the gaussianMixture which doesn't have to be pruned. prunedBayesNetFragment.push_back( boost::make_shared(gaussianMixture)); continue; } + // Get the pruner function. + auto pruner = PrunerFunc(discreteFactor, discreteFactorKeySet, gmKeySet); + // Run the pruning to get a new, pruned tree GaussianMixture::Conditionals prunedTree = gaussianMixture->conditionals().apply(pruner); @@ -173,7 +192,7 @@ GaussianBayesNet HybridBayesNet::choose( return gbn; } -/* *******************************************************************************/ +/* ************************************************************************* */ HybridValues HybridBayesNet::optimize() const { // Solve for the MPE DiscreteBayesNet discrete_bn; @@ -190,7 +209,7 @@ HybridValues HybridBayesNet::optimize() const { return HybridValues(mpe, gbn.optimize()); } -/* *******************************************************************************/ +/* ************************************************************************* */ VectorValues HybridBayesNet::optimize(const DiscreteValues &assignment) const { GaussianBayesNet gbn = this->choose(assignment); return gbn.optimize(); From 2c8fe2584291a77557e1396306efa1210bc4f3d3 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 10 Oct 2022 19:38:10 -0400 Subject: [PATCH 096/118] enumerate missing discrete keys so we can prune all gaussian mixtures --- gtsam/hybrid/HybridBayesNet.cpp | 41 ++++++++++++++++++++++++--------- 1 file changed, 30 insertions(+), 11 deletions(-) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index 163e77e47..96a6dfd63 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -70,12 +70,37 @@ PrunerFunc(const DecisionTreeFactor::shared_ptr &probDecisionTree, // typecast so we can use this to get probability value DiscreteValues values(choices); - if ((*probDecisionTree)(values) == 0.0) { - // empty aka null pointer - boost::shared_ptr null; - return null; + // Case where the gaussian mixture has the same + // discrete keys as the decision tree. + if (gaussianMixtureKeySet == discreteFactorKeySet) { + if ((*probDecisionTree)(values) == 0.0) { + // empty aka null pointer + boost::shared_ptr null; + return null; + } else { + return conditional; + } } else { - return conditional; + std::vector set_diff; + std::set_difference( + discreteFactorKeySet.begin(), discreteFactorKeySet.end(), + gaussianMixtureKeySet.begin(), gaussianMixtureKeySet.end(), + std::back_inserter(set_diff)); + const std::vector assignments = + DiscreteValues::CartesianProduct(set_diff); + for (const DiscreteValues &assignment : assignments) { + DiscreteValues augmented_values(values); + augmented_values.insert(assignment.begin(), assignment.end()); + + // If any one of the sub-branches are non-zero, + // we need this conditional. + if ((*probDecisionTree)(augmented_values) > 0.0) { + return conditional; + } + } + // If we are here, it means that all the sub-branches are 0, + // so we prune. + return nullptr; } }; return pruner; @@ -112,12 +137,6 @@ HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) const { // We may have mixtures with less discrete keys than discreteFactor so // we skip those since the label assignment does not exist. auto gmKeySet = DiscreteKeysAsSet(gaussianMixture->discreteKeys()); - if (gmKeySet != discreteFactorKeySet) { - // Add the gaussianMixture which doesn't have to be pruned. - prunedBayesNetFragment.push_back( - boost::make_shared(gaussianMixture)); - continue; - } // Get the pruner function. auto pruner = PrunerFunc(discreteFactor, discreteFactorKeySet, gmKeySet); From c15cfb606849dcbdb089656da5871118d6f73fdf Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 11 Oct 2022 12:10:02 -0400 Subject: [PATCH 097/118] add PrunerFunc to GaussianMixture --- gtsam/hybrid/GaussianMixture.cpp | 78 +++++++++++++++++++++++++++----- 1 file changed, 67 insertions(+), 11 deletions(-) diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp index 3a654ddad..064905d6b 100644 --- a/gtsam/hybrid/GaussianMixture.cpp +++ b/gtsam/hybrid/GaussianMixture.cpp @@ -122,31 +122,87 @@ void GaussianMixture::print(const std::string &s, if (gf && !gf->empty()) { gf->print("", formatter); return rd.str(); + // return "Node()"; } else { return "nullptr"; } }); } -/* *******************************************************************************/ -void GaussianMixture::prune(const DecisionTreeFactor &decisionTree) { - // Functional which loops over all assignments and create a set of - // GaussianConditionals - auto pruner = [&decisionTree]( - const Assignment &choices, +/* ************************************************************************* */ +/// Return the DiscreteKey vector as a set. +static std::set DiscreteKeysAsSet(const DiscreteKeys &dkeys) { + std::set s; + s.insert(dkeys.begin(), dkeys.end()); + return s; +} + +/* ************************************************************************* */ +/** + * @brief Helper function to get the pruner functional. + * + * @param decisionTree The probability decision tree of only discrete keys. + * @param decisionTreeKeySet Set of DiscreteKeys in decisionTree. + * Pre-computed for efficiency. + * @param gaussianMixtureKeySet Set of DiscreteKeys in the GaussianMixture. + * @return std::function &, const GaussianConditional::shared_ptr &)> + */ +std::function &, const GaussianConditional::shared_ptr &)> +PrunerFunc(const DecisionTreeFactor &decisionTree, + const std::set &decisionTreeKeySet, + const std::set &gaussianMixtureKeySet) { + auto pruner = [&](const Assignment &choices, const GaussianConditional::shared_ptr &conditional) -> GaussianConditional::shared_ptr { // typecast so we can use this to get probability value DiscreteValues values(choices); - if (decisionTree(values) == 0.0) { - // empty aka null pointer - boost::shared_ptr null; - return null; + // Case where the gaussian mixture has the same + // discrete keys as the decision tree. + if (gaussianMixtureKeySet == decisionTreeKeySet) { + if (decisionTree(values) == 0.0) { + // empty aka null pointer + boost::shared_ptr null; + return null; + } else { + return conditional; + } } else { - return conditional; + std::vector set_diff; + std::set_difference(decisionTreeKeySet.begin(), decisionTreeKeySet.end(), + gaussianMixtureKeySet.begin(), + gaussianMixtureKeySet.end(), + std::back_inserter(set_diff)); + + const std::vector assignments = + DiscreteValues::CartesianProduct(set_diff); + for (const DiscreteValues &assignment : assignments) { + DiscreteValues augmented_values(values); + augmented_values.insert(assignment.begin(), assignment.end()); + + // If any one of the sub-branches are non-zero, + // we need this conditional. + if (decisionTree(augmented_values) > 0.0) { + return conditional; + } + } + // If we are here, it means that all the sub-branches are 0, + // so we prune. + return nullptr; } }; + return pruner; +} + +/* *******************************************************************************/ +void GaussianMixture::prune(const DecisionTreeFactor &decisionTree) { + auto decisionTreeKeySet = DiscreteKeysAsSet(decisionTree.discreteKeys()); + auto gmKeySet = DiscreteKeysAsSet(this->discreteKeys()); + // Functional which loops over all assignments and create a set of + // GaussianConditionals + auto pruner = PrunerFunc(decisionTree, decisionTreeKeySet, gmKeySet); auto pruned_conditionals = conditionals_.apply(pruner); conditionals_.root_ = pruned_conditionals.root_; From 2225ecf44277c82b0f2d408236235c772b5f5b10 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 11 Oct 2022 12:35:58 -0400 Subject: [PATCH 098/118] clean up the prunerFunc --- gtsam/hybrid/GaussianMixture.cpp | 19 ++++++++++--------- gtsam/hybrid/GaussianMixture.h | 11 +++++++++++ 2 files changed, 21 insertions(+), 9 deletions(-) diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp index 064905d6b..244d52738 100644 --- a/gtsam/hybrid/GaussianMixture.cpp +++ b/gtsam/hybrid/GaussianMixture.cpp @@ -131,7 +131,7 @@ void GaussianMixture::print(const std::string &s, /* ************************************************************************* */ /// Return the DiscreteKey vector as a set. -static std::set DiscreteKeysAsSet(const DiscreteKeys &dkeys) { +std::set DiscreteKeysAsSet(const DiscreteKeys &dkeys) { std::set s; s.insert(dkeys.begin(), dkeys.end()); return s; @@ -142,18 +142,19 @@ static std::set DiscreteKeysAsSet(const DiscreteKeys &dkeys) { * @brief Helper function to get the pruner functional. * * @param decisionTree The probability decision tree of only discrete keys. - * @param decisionTreeKeySet Set of DiscreteKeys in decisionTree. - * Pre-computed for efficiency. - * @param gaussianMixtureKeySet Set of DiscreteKeys in the GaussianMixture. * @return std::function &, const GaussianConditional::shared_ptr &)> */ std::function &, const GaussianConditional::shared_ptr &)> -PrunerFunc(const DecisionTreeFactor &decisionTree, - const std::set &decisionTreeKeySet, - const std::set &gaussianMixtureKeySet) { - auto pruner = [&](const Assignment &choices, +GaussianMixture::prunerFunc(const DecisionTreeFactor &decisionTree) { + // Get the discrete keys as sets for the decision tree + // and the gaussian mixture. + auto decisionTreeKeySet = DiscreteKeysAsSet(decisionTree.discreteKeys()); + auto gaussianMixtureKeySet = DiscreteKeysAsSet(this->discreteKeys()); + + auto pruner = [decisionTree, decisionTreeKeySet, gaussianMixtureKeySet]( + const Assignment &choices, const GaussianConditional::shared_ptr &conditional) -> GaussianConditional::shared_ptr { // typecast so we can use this to get probability value @@ -202,7 +203,7 @@ void GaussianMixture::prune(const DecisionTreeFactor &decisionTree) { auto gmKeySet = DiscreteKeysAsSet(this->discreteKeys()); // Functional which loops over all assignments and create a set of // GaussianConditionals - auto pruner = PrunerFunc(decisionTree, decisionTreeKeySet, gmKeySet); + auto pruner = prunerFunc(decisionTree); auto pruned_conditionals = conditionals_.apply(pruner); conditionals_.root_ = pruned_conditionals.root_; diff --git a/gtsam/hybrid/GaussianMixture.h b/gtsam/hybrid/GaussianMixture.h index 75deb4d55..9792a8532 100644 --- a/gtsam/hybrid/GaussianMixture.h +++ b/gtsam/hybrid/GaussianMixture.h @@ -69,6 +69,17 @@ class GTSAM_EXPORT GaussianMixture */ Sum asGaussianFactorGraphTree() const; + /** + * @brief Helper function to get the pruner functor. + * + * @param decisionTree The pruned discrete probability decision tree. + * @return std::function &, const GaussianConditional::shared_ptr &)> + */ + std::function &, const GaussianConditional::shared_ptr &)> + prunerFunc(const DecisionTreeFactor &decisionTree); + public: /// @name Constructors /// @{ From 5e99cd7095cbdf1da731dda0de3bc1a6a680d4c9 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 11 Oct 2022 12:36:58 -0400 Subject: [PATCH 099/118] HybridBayesNet and HybridBayesTree both use similar pruning functions --- gtsam/hybrid/HybridBayesNet.cpp | 101 ++---------------- gtsam/hybrid/HybridBayesTree.cpp | 20 ++-- gtsam/hybrid/tests/testHybridGaussianISAM.cpp | 2 +- .../hybrid/tests/testHybridNonlinearISAM.cpp | 2 +- 4 files changed, 15 insertions(+), 110 deletions(-) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index 96a6dfd63..cc27600f0 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -22,14 +22,6 @@ namespace gtsam { -/* ************************************************************************* */ -/// Return the DiscreteKey vector as a set. -static std::set DiscreteKeysAsSet(const DiscreteKeys &dkeys) { - std::set s; - s.insert(dkeys.begin(), dkeys.end()); - return s; -} - /* ************************************************************************* */ DecisionTreeFactor::shared_ptr HybridBayesNet::discreteConditionals() const { AlgebraicDecisionTree decisionTree; @@ -49,63 +41,6 @@ DecisionTreeFactor::shared_ptr HybridBayesNet::discreteConditionals() const { return boost::make_shared(dtFactor); } -/** - * @brief Helper function to get the pruner functional. - * - * @param probDecisionTree The probability decision tree of only discrete keys. - * @param discreteFactorKeySet Set of DiscreteKeys in probDecisionTree. - * Pre-computed for efficiency. - * @param gaussianMixtureKeySet Set of DiscreteKeys in the GaussianMixture. - * @return std::function &, const GaussianConditional::shared_ptr &)> - */ -std::function &, const GaussianConditional::shared_ptr &)> -PrunerFunc(const DecisionTreeFactor::shared_ptr &probDecisionTree, - const std::set &discreteFactorKeySet, - const std::set &gaussianMixtureKeySet) { - auto pruner = [&](const Assignment &choices, - const GaussianConditional::shared_ptr &conditional) - -> GaussianConditional::shared_ptr { - // typecast so we can use this to get probability value - DiscreteValues values(choices); - - // Case where the gaussian mixture has the same - // discrete keys as the decision tree. - if (gaussianMixtureKeySet == discreteFactorKeySet) { - if ((*probDecisionTree)(values) == 0.0) { - // empty aka null pointer - boost::shared_ptr null; - return null; - } else { - return conditional; - } - } else { - std::vector set_diff; - std::set_difference( - discreteFactorKeySet.begin(), discreteFactorKeySet.end(), - gaussianMixtureKeySet.begin(), gaussianMixtureKeySet.end(), - std::back_inserter(set_diff)); - const std::vector assignments = - DiscreteValues::CartesianProduct(set_diff); - for (const DiscreteValues &assignment : assignments) { - DiscreteValues augmented_values(values); - augmented_values.insert(assignment.begin(), assignment.end()); - - // If any one of the sub-branches are non-zero, - // we need this conditional. - if ((*probDecisionTree)(augmented_values) > 0.0) { - return conditional; - } - } - // If we are here, it means that all the sub-branches are 0, - // so we prune. - return nullptr; - } - }; - return pruner; -} - /* ************************************************************************* */ HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) const { // Get the decision tree of only the discrete keys @@ -114,8 +49,6 @@ HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) const { boost::make_shared( discreteConditionals->prune(maxNrLeaves)); - auto discreteFactorKeySet = DiscreteKeysAsSet(discreteFactor->discreteKeys()); - /* To Prune, we visitWith every leaf in the GaussianMixture. * For each leaf, using the assignment we can check the discrete decision tree * for 0.0 probability, then just set the leaf to a nullptr. @@ -130,35 +63,13 @@ HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) const { for (size_t i = 0; i < this->size(); i++) { HybridConditional::shared_ptr conditional = this->at(i); - GaussianMixture::shared_ptr gaussianMixture = - boost::dynamic_pointer_cast(conditional->inner()); + if (conditional->isHybrid()) { + GaussianMixture::shared_ptr gaussianMixture = conditional->asMixture(); - if (gaussianMixture) { - // We may have mixtures with less discrete keys than discreteFactor so - // we skip those since the label assignment does not exist. - auto gmKeySet = DiscreteKeysAsSet(gaussianMixture->discreteKeys()); - - // Get the pruner function. - auto pruner = PrunerFunc(discreteFactor, discreteFactorKeySet, gmKeySet); - - // Run the pruning to get a new, pruned tree - GaussianMixture::Conditionals prunedTree = - gaussianMixture->conditionals().apply(pruner); - - DiscreteKeys discreteKeys = gaussianMixture->discreteKeys(); - // reverse keys to get a natural ordering - std::reverse(discreteKeys.begin(), discreteKeys.end()); - - // Convert from boost::iterator_range to KeyVector - // so we can pass it to constructor. - KeyVector frontals(gaussianMixture->frontals().begin(), - gaussianMixture->frontals().end()), - parents(gaussianMixture->parents().begin(), - gaussianMixture->parents().end()); - - // Create the new gaussian mixture and add it to the bayes net. - auto prunedGaussianMixture = boost::make_shared( - frontals, parents, discreteKeys, prunedTree); + // Make a copy of the gaussian mixture and prune it! + auto prunedGaussianMixture = + boost::make_shared(*gaussianMixture); + prunedGaussianMixture->prune(*discreteFactor); // Type-erase and add to the pruned Bayes Net fragment. prunedBayesNetFragment.push_back( diff --git a/gtsam/hybrid/HybridBayesTree.cpp b/gtsam/hybrid/HybridBayesTree.cpp index 8fb487ae2..266b295dd 100644 --- a/gtsam/hybrid/HybridBayesTree.cpp +++ b/gtsam/hybrid/HybridBayesTree.cpp @@ -149,16 +149,16 @@ void HybridBayesTree::prune(const size_t maxNrLeaves) { auto decisionTree = boost::dynamic_pointer_cast( this->roots_.at(0)->conditional()->inner()); - DecisionTreeFactor prunedDiscreteFactor = decisionTree->prune(maxNrLeaves); - decisionTree->root_ = prunedDiscreteFactor.root_; + DecisionTreeFactor prunedDecisionTree = decisionTree->prune(maxNrLeaves); + decisionTree->root_ = prunedDecisionTree.root_; /// Helper struct for pruning the hybrid bayes tree. struct HybridPrunerData { /// The discrete decision tree after pruning. - DecisionTreeFactor prunedDiscreteFactor; - HybridPrunerData(const DecisionTreeFactor& prunedDiscreteFactor, + DecisionTreeFactor prunedDecisionTree; + HybridPrunerData(const DecisionTreeFactor& prunedDecisionTree, const HybridBayesTree::sharedNode& parentClique) - : prunedDiscreteFactor(prunedDiscreteFactor) {} + : prunedDecisionTree(prunedDecisionTree) {} /** * @brief A function used during tree traversal that operates on each node @@ -178,19 +178,13 @@ void HybridBayesTree::prune(const size_t maxNrLeaves) { if (conditional->isHybrid()) { auto gaussianMixture = conditional->asMixture(); - // Check if the number of discrete keys match, - // else we get an assignment error. - // TODO(Varun) Update prune method to handle assignment subset? - if (gaussianMixture->discreteKeys() == - parentData.prunedDiscreteFactor.discreteKeys()) { - gaussianMixture->prune(parentData.prunedDiscreteFactor); - } + gaussianMixture->prune(parentData.prunedDecisionTree); } return parentData; } }; - HybridPrunerData rootData(prunedDiscreteFactor, 0); + HybridPrunerData rootData(prunedDecisionTree, 0); { treeTraversal::no_op visitorPost; // Limits OpenMP threads since we're mixing TBB and OpenMP diff --git a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp index a0a87933f..a5e3903d9 100644 --- a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp @@ -337,7 +337,7 @@ TEST(HybridGaussianElimination, Incremental_approximate) { EXPECT_LONGS_EQUAL( 2, incrementalHybrid[X(1)]->conditional()->asMixture()->nrComponents()); EXPECT_LONGS_EQUAL( - 4, incrementalHybrid[X(2)]->conditional()->asMixture()->nrComponents()); + 3, incrementalHybrid[X(2)]->conditional()->asMixture()->nrComponents()); EXPECT_LONGS_EQUAL( 5, incrementalHybrid[X(3)]->conditional()->asMixture()->nrComponents()); EXPECT_LONGS_EQUAL( diff --git a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp index 4e1710c42..93a8a1e00 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp @@ -363,7 +363,7 @@ TEST(HybridNonlinearISAM, Incremental_approximate) { EXPECT_LONGS_EQUAL( 2, bayesTree[X(1)]->conditional()->asMixture()->nrComponents()); EXPECT_LONGS_EQUAL( - 4, bayesTree[X(2)]->conditional()->asMixture()->nrComponents()); + 3, bayesTree[X(2)]->conditional()->asMixture()->nrComponents()); EXPECT_LONGS_EQUAL( 5, bayesTree[X(3)]->conditional()->asMixture()->nrComponents()); EXPECT_LONGS_EQUAL( From c2377f3b34e68212a75e7dfb403f32c87a056e0c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 11 Oct 2022 13:15:50 -0400 Subject: [PATCH 100/118] minor fixes to unit test --- .../hybrid/tests/testHybridNonlinearISAM.cpp | 21 +++++++++++++++++-- 1 file changed, 19 insertions(+), 2 deletions(-) diff --git a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp index 93a8a1e00..cc83ccac6 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp @@ -12,7 +12,7 @@ /** * @file testHybridNonlinearISAM.cpp * @brief Unit tests for nonlinear incremental inference - * @author Fan Jiang, Varun Agrawal, Frank Dellaert + * @author Varun Agrawal, Fan Jiang, Frank Dellaert * @date Jan 2021 */ @@ -391,6 +391,23 @@ TEST(HybridNonlinearISAM, Incremental_approximate) { 5, bayesTree[X(5)]->conditional()->asMixture()->nrComponents()); } +void printConditionals(const HybridNonlinearISAM &inc, const KeyVector &keys) { + HybridGaussianISAM bayesTree = inc.bayesTree(); + for (auto &&key : keys) { + std::cout << DefaultKeyFormatter(key) << std::endl; + auto conditional = bayesTree[key]->conditional(); + conditional->printKeys(); + if (conditional->isHybrid()) { + std::cout << conditional->asMixture()->nrComponents() << std::endl; + } else if (conditional->isDiscrete()) { + std::cout << conditional->asDiscreteConditional()->nrLeaves() + << std::endl; + } else { + // std::cout << conditional->asGaussian()->nrComponents() << std::endl; + } + } +} + /* ************************************************************************/ // A GTSAM-only test for running inference on a single-legged robot. // The leg links are represented by the chain X-Y-Z-W, where X is the base and @@ -432,9 +449,9 @@ TEST(HybridNonlinearISAM, NonTrivial) { // Don't run update now since we don't have discrete variables involved. - /*************** Run Round 2 ***************/ using PlanarMotionModel = BetweenFactor; + /*************** Run Round 2 ***************/ // Add odometry factor with discrete modes. Pose2 odometry(1.0, 0.0, 0.0); KeyVector contKeys = {W(0), W(1)}; From 80908126db77063ab390f6794f7b32ed0a399c12 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 11 Oct 2022 13:16:05 -0400 Subject: [PATCH 101/118] add assertions to remove warning --- gtsam/discrete/tests/testDiscreteBayesTree.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/gtsam/discrete/tests/testDiscreteBayesTree.cpp b/gtsam/discrete/tests/testDiscreteBayesTree.cpp index 6635633a2..ab69e82d7 100644 --- a/gtsam/discrete/tests/testDiscreteBayesTree.cpp +++ b/gtsam/discrete/tests/testDiscreteBayesTree.cpp @@ -159,6 +159,10 @@ TEST(DiscreteBayesTree, ThinTree) { clique->separatorMarginal(EliminateDiscrete); DOUBLES_EQUAL(joint_8_12, separatorMarginal0(all1), 1e-9); + DOUBLES_EQUAL(joint_12_14, 0.1875, 1e-9); + DOUBLES_EQUAL(joint_8_12_14, 0.0375, 1e-9); + DOUBLES_EQUAL(joint_9_12_14, 0.15, 1e-9); + // check separator marginal P(S9), should be P(14) clique = (*self.bayesTree)[9]; DiscreteFactorGraph separatorMarginal9 = From eacc888d2065491dbeed5f1dc59b1494534f63b0 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 11 Oct 2022 13:27:15 -0400 Subject: [PATCH 102/118] remove print function --- gtsam/hybrid/tests/testHybridNonlinearISAM.cpp | 17 ----------------- 1 file changed, 17 deletions(-) diff --git a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp index cc83ccac6..fbb114ef3 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp @@ -391,23 +391,6 @@ TEST(HybridNonlinearISAM, Incremental_approximate) { 5, bayesTree[X(5)]->conditional()->asMixture()->nrComponents()); } -void printConditionals(const HybridNonlinearISAM &inc, const KeyVector &keys) { - HybridGaussianISAM bayesTree = inc.bayesTree(); - for (auto &&key : keys) { - std::cout << DefaultKeyFormatter(key) << std::endl; - auto conditional = bayesTree[key]->conditional(); - conditional->printKeys(); - if (conditional->isHybrid()) { - std::cout << conditional->asMixture()->nrComponents() << std::endl; - } else if (conditional->isDiscrete()) { - std::cout << conditional->asDiscreteConditional()->nrLeaves() - << std::endl; - } else { - // std::cout << conditional->asGaussian()->nrComponents() << std::endl; - } - } -} - /* ************************************************************************/ // A GTSAM-only test for running inference on a single-legged robot. // The leg links are represented by the chain X-Y-Z-W, where X is the base and From 4f406650f73f90811f100c7e0b4f212d23836aad Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Wed, 12 Oct 2022 21:29:18 -0400 Subject: [PATCH 103/118] rename KeypointsList -> KeypointsVector --- gtsam/sfm/DsfTrackGenerator.h | 4 ++-- gtsam/sfm/sfm.i | 8 ++++---- python/CMakeLists.txt | 4 ++-- python/gtsam/specializations/sfm.h | 2 +- python/gtsam/tests/test_DsfTrackGenerator.py | 4 ++-- 5 files changed, 11 insertions(+), 11 deletions(-) diff --git a/gtsam/sfm/DsfTrackGenerator.h b/gtsam/sfm/DsfTrackGenerator.h index 05ad61390..a16ed3522 100644 --- a/gtsam/sfm/DsfTrackGenerator.h +++ b/gtsam/sfm/DsfTrackGenerator.h @@ -57,7 +57,7 @@ struct Keypoints { Keypoints(const gtsam::KeypointCoordinates& coordinates): coordinates(coordinates) {}; // boost::none }; -using KeypointsList = std::vector; +using KeypointsVector = std::vector; // Mapping from each image pair to (N,2) array representing indices of matching keypoints. using MatchIndicesMap = std::map; @@ -143,7 +143,7 @@ class DsfTrackGenerator { // @param Length-N list of keypoints, for N images/cameras. std::vector generate_tracks_from_pairwise_matches( const MatchIndicesMap& matches_dict, - const KeypointsList& keypoints_list) { + const KeypointsVector& keypoints_list) { std::vector track_2d_list; std::cout << "[SfmTrack2d] Starting Union-Find..." << std::endl; diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index 6fd380b3c..9613410a7 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -24,9 +24,9 @@ class Keypoints }; // check if this should be a method -class KeypointsList { - KeypointsList(); - KeypointsList(const gtsam::KeypointsList& other); +class KeypointsVector { + KeypointsVector(); + KeypointsVector(const gtsam::KeypointsVector& other); void push_back(const gtsam::Keypoints& keypoints); size_t size() const; bool empty() const; @@ -78,7 +78,7 @@ class DsfTrackGenerator { DsfTrackGenerator(); const gtsam::SfmTrack2dVector generate_tracks_from_pairwise_matches( const gtsam::MatchIndicesMap& matches_dict, - const gtsam::KeypointsList& keypoints_list); + const gtsam::KeypointsVector& keypoints_list); }; diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 21f9f951d..600aeecc9 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -53,7 +53,7 @@ set(ignore gtsam::DiscreteKey gtsam::KeyPairDoubleMap gtsam::MatchIndicesMap - gtsam::KeypointsList + gtsam::KeypointsVector gtsam::SfmTrack2dVector gtsam::NamedSfmMeasurementVector) @@ -154,7 +154,7 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) gtsam::CameraSetCal3Fisheye gtsam::KeyPairDoubleMap gtsam::MatchIndicesMap - gtsam::KeypointsList + gtsam::KeypointsVector gtsam::SfmTrack2dVector gtsam::NamedSfmMeasurementVector) diff --git a/python/gtsam/specializations/sfm.h b/python/gtsam/specializations/sfm.h index 1bbb93570..63f7f1a48 100644 --- a/python/gtsam/specializations/sfm.h +++ b/python/gtsam/specializations/sfm.h @@ -22,7 +22,7 @@ py::bind_map(m_, "MatchIndicesMap"); py::bind_vector< std::vector >( - m_, "KeypointsList"); + m_, "KeypointsVector"); py::bind_vector< diff --git a/python/gtsam/tests/test_DsfTrackGenerator.py b/python/gtsam/tests/test_DsfTrackGenerator.py index 3d4188284..563a01165 100644 --- a/python/gtsam/tests/test_DsfTrackGenerator.py +++ b/python/gtsam/tests/test_DsfTrackGenerator.py @@ -12,7 +12,7 @@ from gtsam import ( DsfTrackGenerator, IndexPair, Keypoints, - KeypointsList, + KeypointsVector, MatchIndicesMap, NamedSfmMeasurement, NamedSfmMeasurementVector, @@ -30,7 +30,7 @@ class TestDsfTrackGenerator(GtsamTestCase): kps_i1 = Keypoints(coordinates=np.array([[50.0, 60], [70, 80], [90, 100]])) kps_i2 = Keypoints(coordinates=np.array([[110.0, 120], [130, 140]])) - keypoints_list = KeypointsList() + keypoints_list = KeypointsVector() keypoints_list.append(kps_i0) keypoints_list.append(kps_i1) keypoints_list.append(kps_i2) From 7dee1af5b4dec1ef1a558669e18e39d7ed638f5a Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Wed, 12 Oct 2022 23:31:45 -0400 Subject: [PATCH 104/118] add verbose flag for logging messages during DSF track generation --- gtsam/sfm/DsfTrackGenerator.h | 19 +++++++++++-------- gtsam/sfm/sfm.i | 3 ++- python/gtsam/tests/test_DsfTrackGenerator.py | 4 +++- 3 files changed, 16 insertions(+), 10 deletions(-) diff --git a/gtsam/sfm/DsfTrackGenerator.h b/gtsam/sfm/DsfTrackGenerator.h index a16ed3522..960f1622c 100644 --- a/gtsam/sfm/DsfTrackGenerator.h +++ b/gtsam/sfm/DsfTrackGenerator.h @@ -143,10 +143,11 @@ class DsfTrackGenerator { // @param Length-N list of keypoints, for N images/cameras. std::vector generate_tracks_from_pairwise_matches( const MatchIndicesMap& matches_dict, - const KeypointsVector& keypoints_list) { + const KeypointsVector& keypoints_list, + bool verbose = false) { std::vector track_2d_list; - std::cout << "[SfmTrack2d] Starting Union-Find..." << std::endl; + if (verbose) std::cout << "[SfmTrack2d] Starting Union-Find..." << std::endl; // Generate the DSF to form tracks. DSFMapIndexPair dsf; @@ -167,7 +168,7 @@ class DsfTrackGenerator { } } - std::cout << "[SfmTrack2d] Union-Find Complete" << std::endl; + if (verbose) std::cout << "[SfmTrack2d] Union-Find Complete" << std::endl; const std::map > key_sets = dsf.sets(); // Return immediately if no sets were found. @@ -202,12 +203,14 @@ class DsfTrackGenerator { } } - double erroneous_track_pct = static_cast(erroneous_track_count) - / static_cast(key_sets.size()) * 100; - // TODO(johnwlambert): restrict decimal places to 2 decimals. - std::cout << "DSF Union-Find: " << erroneous_track_pct; - std::cout << "% of tracks discarded from multiple obs. in a single image." << std::endl; + if (verbose) { + double erroneous_track_pct = static_cast(erroneous_track_count) + / static_cast(key_sets.size()) * 100; + // TODO(johnwlambert): restrict decimal places to 2 decimals. + std::cout << "DSF Union-Find: " << erroneous_track_pct; + std::cout << "% of tracks discarded from multiple obs. in a single image." << std::endl; + } // TODO(johnwlambert): return the Transitivity failure percentage here. return track_2d_list; } diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index 9613410a7..ce840a1a9 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -78,7 +78,8 @@ class DsfTrackGenerator { DsfTrackGenerator(); const gtsam::SfmTrack2dVector generate_tracks_from_pairwise_matches( const gtsam::MatchIndicesMap& matches_dict, - const gtsam::KeypointsVector& keypoints_list); + const gtsam::KeypointsVector& keypoints_list, + bool verbose = false); }; diff --git a/python/gtsam/tests/test_DsfTrackGenerator.py b/python/gtsam/tests/test_DsfTrackGenerator.py index 563a01165..721584cff 100644 --- a/python/gtsam/tests/test_DsfTrackGenerator.py +++ b/python/gtsam/tests/test_DsfTrackGenerator.py @@ -40,7 +40,9 @@ class TestDsfTrackGenerator(GtsamTestCase): matches_dict[IndexPair(0, 1)] = np.array([[0, 0], [1, 1]]) matches_dict[IndexPair(1, 2)] = np.array([[2, 0], [1, 1]]) - tracks = DsfTrackGenerator().generate_tracks_from_pairwise_matches(matches_dict, keypoints_list) + tracks = DsfTrackGenerator().generate_tracks_from_pairwise_matches(matches_dict, + keypoints_list, + verbose=True) assert len(tracks) == 3 # Verify track 0. From f9971f504988f891f9ffae02ba9e94009d2ce2a6 Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Thu, 13 Oct 2022 01:07:41 -0400 Subject: [PATCH 105/118] use gtsam.gtsfm namespace for new API --- gtsam/sfm/DsfTrackGenerator.h | 8 +++- gtsam/sfm/sfm.i | 44 ++++++++++---------- python/CMakeLists.txt | 16 +++---- python/gtsam/preamble/sfm.h | 6 +-- python/gtsam/specializations/sfm.h | 22 ++++------ python/gtsam/tests/test_DsfTrackGenerator.py | 7 +--- 6 files changed, 51 insertions(+), 52 deletions(-) diff --git a/gtsam/sfm/DsfTrackGenerator.h b/gtsam/sfm/DsfTrackGenerator.h index 960f1622c..47578d6ea 100644 --- a/gtsam/sfm/DsfTrackGenerator.h +++ b/gtsam/sfm/DsfTrackGenerator.h @@ -30,6 +30,8 @@ namespace gtsam { +namespace gtsfm { + typedef DSFMap DSFMapIndexPair; typedef Eigen::MatrixX2i CorrespondenceIndices; // N x 2 array @@ -54,7 +56,7 @@ struct Keypoints { /// Optional confidences/responses for each detection, of shape N. boost::optional responses; - Keypoints(const gtsam::KeypointCoordinates& coordinates): coordinates(coordinates) {}; // boost::none + Keypoints(const gtsam::gtsfm::KeypointCoordinates& coordinates): coordinates(coordinates) {}; // boost::none }; using KeypointsVector = std::vector; @@ -119,7 +121,7 @@ class SfmTrack2d { } }; -using SfmTrack2dVector = std::vector; +using SfmTrack2dVector = std::vector; using NamedSfmMeasurementVector = std::vector; @@ -216,5 +218,7 @@ class DsfTrackGenerator { } }; +}///\namespace gtsfm + } // namespace gtsam diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index ce840a1a9..984955d23 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -4,34 +4,36 @@ namespace gtsam { +namespace gtsfm { + #include class MatchIndicesMap { MatchIndicesMap(); - MatchIndicesMap(const gtsam::MatchIndicesMap& other); + MatchIndicesMap(const gtsam::gtsfm::MatchIndicesMap& other); size_t size() const; bool empty() const; void clear(); - gtsam::CorrespondenceIndices at(const pair& keypair) const; + gtsam::gtsfm::CorrespondenceIndices at(const pair& keypair) const; }; class Keypoints { - Keypoints(const gtsam::KeypointCoordinates& coordinates); - gtsam::KeypointCoordinates coordinates; + Keypoints(const gtsam::gtsfm::KeypointCoordinates& coordinates); + gtsam::gtsfm::KeypointCoordinates coordinates; }; // check if this should be a method class KeypointsVector { KeypointsVector(); - KeypointsVector(const gtsam::KeypointsVector& other); - void push_back(const gtsam::Keypoints& keypoints); + KeypointsVector(const gtsam::gtsfm::KeypointsVector& other); + void push_back(const gtsam::gtsfm::Keypoints& keypoints); size_t size() const; bool empty() const; void clear(); - gtsam::Keypoints at(const size_t& index) const; + gtsam::gtsfm::Keypoints at(const size_t& index) const; }; @@ -44,44 +46,44 @@ class NamedSfmMeasurement class NamedSfmMeasurementVector { NamedSfmMeasurementVector(); - NamedSfmMeasurementVector(const gtsam::NamedSfmMeasurementVector& other); - void push_back(const gtsam::NamedSfmMeasurement& measurement); + NamedSfmMeasurementVector(const gtsam::gtsfm::NamedSfmMeasurementVector& other); + void push_back(const gtsam::gtsfm::NamedSfmMeasurement& measurement); size_t size() const; bool empty() const; void clear(); - gtsam::NamedSfmMeasurement at(const size_t& index) const; + gtsam::gtsfm::NamedSfmMeasurement at(const size_t& index) const; }; class SfmTrack2d { SfmTrack2d(); - SfmTrack2d(std::vector &measurements); + SfmTrack2d(std::vector &measurements); size_t numberMeasurements() const; - void addMeasurement(const gtsam::NamedSfmMeasurement &m); - std::vector measurements(); - gtsam::NamedSfmMeasurement measurement(size_t idx) const; + void addMeasurement(const gtsam::gtsfm::NamedSfmMeasurement &m); + std::vector measurements(); + gtsam::gtsfm::NamedSfmMeasurement measurement(size_t idx) const; bool hasUniqueCameras(); }; class SfmTrack2dVector { SfmTrack2dVector(); - SfmTrack2dVector(const gtsam::SfmTrack2dVector& other); - void push_back(const gtsam::SfmTrack2d& track); + SfmTrack2dVector(const gtsam::gtsfm::SfmTrack2dVector& other); + void push_back(const gtsam::gtsfm::SfmTrack2d& track); size_t size() const; bool empty() const; void clear(); - gtsam::SfmTrack2d at(const size_t& index) const; + gtsam::gtsfm::SfmTrack2d at(const size_t& index) const; }; class DsfTrackGenerator { DsfTrackGenerator(); - const gtsam::SfmTrack2dVector generate_tracks_from_pairwise_matches( - const gtsam::MatchIndicesMap& matches_dict, - const gtsam::KeypointsVector& keypoints_list, + const gtsam::gtsfm::SfmTrack2dVector generate_tracks_from_pairwise_matches( + const gtsam::gtsfm::MatchIndicesMap& matches_dict, + const gtsam::gtsfm::KeypointsVector& keypoints_list, bool verbose = false); }; - +}///\namespace gtsfm #include #include diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 600aeecc9..398ee2788 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -52,10 +52,10 @@ set(ignore gtsam::BinaryMeasurementsRot3 gtsam::DiscreteKey gtsam::KeyPairDoubleMap - gtsam::MatchIndicesMap - gtsam::KeypointsVector - gtsam::SfmTrack2dVector - gtsam::NamedSfmMeasurementVector) + gtsam::gtsfm::MatchIndicesMap + gtsam::gtsfm::KeypointsVector + gtsam::gtsfm::SfmTrack2dVector + gtsam::gtsfm::NamedSfmMeasurementVector) set(interface_headers ${PROJECT_SOURCE_DIR}/gtsam/gtsam.i @@ -153,10 +153,10 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) gtsam::CameraSetCal3Unified gtsam::CameraSetCal3Fisheye gtsam::KeyPairDoubleMap - gtsam::MatchIndicesMap - gtsam::KeypointsVector - gtsam::SfmTrack2dVector - gtsam::NamedSfmMeasurementVector) + gtsam::gtsfm::MatchIndicesMap + gtsam::gtsfm::KeypointsVector + gtsam::gtsfm::SfmTrack2dVector + gtsam::gtsfm::NamedSfmMeasurementVector) pybind_wrap(${GTSAM_PYTHON_UNSTABLE_TARGET} # target diff --git a/python/gtsam/preamble/sfm.h b/python/gtsam/preamble/sfm.h index 91c9fc418..067dd8801 100644 --- a/python/gtsam/preamble/sfm.h +++ b/python/gtsam/preamble/sfm.h @@ -24,7 +24,7 @@ PYBIND11_MAKE_OPAQUE( PYBIND11_MAKE_OPAQUE( std::vector>); PYBIND11_MAKE_OPAQUE( - std::vector); -PYBIND11_MAKE_OPAQUE(gtsam::MatchIndicesMap); + std::vector); +PYBIND11_MAKE_OPAQUE(gtsam::gtsfm::MatchIndicesMap); PYBIND11_MAKE_OPAQUE( - std::vector); \ No newline at end of file + std::vector); \ No newline at end of file diff --git a/python/gtsam/specializations/sfm.h b/python/gtsam/specializations/sfm.h index 63f7f1a48..10fb1a0ab 100644 --- a/python/gtsam/specializations/sfm.h +++ b/python/gtsam/specializations/sfm.h @@ -18,29 +18,25 @@ py::bind_vector > >( py::bind_vector > >( m_, "BinaryMeasurementsRot3"); py::bind_map(m_, "KeyPairDoubleMap"); -py::bind_map(m_, "MatchIndicesMap"); - -py::bind_vector< - std::vector >( - m_, "KeypointsVector"); - - py::bind_vector< std::vector >( m_, "SfmTracks"); - py::bind_vector< std::vector >( m_, "SfmCameras"); - py::bind_vector< std::vector>>( m_, "SfmMeasurementVector" ); -py::bind_vector< - std::vector >( - m_, "SfmTrack2dVector"); py::bind_vector< - std::vector >( + std::vector >( + m_, "SfmTrack2dVector"); +py::bind_vector< + std::vector >( m_, "NamedSfmMeasurementVector"); +py::bind_map(m_, "MatchIndicesMap"); + +py::bind_vector< + std::vector >( + m_, "KeypointsVector"); diff --git a/python/gtsam/tests/test_DsfTrackGenerator.py b/python/gtsam/tests/test_DsfTrackGenerator.py index 721584cff..f1851c221 100644 --- a/python/gtsam/tests/test_DsfTrackGenerator.py +++ b/python/gtsam/tests/test_DsfTrackGenerator.py @@ -8,14 +8,11 @@ import unittest import numpy as np import gtsam -from gtsam import ( +from gtsam import IndexPair, KeypointsVector, MatchIndicesMap, NamedSfmMeasurementVector +from gtsam.gtsam.gtsfm import ( DsfTrackGenerator, - IndexPair, Keypoints, - KeypointsVector, - MatchIndicesMap, NamedSfmMeasurement, - NamedSfmMeasurementVector, SfmTrack2d, ) from gtsam.utils.test_case import GtsamTestCase From 73fd3f9dc2eb249397909a098247b0a66da709c6 Mon Sep 17 00:00:00 2001 From: senselessdev1 Date: Thu, 13 Oct 2022 09:12:55 -0400 Subject: [PATCH 106/118] add namespace hack to prevent gtsam.gtsam.gtsfm --- python/gtsam/gtsfm.py | 4 ++++ python/gtsam/tests/test_DsfTrackGenerator.py | 2 +- 2 files changed, 5 insertions(+), 1 deletion(-) create mode 100644 python/gtsam/gtsfm.py diff --git a/python/gtsam/gtsfm.py b/python/gtsam/gtsfm.py new file mode 100644 index 000000000..afa709083 --- /dev/null +++ b/python/gtsam/gtsfm.py @@ -0,0 +1,4 @@ +# This trick is to allow direct import of sub-modules +# without this, we can only do `from gtsam.gtsam.gtsfm import X` +# with this trick, we can do `from gtsam.gtsfm import X` +from .gtsam.gtsfm import * \ No newline at end of file diff --git a/python/gtsam/tests/test_DsfTrackGenerator.py b/python/gtsam/tests/test_DsfTrackGenerator.py index f1851c221..e63025cd3 100644 --- a/python/gtsam/tests/test_DsfTrackGenerator.py +++ b/python/gtsam/tests/test_DsfTrackGenerator.py @@ -9,7 +9,7 @@ import numpy as np import gtsam from gtsam import IndexPair, KeypointsVector, MatchIndicesMap, NamedSfmMeasurementVector -from gtsam.gtsam.gtsfm import ( +from gtsam.gtsfm import ( DsfTrackGenerator, Keypoints, NamedSfmMeasurement, From 8c19f499a3ff8d2f5fb7413aafa4ce03aeb722ea Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 13 Oct 2022 10:13:43 -0400 Subject: [PATCH 107/118] add curlyy brackets to for loop --- gtsam/hybrid/HybridGaussianISAM.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianISAM.cpp b/gtsam/hybrid/HybridGaussianISAM.cpp index 57e50104d..de87dd92f 100644 --- a/gtsam/hybrid/HybridGaussianISAM.cpp +++ b/gtsam/hybrid/HybridGaussianISAM.cpp @@ -59,8 +59,9 @@ void HybridGaussianISAM::updateInternal( factors += newFactors; // Add the orphaned subtrees - for (const sharedClique& orphan : *orphans) - factors += boost::make_shared >(orphan); + for (const sharedClique& orphan : *orphans) { + factors += boost::make_shared>(orphan); + } // Get all the discrete keys from the factors KeySet allDiscrete = factors.discreteKeys(); From 1d70d14c72b04e01e183d070ecfa71665b4123b5 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 13 Oct 2022 10:14:46 -0400 Subject: [PATCH 108/118] remove custom keyformatter --- gtsam/base/types.cpp | 46 -------------------------------------------- gtsam/base/types.h | 2 -- 2 files changed, 48 deletions(-) diff --git a/gtsam/base/types.cpp b/gtsam/base/types.cpp index f96e037c1..edc449b12 100644 --- a/gtsam/base/types.cpp +++ b/gtsam/base/types.cpp @@ -64,50 +64,4 @@ std::string demangle(const char* name) { return demangled_name; } -std::string GTDKeyFormatter(const Key &key) { - constexpr size_t kMax_uchar_ = std::numeric_limits::max(); - constexpr size_t key_bits = sizeof(gtsam::Key) * 8; - constexpr size_t ch1_bits = sizeof(uint8_t) * 8; - constexpr size_t ch2_bits = sizeof(uint8_t) * 8; - constexpr size_t link_bits = sizeof(uint8_t) * 8; - constexpr size_t joint_bits = sizeof(uint8_t) * 8; - constexpr size_t time_bits = - key_bits - ch1_bits - ch2_bits - link_bits - joint_bits; - - constexpr gtsam::Key ch1_mask = gtsam::Key(kMax_uchar_) - << (key_bits - ch1_bits); - constexpr gtsam::Key ch2_mask = gtsam::Key(kMax_uchar_) - << (key_bits - ch1_bits - ch2_bits); - constexpr gtsam::Key link_mask = gtsam::Key(kMax_uchar_) - << (time_bits + joint_bits); - constexpr gtsam::Key joint_mask = gtsam::Key(kMax_uchar_) << time_bits; - constexpr gtsam::Key time_mask = - ~(ch1_mask | ch2_mask | link_mask | joint_mask); - - uint8_t c1_, c2_, link_idx_, joint_idx_; - uint64_t t_; - - c1_ = (uint8_t)((key & ch1_mask) >> (key_bits - ch1_bits)); - c2_ = (uint8_t)((key & ch2_mask) >> (key_bits - ch1_bits - ch2_bits)); - link_idx_ = (uint8_t)((key & link_mask) >> (time_bits + joint_bits)); - joint_idx_ = (uint8_t)((key & joint_mask) >> time_bits); - t_ = key & time_mask; - - std::string s = ""; - if (c1_ != 0) { - s += c1_; - } - if (c2_ != 0) { - s += c2_; - } - if (link_idx_ != kMax_uchar_) { - s += "[" + std::to_string((int)(link_idx_)) + "]"; - } - if (joint_idx_ != kMax_uchar_) { - s += "(" + std::to_string((int)(joint_idx_)) + ")"; - } - s += std::to_string(t_); - return s; -} - } /* namespace gtsam */ diff --git a/gtsam/base/types.h b/gtsam/base/types.h index dcb389c9b..a0d24f1a6 100644 --- a/gtsam/base/types.h +++ b/gtsam/base/types.h @@ -74,8 +74,6 @@ namespace gtsam { /// The index type for Eigen objects typedef ptrdiff_t DenseIndex; - std::string GTDKeyFormatter(const Key &key); - /* ************************************************************************* */ /** * Helper class that uses templates to select between two types based on From 8f7473d0265e42227376b9555340f237768b45d4 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 13 Oct 2022 10:46:04 -0400 Subject: [PATCH 109/118] remove added test file --- gtsam/hybrid/tests/testHybridEstimation.cpp | 433 -------------------- 1 file changed, 433 deletions(-) delete mode 100644 gtsam/hybrid/tests/testHybridEstimation.cpp diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp deleted file mode 100644 index 5d47a22dd..000000000 --- a/gtsam/hybrid/tests/testHybridEstimation.cpp +++ /dev/null @@ -1,433 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file testHybridEstimation.cpp - * @brief Unit tests for Hybrid Estimation - * @author Varun Agrawal - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -// Include for test suite -#include - -using namespace std; -using namespace gtsam; - -using noiseModel::Isotropic; -using symbol_shorthand::L; -using symbol_shorthand::M; -using symbol_shorthand::X; - -class Robot { - DiscreteKeys modes_; - HybridNonlinearFactorGraph nonlinearFactorGraph_; - HybridGaussianFactorGraph linearizedFactorGraph_; - Values linearizationPoint_; - - public: - Robot(size_t K, std::vector measurements) { - // Create DiscreteKeys for binary K modes - for (size_t k = 0; k < K; k++) { - modes_.emplace_back(M(k), 2); - } - - ////// Create hybrid factor graph. - // Add measurement factors - auto measurement_noise = noiseModel::Isotropic::Sigma(1, 1.0); - for (size_t k = 0; k < K; k++) { - nonlinearFactorGraph_.emplace_nonlinear>( - X(k), measurements.at(k), measurement_noise); - } - - // 2 noise models where moving has a higher covariance. - auto still_noise_model = noiseModel::Isotropic::Sigma(1, 1e-2); - auto moving_noise_model = noiseModel::Isotropic::Sigma(1, 1e2); - - // Add "motion models". - // The idea is that the robot has a higher "freedom" (aka higher covariance) - // for movement - using MotionModel = BetweenFactor; - - for (size_t k = 1; k < K; k++) { - KeyVector keys = {X(k - 1), X(k)}; - DiscreteKeys dkeys{modes_[k - 1]}; - auto still = boost::make_shared(X(k - 1), X(k), 0.0, - still_noise_model), - moving = boost::make_shared(X(k - 1), X(k), 0.0, - moving_noise_model); - std::vector> components = {still, moving}; - nonlinearFactorGraph_.emplace_hybrid(keys, dkeys, - components); - } - - // Create the linearization point. - for (size_t k = 0; k < K; k++) { - linearizationPoint_.insert(X(k), static_cast(k + 1)); - } - - linearizedFactorGraph_ = - *nonlinearFactorGraph_.linearize(linearizationPoint_); - } - - void print() const { - nonlinearFactorGraph_.print(); - linearizationPoint_.print(); - linearizedFactorGraph_.print(); - } - - HybridValues optimize() const { - Ordering hybridOrdering = linearizedFactorGraph_.getHybridOrdering(); - HybridBayesNet::shared_ptr hybridBayesNet = - linearizedFactorGraph_.eliminateSequential(hybridOrdering); - - HybridValues delta = hybridBayesNet->optimize(); - return delta; - } -}; - -/* ****************************************************************************/ -/** - * I am trying to test if setting the hybrid mixture components to just differ - * in covariance makes sense. This is done by setting the "moving" covariance to - * be 1e2 while the "still" covariance is 1e-2. - */ -TEST(Estimation, StillRobot) { - size_t K = 2; - vector measurements = {0, 0}; - - Robot robot(K, measurements); - - HybridValues delta = robot.optimize(); - - delta.continuous().print("delta update:"); - if (delta.discrete()[M(0)] == 0) { - std::cout << "The robot is stationary!" << std::endl; - } else { - std::cout << "The robot has moved!" << std::endl; - } - EXPECT_LONGS_EQUAL(0, delta.discrete()[M(0)]); -} - -/* ****************************************************************************/ -TEST(Estimation, MovingRobot) { - size_t K = 2; - vector measurements = {0, 2}; - - Robot robot(K, measurements); - - HybridValues delta = robot.optimize(); - - delta.continuous().print("delta update:"); - if (delta.discrete()[M(0)] == 0) { - std::cout << "The robot is stationary!" << std::endl; - } else { - std::cout << "The robot has moved!" << std::endl; - } - EXPECT_LONGS_EQUAL(1, delta.discrete()[M(0)]); -} - -/// A robot with a single leg. -class SingleLeg { - DiscreteKeys modes_; - HybridNonlinearFactorGraph nonlinearFactorGraph_; - HybridGaussianFactorGraph linearizedFactorGraph_; - Values linearizationPoint_; - - public: - /** - * @brief Construct a new Single Leg object. - * - * @param K The number of discrete timesteps - * @param pims std::vector of preintegrated IMU measurements. - * @param fk std::vector of forward kinematic measurements for the leg. - */ - SingleLeg(size_t K, std::vector pims, std::vector fk) { - // Create DiscreteKeys for binary K modes - for (size_t k = 0; k < K; k++) { - modes_.emplace_back(M(k), 2); - } - - ////// Create hybrid factor graph. - - auto measurement_noise = noiseModel::Isotropic::Sigma(3, 1.0); - - // Add prior on the first pose - nonlinearFactorGraph_.emplace_nonlinear>( - X(0), Pose2(0, 2, 0), measurement_noise); - - // Add measurement factors. - // These are the preintegrated IMU measurements of the base. - for (size_t k = 0; k < K - 1; k++) { - nonlinearFactorGraph_.emplace_nonlinear>( - X(k), X(k + 1), pims.at(k), measurement_noise); - } - - // Forward kinematics from base X to foot L - auto fk_noise = noiseModel::Isotropic::Sigma(3, 1.0); - for (size_t k = 0; k < K; k++) { - nonlinearFactorGraph_.emplace_nonlinear>( - X(k), L(k), fk.at(k), fk_noise); - } - - // 2 noise models where moving has a higher covariance. - auto stance_model = noiseModel::Isotropic::Sigma(3, 1e-2); - auto swing_model = noiseModel::Isotropic::Sigma(3, 1e2); - - // Add "contact models" for the foot. - // The idea is that the robot's leg has a tight covariance for stance and - // loose covariance for swing. - using ContactFactor = BetweenFactor; - - for (size_t k = 0; k < K - 1; k++) { - KeyVector keys = {L(k), L(k + 1)}; - DiscreteKeys dkeys{modes_[k], modes_[k + 1]}; - auto stance = boost::make_shared( - keys.at(0), keys.at(1), Pose2(0, 0, 0), stance_model), - lift = boost::make_shared( - keys.at(0), keys.at(1), Pose2(0, -1, 0), swing_model), - land = boost::make_shared( - keys.at(0), keys.at(1), Pose2(0, 1, 0), swing_model), - swing = boost::make_shared( - keys.at(0), keys.at(1), Pose2(1, 0, 0), swing_model); - // 00 - swing, 01 - land, 10 - toe-off, 11 - stance - std::vector> components = {swing, land, - lift, stance}; - nonlinearFactorGraph_.emplace_hybrid(keys, dkeys, - components); - } - - // Create the linearization point. - for (size_t k = 0; k < K; k++) { - linearizationPoint_.insert(X(k), Pose2(k, 2, 0)); - linearizationPoint_.insert(L(k), Pose2(0, 0, 0)); - } - - linearizedFactorGraph_ = - *nonlinearFactorGraph_.linearize(linearizationPoint_); - } - - void print() const { - nonlinearFactorGraph_.print(); - linearizationPoint_.print(); - linearizedFactorGraph_.print(); - } - - HybridValues optimize() const { - Ordering hybridOrdering = linearizedFactorGraph_.getHybridOrdering(); - HybridBayesNet::shared_ptr hybridBayesNet = - linearizedFactorGraph_.eliminateSequential(hybridOrdering); - - HybridValues delta = hybridBayesNet->optimize(); - return delta; - } - - Values linearizationPoint() const { return linearizationPoint_; } -}; - -/* ****************************************************************************/ -TEST(Estimation, LeggedRobot) { - std::vector pims = {Pose2(1, 0, 0)}; - // Leg is in stance throughout - std::vector fk = {Pose2(0, -2, 0), Pose2(-1, -2, 0)}; - SingleLeg robot(2, pims, fk); - - std::cout << "\n\n\n" << std::endl; - // robot.print(); - Values initial = robot.linearizationPoint(); - // initial.print(); - - HybridValues delta = robot.optimize(); - // delta.print(); - - initial.retract(delta.continuous()).print("\n\n========="); - std::cout << "\n\n\n" << std::endl; -} - -/// A robot with a single leg - non-hybrid version. -class SL { - NonlinearFactorGraph nonlinearFactorGraph_; - GaussianFactorGraph linearizedFactorGraph_; - GaussianBayesNet bayesNet_; - Values linearizationPoint_; - - public: - /** - * @brief Construct a new Single Leg object. - * - * @param K The number of discrete timesteps - * @param pims std::vector of preintegrated IMU measurements. - * @param fk std::vector of forward kinematic measurements for the leg. - */ - SL(size_t K, const std::vector& pims, const std::vector& fk, - const std::vector& contacts) { - ////// Create hybrid factor graph. - - auto measurement_noise = noiseModel::Isotropic::Sigma(3, 1.0); - - // Add prior on the first pose - nonlinearFactorGraph_.emplace_shared>( - X(0), Pose2(0, 2, 0), measurement_noise); - - // Add measurement factors. - // These are the preintegrated IMU measurements of the base. - for (size_t k = 0; k < K - 1; k++) { - nonlinearFactorGraph_.emplace_shared>( - X(k), X(k + 1), pims.at(k), measurement_noise); - } - - // Forward kinematics from base X to foot L - auto fk_noise = noiseModel::Isotropic::Sigma(3, 1.0); - for (size_t k = 0; k < K; k++) { - nonlinearFactorGraph_.emplace_shared>( - X(k), L(k), fk.at(k), fk_noise); - } - - // 2 noise models where moving has a higher covariance. - auto stance_model = noiseModel::Isotropic::Sigma(3, 1e-4); - auto swing_model = noiseModel::Isotropic::Sigma(3, 1e8); - - // Add "contact models" for the foot. - // The idea is that the robot's leg has a tight covariance for stance and - // loose covariance for swing. - using ContactFactor = BetweenFactor; - - for (size_t k = 0; k < K - 1; k++) { - KeyVector keys = {L(k), L(k + 1)}; - ContactFactor::shared_ptr factor; - if (contacts[k] && contacts[k + 1]) { - // stance - std::cout << "stance 11" << std::endl; - factor = boost::make_shared( - keys.at(0), keys.at(1), Pose2(0, 0, 0), stance_model); - } else if (contacts[k] && !contacts[k + 1]) { - // toe-off - std::cout << "toe-off 10" << std::endl; - factor = boost::make_shared(keys.at(0), keys.at(1), - Pose2(0, 0, 0), swing_model); - } else if (!contacts[k] && contacts[k + 1]) { - // land - std::cout << "land 01" << std::endl; - factor = boost::make_shared(keys.at(0), keys.at(1), - Pose2(0, 0, 0), swing_model); - } else if (!contacts[k] && !contacts[k + 1]) { - // swing - std::cout << "swing 00" << std::endl; - factor = boost::make_shared(keys.at(0), keys.at(1), - Pose2(0, 0, 0), swing_model); - } - - nonlinearFactorGraph_.push_back(factor); - } - - // Create the linearization point. - for (size_t k = 0; k < K; k++) { - linearizationPoint_.insert(X(k), Pose2(k, 2, 0)); - linearizationPoint_.insert(L(k), Pose2(0, 0, 0)); - } - - linearizedFactorGraph_ = - *nonlinearFactorGraph_.linearize(linearizationPoint_); - } - - void print() const { - nonlinearFactorGraph_.print(); - linearizationPoint_.print(); - linearizedFactorGraph_.print(); - } - - VectorValues optimize() { - bayesNet_ = *linearizedFactorGraph_.eliminateSequential(); - - // bayesNet->print(); - VectorValues delta = bayesNet_.optimize(); - return delta; - } - - Values linearizationPoint() const { return linearizationPoint_; } - NonlinearFactorGraph nonlinearFactorGraph() const { - return nonlinearFactorGraph_; - } - GaussianFactorGraph linearizedFactorGraph() const { - return linearizedFactorGraph_; - } - GaussianBayesNet bayesNet() const { return bayesNet_; } -}; - -/* ****************************************************************************/ -TEST(Estimation, LR) { - std::vector pims = {Pose2(1, 0, 0)}; - // Leg is in stance throughout - // std::vector fk = {Pose2(0, -2, 0), Pose2(-1, -2, 0)}; - // Leg is in swing - // std::vector fk = {Pose2(0, -1, 0), Pose2(0, -1, 0)}; - // Leg is in toe-off - // std::vector fk = {Pose2(0, -2, 0), Pose2(0, -1, 0)}; - // Leg is in land - std::vector fk = {Pose2(0, -1, 0), Pose2(0, -2, 0)}; - - vector contacts; - contacts = {1, 1}; - SL robot11(2, pims, fk, contacts); - VectorValues delta = robot11.optimize(); - // robot11.nonlinearFactorGraph().print(); - std::cout << "Error with optimized delta: " << robot11.bayesNet().error(delta) - << std::endl; - robot11.linearizationPoint().retract(delta).print(); - std::cout << "\n===========================\n\n" << std::endl; - - contacts = {1, 0}; - SL robot10(2, pims, fk, contacts); - delta = robot10.optimize(); - // robot10.nonlinearFactorGraph().print(); - std::cout << "Error with optimized delta: " << robot10.bayesNet().error(delta) - << std::endl; - robot10.linearizationPoint().retract(delta).print(); - std::cout << "\n===========================\n\n" << std::endl; - - contacts = {0, 1}; - SL robot01(2, pims, fk, contacts); - delta = robot01.optimize(); - // robot01.nonlinearFactorGraph().print(); - std::cout << "Error with optimized delta: " << robot01.bayesNet().error(delta) - << std::endl; - robot01.linearizationPoint().retract(delta).print(); - std::cout << "\n===========================\n\n" << std::endl; - - contacts = {0, 0}; - SL robot00(2, pims, fk, contacts); - delta = robot00.optimize(); - // robot00.nonlinearFactorGraph().print(); - std::cout << "Error with optimized delta: " << robot00.bayesNet().error(delta) - << std::endl; - robot00.linearizationPoint().retract(delta).print(); - std::cout << "\n===========================\n\n" << std::endl; -} - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ From 0faf2226d46ff355e2bf5374d01131d595d3b39e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 13 Oct 2022 11:10:42 -0400 Subject: [PATCH 110/118] remove leftover comment --- gtsam/hybrid/GaussianMixture.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp index 244d52738..5172a9798 100644 --- a/gtsam/hybrid/GaussianMixture.cpp +++ b/gtsam/hybrid/GaussianMixture.cpp @@ -122,7 +122,6 @@ void GaussianMixture::print(const std::string &s, if (gf && !gf->empty()) { gf->print("", formatter); return rd.str(); - // return "Node()"; } else { return "nullptr"; } From cafa3c556c23db08f6eb5242480b18be1cba5253 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 22 Oct 2022 18:37:20 -0700 Subject: [PATCH 111/118] Split SfmTrack into 2 classes --- gtsam/sfm/SfmTrack.h | 62 +++++++++++++++++++++++++------- gtsam/sfm/tests/testSfmTrack.cpp | 53 +++++++++++++++++++++++++++ 2 files changed, 103 insertions(+), 12 deletions(-) create mode 100644 gtsam/sfm/tests/testSfmTrack.cpp diff --git a/gtsam/sfm/SfmTrack.h b/gtsam/sfm/SfmTrack.h index 37731b32a..471eff994 100644 --- a/gtsam/sfm/SfmTrack.h +++ b/gtsam/sfm/SfmTrack.h @@ -35,28 +35,26 @@ typedef std::pair SfmMeasurement; typedef std::pair SiftIndex; /** - * @brief An SfmTrack stores SfM measurements grouped in a track - * @addtogroup sfm + * @brief Track containing 2D measurements associated with a single 3D point. + * Note: Equivalent to gtsam.SfmTrack, but without the 3d measurement. + * This class holds data temporarily before 3D point is initialized. */ -struct GTSAM_EXPORT SfmTrack { - Point3 p; ///< 3D position of the point - float r, g, b; ///< RGB color of the 3D point - +struct GTSAM_EXPORT SfmTrack2d { /// The 2D image projections (id,(u,v)) std::vector measurements; - /// The feature descriptors + /// The feature descriptors (optional) std::vector siftIndices; /// @name Constructors /// @{ - explicit SfmTrack(float r = 0, float g = 0, float b = 0) - : p(0, 0, 0), r(r), g(g), b(b) {} + // Default constructor. + SfmTrack2d() = default; - explicit SfmTrack(const gtsam::Point3& pt, float r = 0, float g = 0, - float b = 0) - : p(pt), r(r), g(g), b(b) {} + // Constructor from measurements. + explicit SfmTrack2d(const std::vector& measurements) + : measurements(measurements) {} /// @} /// @name Standard Interface @@ -78,6 +76,46 @@ struct GTSAM_EXPORT SfmTrack { /// Get the SIFT feature index corresponding to the measurement at `idx` const SiftIndex& siftIndex(size_t idx) const { return siftIndices[idx]; } + /** + * @brief Validates the track by checking that no two measurements are from + * @returns boolean result of the validation. + */ + bool hasUniqueCameras() { + std::vector track_cam_indices; + for (auto& measurement : measurements) { + track_cam_indices.emplace_back(measurement.first); + } + auto i = + std::adjacent_find(track_cam_indices.begin(), track_cam_indices.end()); + bool all_cameras_unique = (i == track_cam_indices.end()); + return all_cameras_unique; + } +}; + +using SfmTrack2dVector = std::vector; + +/** + * @brief An SfmTrack stores SfM measurements grouped in a track + * @addtogroup sfm + */ +struct GTSAM_EXPORT SfmTrack : SfmTrack2d { + Point3 p; ///< 3D position of the point + float r, g, b; ///< RGB color of the 3D point + + /// @name Constructors + /// @{ + + explicit SfmTrack(float r = 0, float g = 0, float b = 0) + : p(0, 0, 0), r(r), g(g), b(b) {} + + explicit SfmTrack(const gtsam::Point3& pt, float r = 0, float g = 0, + float b = 0) + : p(pt), r(r), g(g), b(b) {} + + /// @} + /// @name Standard Interface + /// @{ + /// Get 3D point const Point3& point3() const { return p; } diff --git a/gtsam/sfm/tests/testSfmTrack.cpp b/gtsam/sfm/tests/testSfmTrack.cpp new file mode 100644 index 000000000..1b8c6bd9a --- /dev/null +++ b/gtsam/sfm/tests/testSfmTrack.cpp @@ -0,0 +1,53 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file TestSfmTrack.cpp + * @date October 2022 + * @author Frank Dellaert + * @brief tests for SfmTrack class + */ + +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +TEST(SfmTrack2d, defaultConstructor) { + SfmTrack2d track; + EXPECT_LONGS_EQUAL(0, track.measurements.size()); + EXPECT_LONGS_EQUAL(0, track.siftIndices.size()); +} + +/* ************************************************************************* */ +TEST(SfmTrack2d, measurementConstructor) { + SfmTrack2d track({{0, Point2(1, 2)}, {1, Point2(3, 4)}}); + EXPECT_LONGS_EQUAL(2, track.measurements.size()); + EXPECT_LONGS_EQUAL(0, track.siftIndices.size()); +} + +/* ************************************************************************* */ +TEST(SfmTrack, construction) { + SfmTrack track(Point3(1, 2, 3), 4, 5, 6); + EXPECT(assert_equal(Point3(1, 2, 3), track.point3())); + EXPECT(assert_equal(Point3(4, 5, 6), track.rgb())); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ From 38be12eaf46885d2ae257215582ba616472b5b7c Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 22 Oct 2022 18:37:44 -0700 Subject: [PATCH 112/118] Use SfmMeasurement and SfmTrack --- gtsam/sfm/DsfTrackGenerator.h | 139 ++++++------------- gtsam/sfm/SfmTrack.h | 2 +- gtsam/sfm/sfm.i | 139 +++++++------------ python/gtsam/preamble/sfm.h | 11 +- python/gtsam/specializations/sfm.h | 26 +--- python/gtsam/tests/test_DsfTrackGenerator.py | 77 +++++----- 6 files changed, 144 insertions(+), 250 deletions(-) diff --git a/gtsam/sfm/DsfTrackGenerator.h b/gtsam/sfm/DsfTrackGenerator.h index 47578d6ea..0dae3d216 100644 --- a/gtsam/sfm/DsfTrackGenerator.h +++ b/gtsam/sfm/DsfTrackGenerator.h @@ -17,11 +17,11 @@ */ #pragma once -#include - #include #include +#include +#include #include #include #include @@ -33,19 +33,18 @@ namespace gtsam { namespace gtsfm { typedef DSFMap DSFMapIndexPair; -typedef Eigen::MatrixX2i CorrespondenceIndices; // N x 2 array +typedef Eigen::MatrixX2i CorrespondenceIndices; // N x 2 array -//struct Keypoints; +// struct Keypoints; using KeypointCoordinates = Eigen::MatrixX2d; - // Output of detections in an image. // Coordinate system convention: -// 1. The x coordinate denotes the horizontal direction (+ve direction towards the right). +// 1. The x coordinate denotes the horizontal direction (+ve direction towards +// the right). // 2. The y coordinate denotes the vertical direction (+ve direction downwards). // 3. Origin is at the top left corner of the image. struct Keypoints { - // The (x, y) coordinates of the features, of shape Nx2. KeypointCoordinates coordinates; @@ -56,80 +55,20 @@ struct Keypoints { /// Optional confidences/responses for each detection, of shape N. boost::optional responses; - Keypoints(const gtsam::gtsfm::KeypointCoordinates& coordinates): coordinates(coordinates) {}; // boost::none + Keypoints(const KeypointCoordinates& coordinates) + : coordinates(coordinates){}; // boost::none }; using KeypointsVector = std::vector; -// Mapping from each image pair to (N,2) array representing indices of matching keypoints. +// Mapping from each image pair to (N,2) array representing indices of matching +// keypoints. using MatchIndicesMap = std::map; - -// @param camera index -// @param 2d measurement -// Implemented as named tuple, instead of std::pair (like SfmMeasurement in SfmTrack.h) -struct NamedSfmMeasurement { - size_t i; - gtsam::Point2 uv; - - NamedSfmMeasurement(size_t i, gtsam::Point2 uv) : i(i), uv(uv) {} -}; - - /** - * @brief Track containing 2D measurements associated with a single 3D point. - * Note: Equivalent to gtsam.SfmTrack, but without the 3d measurement. - * This class holds data temporarily before 3D point is initialized. - */ -class SfmTrack2d { - private: - std::vector measurements_; - - public: - // Default constructor. - SfmTrack2d() = default; - - // Constructor from measurements. - SfmTrack2d(std::vector &measurements) : measurements_(measurements) {} - - // Add a measurement to the track. - void addMeasurement(const NamedSfmMeasurement &m) { - measurements_.emplace_back(m); - } - - /// The measurement at index `idx` - NamedSfmMeasurement measurement(size_t idx) const { return measurements_[idx]; } - - // Return all measurements in the track. - std::vector measurements() {return measurements_; } - - /// Total number of measurements in this track. - size_t numberMeasurements() const { return measurements_.size(); } - - // @brief Validates the track by checking that no two measurements are from the same camera. - // - // returns boolean result of the validation. - bool hasUniqueCameras() - { - std::vector track_cam_idxs; - for (auto & measurement : measurements_) - { - track_cam_idxs.emplace_back(measurement.i); - } - auto i = std::adjacent_find(track_cam_idxs.begin(), track_cam_idxs.end()); - bool all_cameras_unique = (i == track_cam_idxs.end()); - return all_cameras_unique; - } -}; - -using SfmTrack2dVector = std::vector; -using NamedSfmMeasurementVector = std::vector; - - -/** - * @brief Generates point tracks from connected components in the keypoint matches graph. + * @brief Generates point tracks from connected components in the keypoint + * matches graph. */ class DsfTrackGenerator { - public: /** Default constructor. */ DsfTrackGenerator() {} @@ -144,28 +83,28 @@ class DsfTrackGenerator { // correspondence indices, from each image. // @param Length-N list of keypoints, for N images/cameras. std::vector generate_tracks_from_pairwise_matches( - const MatchIndicesMap& matches_dict, - const KeypointsVector& keypoints_list, - bool verbose = false) { + const MatchIndicesMap& matches_dict, + const KeypointsVector& keypoints_list, bool verbose = false) { std::vector track_2d_list; - if (verbose) std::cout << "[SfmTrack2d] Starting Union-Find..." << std::endl; + if (verbose) + std::cout << "[SfmTrack2d] Starting Union-Find..." << std::endl; // Generate the DSF to form tracks. DSFMapIndexPair dsf; for (const auto& kv : matches_dict) { - const auto pair_idxs = kv.first; - const auto corr_idxs = kv.second; + const auto pair_indices = kv.first; + const auto corr_indices = kv.second; // Image pair is (i1,i2). - size_t i1 = pair_idxs.first; - size_t i2 = pair_idxs.second; - for (size_t k = 0; k < corr_idxs.rows(); k++) - { + size_t i1 = pair_indices.first; + size_t i2 = pair_indices.second; + for (size_t k = 0; k < corr_indices.rows(); k++) { // Measurement indices are found in a single matrix row, as (k1,k2). - size_t k1 = corr_idxs(k, 0); - size_t k2 = corr_idxs(k, 1); - // Unique keys for the DSF are (i,k), representing keypoint index in an image. + size_t k1 = corr_indices(k, 0); + size_t k2 = corr_indices(k, 1); + // Unique keys for the DSF are (i,k), representing keypoint index in an + // image. dsf.merge(IndexPair(i1, k1), IndexPair(i2, k2)); } } @@ -186,19 +125,19 @@ class DsfTrackGenerator { // Initialize track from measurements. SfmTrack2d track_2d; - for (const auto& index_pair : index_pair_set) - { - // Camera index is represented by i, and measurement index is represented by k. + for (const auto& index_pair : index_pair_set) { + // Camera index is represented by i, and measurement index is + // represented by k. size_t i = index_pair.i(); size_t k = index_pair.j(); // Add measurement to this track. - track_2d.addMeasurement(NamedSfmMeasurement(i, keypoints_list[i].coordinates.row(k))); + track_2d.addMeasurement(i, keypoints_list[i].coordinates.row(k)); } - // Skip erroneous track that had repeated measurements within the same image. - // This is an expected result from an incorrect correspondence slipping through. - if (track_2d.hasUniqueCameras()) - { + // Skip erroneous track that had repeated measurements within the same + // image. This is an expected result from an incorrect correspondence + // slipping through. + if (track_2d.hasUniqueCameras()) { track_2d_list.emplace_back(track_2d); } else { erroneous_track_count++; @@ -206,19 +145,19 @@ class DsfTrackGenerator { } if (verbose) { - double erroneous_track_pct = static_cast(erroneous_track_count) - / static_cast(key_sets.size()) * 100; + double erroneous_track_pct = static_cast(erroneous_track_count) / + static_cast(key_sets.size()) * 100; // TODO(johnwlambert): restrict decimal places to 2 decimals. std::cout << "DSF Union-Find: " << erroneous_track_pct; - std::cout << "% of tracks discarded from multiple obs. in a single image." << std::endl; + std::cout << "% of tracks discarded from multiple obs. in a single image." + << std::endl; } // TODO(johnwlambert): return the Transitivity failure percentage here. return track_2d_list; } }; -}///\namespace gtsfm - -} // namespace gtsam +} // namespace gtsfm +} // namespace gtsam diff --git a/gtsam/sfm/SfmTrack.h b/gtsam/sfm/SfmTrack.h index 471eff994..1fe5aedf4 100644 --- a/gtsam/sfm/SfmTrack.h +++ b/gtsam/sfm/SfmTrack.h @@ -80,7 +80,7 @@ struct GTSAM_EXPORT SfmTrack2d { * @brief Validates the track by checking that no two measurements are from * @returns boolean result of the validation. */ - bool hasUniqueCameras() { + bool hasUniqueCameras() const { std::vector track_cam_indices; for (auto& measurement : measurements) { track_cam_indices.emplace_back(measurement.first); diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index 984955d23..02d3b7b5b 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -4,91 +4,22 @@ namespace gtsam { -namespace gtsfm { - -#include - -class MatchIndicesMap { - MatchIndicesMap(); - MatchIndicesMap(const gtsam::gtsfm::MatchIndicesMap& other); - - size_t size() const; - bool empty() const; - void clear(); - gtsam::gtsfm::CorrespondenceIndices at(const pair& keypair) const; -}; - - -class Keypoints -{ - Keypoints(const gtsam::gtsfm::KeypointCoordinates& coordinates); - gtsam::gtsfm::KeypointCoordinates coordinates; -}; // check if this should be a method - - -class KeypointsVector { - KeypointsVector(); - KeypointsVector(const gtsam::gtsfm::KeypointsVector& other); - void push_back(const gtsam::gtsfm::Keypoints& keypoints); - size_t size() const; - bool empty() const; - void clear(); - gtsam::gtsfm::Keypoints at(const size_t& index) const; -}; - - -class NamedSfmMeasurement -{ - size_t i; - gtsam::Point2 uv; - NamedSfmMeasurement(size_t i, gtsam::Point2 uv); -}; - -class NamedSfmMeasurementVector { - NamedSfmMeasurementVector(); - NamedSfmMeasurementVector(const gtsam::gtsfm::NamedSfmMeasurementVector& other); - void push_back(const gtsam::gtsfm::NamedSfmMeasurement& measurement); - size_t size() const; - bool empty() const; - void clear(); - gtsam::gtsfm::NamedSfmMeasurement at(const size_t& index) const; -}; - +#include class SfmTrack2d { + std::vector> measurements; + SfmTrack2d(); - SfmTrack2d(std::vector &measurements); + SfmTrack2d(std::vector &measurements); size_t numberMeasurements() const; - void addMeasurement(const gtsam::gtsfm::NamedSfmMeasurement &m); - std::vector measurements(); - gtsam::gtsfm::NamedSfmMeasurement measurement(size_t idx) const; + pair measurement(size_t idx) const; + pair siftIndex(size_t idx) const; + void addMeasurement(size_t idx, const gtsam::Point2& m); + gtsam::SfmMeasurement measurement(size_t idx) const; bool hasUniqueCameras(); }; -class SfmTrack2dVector { - SfmTrack2dVector(); - SfmTrack2dVector(const gtsam::gtsfm::SfmTrack2dVector& other); - void push_back(const gtsam::gtsfm::SfmTrack2d& track); - size_t size() const; - bool empty() const; - void clear(); - gtsam::gtsfm::SfmTrack2d at(const size_t& index) const; -}; - - -class DsfTrackGenerator { - DsfTrackGenerator(); - const gtsam::gtsfm::SfmTrack2dVector generate_tracks_from_pairwise_matches( - const gtsam::gtsfm::MatchIndicesMap& matches_dict, - const gtsam::gtsfm::KeypointsVector& keypoints_list, - bool verbose = false); -}; -}///\namespace gtsfm - -#include -#include -#include -class SfmTrack { +virtual class SfmTrack : gtsam::SfmTrack2d { SfmTrack(); SfmTrack(const gtsam::Point3& pt); const Point3& point3() const; @@ -99,13 +30,6 @@ class SfmTrack { double g; double b; - std::vector> measurements; - - size_t numberMeasurements() const; - pair measurement(size_t idx) const; - pair siftIndex(size_t idx) const; - void addMeasurement(size_t idx, const gtsam::Point2& m); - // enabling serialization functionality void serialize() const; @@ -113,6 +37,8 @@ class SfmTrack { bool equals(const gtsam::SfmTrack& expected, double tol) const; }; +#include +#include #include class SfmData { SfmData(); @@ -196,7 +122,7 @@ class BinaryMeasurementsRot3 { #include -// TODO(frank): copy/pasta below until we have integer template paremeters in +// TODO(frank): copy/pasta below until we have integer template parameters in // wrap! class ShonanAveragingParameters2 { @@ -391,4 +317,45 @@ class TranslationRecovery { const gtsam::BinaryMeasurementsUnit3& relativeTranslations) const; }; +namespace gtsfm { + +#include + +class MatchIndicesMap { + MatchIndicesMap(); + MatchIndicesMap(const gtsam::gtsfm::MatchIndicesMap& other); + + size_t size() const; + bool empty() const; + void clear(); + gtsam::gtsfm::CorrespondenceIndices at(const pair& keypair) const; +}; + + +class Keypoints +{ + Keypoints(const gtsam::gtsfm::KeypointCoordinates& coordinates); + gtsam::gtsfm::KeypointCoordinates coordinates; +}; // check if this should be a method + + +class KeypointsVector { + KeypointsVector(); + KeypointsVector(const gtsam::gtsfm::KeypointsVector& other); + void push_back(const gtsam::gtsfm::Keypoints& keypoints); + size_t size() const; + bool empty() const; + void clear(); + gtsam::gtsfm::Keypoints at(const size_t& index) const; +}; + +class DsfTrackGenerator { + DsfTrackGenerator(); + const gtsam::SfmTrack2dVector generate_tracks_from_pairwise_matches( + const gtsam::gtsfm::MatchIndicesMap& matches_dict, + const gtsam::gtsfm::KeypointsVector& keypoints_list, + bool verbose = false); +}; +} // namespace gtsfm + } // namespace gtsam diff --git a/python/gtsam/preamble/sfm.h b/python/gtsam/preamble/sfm.h index 067dd8801..27a4e5de9 100644 --- a/python/gtsam/preamble/sfm.h +++ b/python/gtsam/preamble/sfm.h @@ -15,16 +15,13 @@ // #include #include -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::gtsfm::MatchIndicesMap); -PYBIND11_MAKE_OPAQUE( - std::vector); \ No newline at end of file +PYBIND11_MAKE_OPAQUE(gtsam::gtsfm::MatchIndicesMap); \ No newline at end of file diff --git a/python/gtsam/specializations/sfm.h b/python/gtsam/specializations/sfm.h index 10fb1a0ab..c4817f555 100644 --- a/python/gtsam/specializations/sfm.h +++ b/python/gtsam/specializations/sfm.h @@ -18,25 +18,11 @@ py::bind_vector > >( py::bind_vector > >( m_, "BinaryMeasurementsRot3"); py::bind_map(m_, "KeyPairDoubleMap"); -py::bind_vector< - std::vector >( - m_, "SfmTracks"); -py::bind_vector< - std::vector >( - m_, "SfmCameras"); -py::bind_vector< - std::vector>>( - m_, "SfmMeasurementVector" - ); +py::bind_vector>(m_, "SfmTrack2dVector"); +py::bind_vector>(m_, "SfmTracks"); +py::bind_vector>(m_, "SfmCameras"); +py::bind_vector>>( + m_, "SfmMeasurementVector"); -py::bind_vector< - std::vector >( - m_, "SfmTrack2dVector"); -py::bind_vector< - std::vector >( - m_, "NamedSfmMeasurementVector"); py::bind_map(m_, "MatchIndicesMap"); - -py::bind_vector< - std::vector >( - m_, "KeypointsVector"); +py::bind_vector>(m_, "KeypointsVector"); diff --git a/python/gtsam/tests/test_DsfTrackGenerator.py b/python/gtsam/tests/test_DsfTrackGenerator.py index e63025cd3..3452c03e8 100644 --- a/python/gtsam/tests/test_DsfTrackGenerator.py +++ b/python/gtsam/tests/test_DsfTrackGenerator.py @@ -6,15 +6,9 @@ Authors: John Lambert import unittest import numpy as np - -import gtsam -from gtsam import IndexPair, KeypointsVector, MatchIndicesMap, NamedSfmMeasurementVector -from gtsam.gtsfm import ( - DsfTrackGenerator, - Keypoints, - NamedSfmMeasurement, - SfmTrack2d, -) +from gtsam import (IndexPair, KeypointsVector, MatchIndicesMap, Point2, + SfmMeasurementVector, SfmTrack2d) +from gtsam.gtsfm import DsfTrackGenerator, Keypoints from gtsam.utils.test_case import GtsamTestCase @@ -22,48 +16,55 @@ class TestDsfTrackGenerator(GtsamTestCase): """Tests for DsfTrackGenerator.""" def test_track_generation(self) -> None: - """Ensures that DSF generates three tracks from measurements in 3 images (H=200,W=400).""" - kps_i0 = Keypoints(coordinates=np.array([[10.0, 20], [30, 40]])) - kps_i1 = Keypoints(coordinates=np.array([[50.0, 60], [70, 80], [90, 100]])) - kps_i2 = Keypoints(coordinates=np.array([[110.0, 120], [130, 140]])) + """Ensures that DSF generates three tracks from measurements + in 3 images (H=200,W=400).""" + kps_i0 = Keypoints(np.array([[10.0, 20], [30, 40]])) + kps_i1 = Keypoints(np.array([[50.0, 60], [70, 80], [90, 100]])) + kps_i2 = Keypoints(np.array([[110.0, 120], [130, 140]])) keypoints_list = KeypointsVector() keypoints_list.append(kps_i0) keypoints_list.append(kps_i1) keypoints_list.append(kps_i2) - # For each image pair (i1,i2), we provide a (K,2) matrix of corresponding image indices (k1,k2). + # For each image pair (i1,i2), we provide a (K,2) matrix + # of corresponding image indices (k1,k2). matches_dict = MatchIndicesMap() matches_dict[IndexPair(0, 1)] = np.array([[0, 0], [1, 1]]) matches_dict[IndexPair(1, 2)] = np.array([[2, 0], [1, 1]]) - tracks = DsfTrackGenerator().generate_tracks_from_pairwise_matches(matches_dict, - keypoints_list, - verbose=True) + tracks = DsfTrackGenerator().generate_tracks_from_pairwise_matches( + matches_dict, + keypoints_list, + verbose=False, + ) assert len(tracks) == 3 # Verify track 0. - assert np.allclose(tracks[0].measurements()[0].uv, np.array([10.0, 20.0])) - assert np.allclose(tracks[0].measurements()[1].uv, np.array([50.0, 60.0])) - assert tracks[0].measurements()[0].i == 0 - assert tracks[0].measurements()[1].i == 1 - assert tracks[0].numberMeasurements() == 2 + track0 = tracks[0] + np.testing.assert_allclose(track0.measurements[0][1], Point2(10, 20)) + np.testing.assert_allclose(track0.measurements[1][1], Point2(50, 60)) + assert track0.measurements[0][0] == 0 + assert track0.measurements[1][0] == 1 + assert track0.numberMeasurements() == 2 # Verify track 1. - assert np.allclose(tracks[1].measurements()[0].uv, np.array([30.0, 40.0])) - assert np.allclose(tracks[1].measurements()[1].uv, np.array([70.0, 80.0])) - assert np.allclose(tracks[1].measurements()[2].uv, np.array([130.0, 140.0])) - assert tracks[1].measurements()[0].i == 0 - assert tracks[1].measurements()[1].i == 1 - assert tracks[1].measurements()[2].i == 2 - assert tracks[1].numberMeasurements() == 3 + track1 = tracks[1] + np.testing.assert_allclose(track1.measurements[0][1], Point2(30, 40)) + np.testing.assert_allclose(track1.measurements[1][1], Point2(70, 80)) + np.testing.assert_allclose(track1.measurements[2][1], Point2(130, 140)) + assert track1.measurements[0][0] == 0 + assert track1.measurements[1][0] == 1 + assert track1.measurements[2][0] == 2 + assert track1.numberMeasurements() == 3 # Verify track 2. - assert np.allclose(tracks[2].measurements()[0].uv, np.array([90.0, 100.0])) - assert np.allclose(tracks[2].measurements()[1].uv, np.array([110.0, 120.0])) - assert tracks[2].measurements()[0].i == 1 - assert tracks[2].measurements()[1].i == 2 - assert tracks[2].numberMeasurements() == 2 + track2 = tracks[2] + np.testing.assert_allclose(track2.measurements[0][1], Point2(90, 100)) + np.testing.assert_allclose(track2.measurements[1][1], Point2(110, 120)) + assert track2.measurements[0][0] == 1 + assert track2.measurements[1][0] == 2 + assert track2.numberMeasurements() == 2 class TestSfmTrack2d(GtsamTestCase): @@ -71,8 +72,12 @@ class TestSfmTrack2d(GtsamTestCase): def test_sfm_track_2d_constructor(self) -> None: """ """ - measurements = NamedSfmMeasurementVector() - measurements.append(NamedSfmMeasurement(i=0, uv=np.array([10.0, 20.0]))) + measurements = SfmMeasurementVector() + measurements.append((0, Point2(10, 20))) track = SfmTrack2d(measurements=measurements) track.measurement(0) track.numberMeasurements() == 1 + + +if __name__ == "__main__": + unittest.main() From 7e411fee514ea681f623ccb9a1ce740336704b2f Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 22 Oct 2022 19:18:37 -0700 Subject: [PATCH 113/118] Refactor into smaller functions --- gtsam/sfm/DsfTrackGenerator.h | 128 +++++++++++-------- gtsam/sfm/sfm.i | 2 +- python/gtsam/tests/test_DsfTrackGenerator.py | 2 +- 3 files changed, 77 insertions(+), 55 deletions(-) diff --git a/gtsam/sfm/DsfTrackGenerator.h b/gtsam/sfm/DsfTrackGenerator.h index 0dae3d216..86ae7131b 100644 --- a/gtsam/sfm/DsfTrackGenerator.h +++ b/gtsam/sfm/DsfTrackGenerator.h @@ -71,28 +71,13 @@ using MatchIndicesMap = std::map; class DsfTrackGenerator { public: /** Default constructor. */ - DsfTrackGenerator() {} + DsfTrackGenerator() = default; - // Creates a list of tracks from 2d point correspondences. - // - // Creates a disjoint-set forest (DSF) and 2d tracks from pairwise matches. - // We create a singleton for union-find set elements from camera index of a - // detection and the index of that detection in that camera's keypoint list, - // i.e. (i,k). - // @param Map from (i1,i2) image pair indices to (K,2) matrix, for K - // correspondence indices, from each image. - // @param Length-N list of keypoints, for N images/cameras. - std::vector generate_tracks_from_pairwise_matches( - const MatchIndicesMap& matches_dict, - const KeypointsVector& keypoints_list, bool verbose = false) { - std::vector track_2d_list; - - if (verbose) - std::cout << "[SfmTrack2d] Starting Union-Find..." << std::endl; - // Generate the DSF to form tracks. + /// Generate the DSF to form tracks. + static DSFMapIndexPair generateDSF(const MatchIndicesMap& matches) { DSFMapIndexPair dsf; - for (const auto& kv : matches_dict) { + for (const auto& kv : matches) { const auto pair_indices = kv.first; const auto corr_indices = kv.second; @@ -101,60 +86,97 @@ class DsfTrackGenerator { size_t i2 = pair_indices.second; for (size_t k = 0; k < corr_indices.rows(); k++) { // Measurement indices are found in a single matrix row, as (k1,k2). - size_t k1 = corr_indices(k, 0); - size_t k2 = corr_indices(k, 1); - // Unique keys for the DSF are (i,k), representing keypoint index in an - // image. - dsf.merge(IndexPair(i1, k1), IndexPair(i2, k2)); + size_t k1 = corr_indices(k, 0), k2 = corr_indices(k, 1); + // Unique key for DSF is (i,k), representing keypoint index in an image. + dsf.merge({i1, k1}, {i2, k2}); } } - if (verbose) std::cout << "[SfmTrack2d] Union-Find Complete" << std::endl; + return dsf; + } + + /// Generate a single track from a set of index pairs + static SfmTrack2d trackFromIndexPairs( + const std::set& index_pair_set, + const KeypointsVector& keypoints) { + // Initialize track from measurements. + SfmTrack2d track2d; + + for (const auto& index_pair : index_pair_set) { + // Camera index is represented by i, and measurement index is + // represented by k. + size_t i = index_pair.i(); + size_t k = index_pair.j(); + // Add measurement to this track. + track2d.addMeasurement(i, keypoints[i].coordinates.row(k)); + } + + return track2d; + } + + /// Generate tracks from the DSF. + static std::vector tracksFromDSF( + const DSFMapIndexPair& dsf, const KeypointsVector& keypoints) { const std::map > key_sets = dsf.sets(); // Return immediately if no sets were found. - if (key_sets.empty()) return track_2d_list; + if (key_sets.empty()) return {}; - size_t erroneous_track_count = 0; // Create a list of tracks. // Each track will be represented as a list of (camera_idx, measurements). + std::vector tracks2d; for (const auto& kv : key_sets) { - const auto set_id = kv.first; - const auto index_pair_set = kv.second; - // Initialize track from measurements. - SfmTrack2d track_2d; - - for (const auto& index_pair : index_pair_set) { - // Camera index is represented by i, and measurement index is - // represented by k. - size_t i = index_pair.i(); - size_t k = index_pair.j(); - // Add measurement to this track. - track_2d.addMeasurement(i, keypoints_list[i].coordinates.row(k)); - } - - // Skip erroneous track that had repeated measurements within the same - // image. This is an expected result from an incorrect correspondence - // slipping through. - if (track_2d.hasUniqueCameras()) { - track_2d_list.emplace_back(track_2d); - } else { - erroneous_track_count++; - } + SfmTrack2d track2d = trackFromIndexPairs(kv.second, keypoints); + tracks2d.emplace_back(track2d); } + return tracks2d; + } + + /** + * @brief Creates a list of tracks from 2d point correspondences. + * + * Creates a disjoint-set forest (DSF) and 2d tracks from pairwise matches. + * We create a singleton for union-find set elements from camera index of a + * detection and the index of that detection in that camera's keypoint list, + * i.e. (i,k). + * + * @param Map from (i1,i2) image pair indices to (K,2) matrix, for K + * correspondence indices, from each image. + * @param Length-N list of keypoints, for N images/cameras. + */ + static std::vector tracksFromPairwiseMatches( + const MatchIndicesMap& matches, const KeypointsVector& keypoints, + bool verbose = false) { + // Generate the DSF to form tracks. + if (verbose) + std::cout << "[SfmTrack2d] Starting Union-Find..." << std::endl; + DSFMapIndexPair dsf = generateDSF(matches); + if (verbose) std::cout << "[SfmTrack2d] Union-Find Complete" << std::endl; + + std::vector tracks2d = tracksFromDSF(dsf, keypoints); + + // Filter out erroneous tracks that had repeated measurements within the + // same image. This is an expected result from an incorrect correspondence + // slipping through. + std::vector validTracks; + std::copy_if( + tracks2d.begin(), tracks2d.end(), std::back_inserter(validTracks), + [](const SfmTrack2d& track2d) { return track2d.hasUniqueCameras(); }); if (verbose) { - double erroneous_track_pct = static_cast(erroneous_track_count) / - static_cast(key_sets.size()) * 100; + size_t erroneous_track_count = tracks2d.size() - validTracks.size(); + double erroneous_percentage = static_cast(erroneous_track_count) / + static_cast(tracks2d.size()) * 100; // TODO(johnwlambert): restrict decimal places to 2 decimals. - std::cout << "DSF Union-Find: " << erroneous_track_pct; + std::cout << "DSF Union-Find: " << erroneous_percentage; std::cout << "% of tracks discarded from multiple obs. in a single image." << std::endl; } + // TODO(johnwlambert): return the Transitivity failure percentage here. - return track_2d_list; + return tracks2d; } }; diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index 02d3b7b5b..da054711e 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -351,7 +351,7 @@ class KeypointsVector { class DsfTrackGenerator { DsfTrackGenerator(); - const gtsam::SfmTrack2dVector generate_tracks_from_pairwise_matches( + const gtsam::SfmTrack2dVector tracksFromPairwiseMatches( const gtsam::gtsfm::MatchIndicesMap& matches_dict, const gtsam::gtsfm::KeypointsVector& keypoints_list, bool verbose = false); diff --git a/python/gtsam/tests/test_DsfTrackGenerator.py b/python/gtsam/tests/test_DsfTrackGenerator.py index 3452c03e8..df6e60c9f 100644 --- a/python/gtsam/tests/test_DsfTrackGenerator.py +++ b/python/gtsam/tests/test_DsfTrackGenerator.py @@ -33,7 +33,7 @@ class TestDsfTrackGenerator(GtsamTestCase): matches_dict[IndexPair(0, 1)] = np.array([[0, 0], [1, 1]]) matches_dict[IndexPair(1, 2)] = np.array([[2, 0], [1, 1]]) - tracks = DsfTrackGenerator().generate_tracks_from_pairwise_matches( + tracks = DsfTrackGenerator().tracksFromPairwiseMatches( matches_dict, keypoints_list, verbose=False, From 466290cbdc5d06d1780fcaf72c52e32b6492a630 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 22 Oct 2022 19:30:15 -0700 Subject: [PATCH 114/118] Moved code to cpp file --- gtsam/sfm/DsfTrackGenerator.cpp | 136 ++++++++++++++++++++++++++++++++ gtsam/sfm/DsfTrackGenerator.h | 96 +--------------------- 2 files changed, 137 insertions(+), 95 deletions(-) create mode 100644 gtsam/sfm/DsfTrackGenerator.cpp diff --git a/gtsam/sfm/DsfTrackGenerator.cpp b/gtsam/sfm/DsfTrackGenerator.cpp new file mode 100644 index 000000000..5052bca74 --- /dev/null +++ b/gtsam/sfm/DsfTrackGenerator.cpp @@ -0,0 +1,136 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file DsfTrackGenerator.cpp + * @date October 2022 + * @author John Lambert + * @brief Identifies connected components in the keypoint matches graph. + */ + +#include + +#include +#include + +namespace gtsam { + +namespace gtsfm { + +typedef DSFMap DSFMapIndexPair; + +/// Generate the DSF to form tracks. +static DSFMapIndexPair generateDSF(const MatchIndicesMap& matches) { + DSFMapIndexPair dsf; + + for (const auto& kv : matches) { + const auto pair_indices = kv.first; + const auto corr_indices = kv.second; + + // Image pair is (i1,i2). + size_t i1 = pair_indices.first; + size_t i2 = pair_indices.second; + for (size_t k = 0; k < corr_indices.rows(); k++) { + // Measurement indices are found in a single matrix row, as (k1,k2). + size_t k1 = corr_indices(k, 0), k2 = corr_indices(k, 1); + // Unique key for DSF is (i,k), representing keypoint index in an image. + dsf.merge({i1, k1}, {i2, k2}); + } + } + + return dsf; +} + +/// Generate a single track from a set of index pairs +static SfmTrack2d trackFromIndexPairs(const std::set& index_pair_set, + const KeypointsVector& keypoints) { + // Initialize track from measurements. + SfmTrack2d track2d; + + for (const auto& index_pair : index_pair_set) { + // Camera index is represented by i, and measurement index is + // represented by k. + size_t i = index_pair.i(); + size_t k = index_pair.j(); + // Add measurement to this track. + track2d.addMeasurement(i, keypoints[i].coordinates.row(k)); + } + + return track2d; +} + +/// Generate tracks from the DSF. +static std::vector tracksFromDSF(const DSFMapIndexPair& dsf, + const KeypointsVector& keypoints) { + const std::map > key_sets = dsf.sets(); + + // Return immediately if no sets were found. + if (key_sets.empty()) return {}; + + // Create a list of tracks. + // Each track will be represented as a list of (camera_idx, measurements). + std::vector tracks2d; + for (const auto& kv : key_sets) { + // Initialize track from measurements. + SfmTrack2d track2d = trackFromIndexPairs(kv.second, keypoints); + tracks2d.emplace_back(track2d); + } + return tracks2d; +} + +/** + * @brief Creates a list of tracks from 2d point correspondences. + * + * Creates a disjoint-set forest (DSF) and 2d tracks from pairwise matches. + * We create a singleton for union-find set elements from camera index of a + * detection and the index of that detection in that camera's keypoint list, + * i.e. (i,k). + * + * @param Map from (i1,i2) image pair indices to (K,2) matrix, for K + * correspondence indices, from each image. + * @param Length-N list of keypoints, for N images/cameras. + */ +std::vector DsfTrackGenerator::tracksFromPairwiseMatches( + const MatchIndicesMap& matches, const KeypointsVector& keypoints, + bool verbose) { + // Generate the DSF to form tracks. + if (verbose) std::cout << "[SfmTrack2d] Starting Union-Find..." << std::endl; + DSFMapIndexPair dsf = generateDSF(matches); + if (verbose) std::cout << "[SfmTrack2d] Union-Find Complete" << std::endl; + + std::vector tracks2d = tracksFromDSF(dsf, keypoints); + + // Filter out erroneous tracks that had repeated measurements within the + // same image. This is an expected result from an incorrect correspondence + // slipping through. + std::vector validTracks; + std::copy_if( + tracks2d.begin(), tracks2d.end(), std::back_inserter(validTracks), + [](const SfmTrack2d& track2d) { return track2d.hasUniqueCameras(); }); + + if (verbose) { + size_t erroneous_track_count = tracks2d.size() - validTracks.size(); + double erroneous_percentage = static_cast(erroneous_track_count) / + static_cast(tracks2d.size()) * 100; + + // TODO(johnwlambert): restrict decimal places to 2 decimals. + std::cout << "DSF Union-Find: " << erroneous_percentage; + std::cout << "% of tracks discarded from multiple obs. in a single image." + << std::endl; + } + + // TODO(johnwlambert): return the Transitivity failure percentage here. + return tracks2d; +} + +} // namespace gtsfm + +} // namespace gtsam diff --git a/gtsam/sfm/DsfTrackGenerator.h b/gtsam/sfm/DsfTrackGenerator.h index 86ae7131b..dbf2e6680 100644 --- a/gtsam/sfm/DsfTrackGenerator.h +++ b/gtsam/sfm/DsfTrackGenerator.h @@ -18,12 +18,9 @@ #pragma once #include -#include #include #include -#include -#include #include #include #include @@ -32,7 +29,6 @@ namespace gtsam { namespace gtsfm { -typedef DSFMap DSFMapIndexPair; typedef Eigen::MatrixX2i CorrespondenceIndices; // N x 2 array // struct Keypoints; @@ -73,66 +69,6 @@ class DsfTrackGenerator { /** Default constructor. */ DsfTrackGenerator() = default; - /// Generate the DSF to form tracks. - static DSFMapIndexPair generateDSF(const MatchIndicesMap& matches) { - DSFMapIndexPair dsf; - - for (const auto& kv : matches) { - const auto pair_indices = kv.first; - const auto corr_indices = kv.second; - - // Image pair is (i1,i2). - size_t i1 = pair_indices.first; - size_t i2 = pair_indices.second; - for (size_t k = 0; k < corr_indices.rows(); k++) { - // Measurement indices are found in a single matrix row, as (k1,k2). - size_t k1 = corr_indices(k, 0), k2 = corr_indices(k, 1); - // Unique key for DSF is (i,k), representing keypoint index in an image. - dsf.merge({i1, k1}, {i2, k2}); - } - } - - return dsf; - } - - /// Generate a single track from a set of index pairs - static SfmTrack2d trackFromIndexPairs( - const std::set& index_pair_set, - const KeypointsVector& keypoints) { - // Initialize track from measurements. - SfmTrack2d track2d; - - for (const auto& index_pair : index_pair_set) { - // Camera index is represented by i, and measurement index is - // represented by k. - size_t i = index_pair.i(); - size_t k = index_pair.j(); - // Add measurement to this track. - track2d.addMeasurement(i, keypoints[i].coordinates.row(k)); - } - - return track2d; - } - - /// Generate tracks from the DSF. - static std::vector tracksFromDSF( - const DSFMapIndexPair& dsf, const KeypointsVector& keypoints) { - const std::map > key_sets = dsf.sets(); - - // Return immediately if no sets were found. - if (key_sets.empty()) return {}; - - // Create a list of tracks. - // Each track will be represented as a list of (camera_idx, measurements). - std::vector tracks2d; - for (const auto& kv : key_sets) { - // Initialize track from measurements. - SfmTrack2d track2d = trackFromIndexPairs(kv.second, keypoints); - tracks2d.emplace_back(track2d); - } - return tracks2d; - } - /** * @brief Creates a list of tracks from 2d point correspondences. * @@ -147,37 +83,7 @@ class DsfTrackGenerator { */ static std::vector tracksFromPairwiseMatches( const MatchIndicesMap& matches, const KeypointsVector& keypoints, - bool verbose = false) { - // Generate the DSF to form tracks. - if (verbose) - std::cout << "[SfmTrack2d] Starting Union-Find..." << std::endl; - DSFMapIndexPair dsf = generateDSF(matches); - if (verbose) std::cout << "[SfmTrack2d] Union-Find Complete" << std::endl; - - std::vector tracks2d = tracksFromDSF(dsf, keypoints); - - // Filter out erroneous tracks that had repeated measurements within the - // same image. This is an expected result from an incorrect correspondence - // slipping through. - std::vector validTracks; - std::copy_if( - tracks2d.begin(), tracks2d.end(), std::back_inserter(validTracks), - [](const SfmTrack2d& track2d) { return track2d.hasUniqueCameras(); }); - - if (verbose) { - size_t erroneous_track_count = tracks2d.size() - validTracks.size(); - double erroneous_percentage = static_cast(erroneous_track_count) / - static_cast(tracks2d.size()) * 100; - - // TODO(johnwlambert): restrict decimal places to 2 decimals. - std::cout << "DSF Union-Find: " << erroneous_percentage; - std::cout << "% of tracks discarded from multiple obs. in a single image." - << std::endl; - } - - // TODO(johnwlambert): return the Transitivity failure percentage here. - return tracks2d; - } + bool verbose = false); }; } // namespace gtsfm From ca451141c059387342a7c6ac9ec10b6d59a38732 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 22 Oct 2022 19:48:48 -0700 Subject: [PATCH 115/118] removed NamedSfmMeasurementVector --- python/CMakeLists.txt | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 398ee2788..79a27f17f 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -54,8 +54,7 @@ set(ignore gtsam::KeyPairDoubleMap gtsam::gtsfm::MatchIndicesMap gtsam::gtsfm::KeypointsVector - gtsam::gtsfm::SfmTrack2dVector - gtsam::gtsfm::NamedSfmMeasurementVector) + gtsam::gtsfm::SfmTrack2dVector) set(interface_headers ${PROJECT_SOURCE_DIR}/gtsam/gtsam.i @@ -155,8 +154,7 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) gtsam::KeyPairDoubleMap gtsam::gtsfm::MatchIndicesMap gtsam::gtsfm::KeypointsVector - gtsam::gtsfm::SfmTrack2dVector - gtsam::gtsfm::NamedSfmMeasurementVector) + gtsam::gtsfm::SfmTrack2dVector) pybind_wrap(${GTSAM_PYTHON_UNSTABLE_TARGET} # target From f68f0dbe4142721121c0bacdfb5b8f7a082f6d60 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 23 Oct 2022 11:37:37 -0700 Subject: [PATCH 116/118] Addressed review comments - removed class in favor of free function --- gtsam/sfm/DsfTrackGenerator.cpp | 4 +-- gtsam/sfm/DsfTrackGenerator.h | 37 +++++++------------- gtsam/sfm/SfmTrack.h | 2 +- gtsam/sfm/sfm.i | 22 ++++-------- python/gtsam/tests/test_DsfTrackGenerator.py | 5 +-- 5 files changed, 26 insertions(+), 44 deletions(-) diff --git a/gtsam/sfm/DsfTrackGenerator.cpp b/gtsam/sfm/DsfTrackGenerator.cpp index 5052bca74..e82880193 100644 --- a/gtsam/sfm/DsfTrackGenerator.cpp +++ b/gtsam/sfm/DsfTrackGenerator.cpp @@ -98,7 +98,7 @@ static std::vector tracksFromDSF(const DSFMapIndexPair& dsf, * correspondence indices, from each image. * @param Length-N list of keypoints, for N images/cameras. */ -std::vector DsfTrackGenerator::tracksFromPairwiseMatches( +std::vector tracksFromPairwiseMatches( const MatchIndicesMap& matches, const KeypointsVector& keypoints, bool verbose) { // Generate the DSF to form tracks. @@ -121,7 +121,7 @@ std::vector DsfTrackGenerator::tracksFromPairwiseMatches( double erroneous_percentage = static_cast(erroneous_track_count) / static_cast(tracks2d.size()) * 100; - // TODO(johnwlambert): restrict decimal places to 2 decimals. + std::cout << std::fixed << std::setprecision(2); std::cout << "DSF Union-Find: " << erroneous_percentage; std::cout << "% of tracks discarded from multiple obs. in a single image." << std::endl; diff --git a/gtsam/sfm/DsfTrackGenerator.h b/gtsam/sfm/DsfTrackGenerator.h index dbf2e6680..b02073d8b 100644 --- a/gtsam/sfm/DsfTrackGenerator.h +++ b/gtsam/sfm/DsfTrackGenerator.h @@ -31,7 +31,6 @@ namespace gtsfm { typedef Eigen::MatrixX2i CorrespondenceIndices; // N x 2 array -// struct Keypoints; using KeypointCoordinates = Eigen::MatrixX2d; // Output of detections in an image. @@ -61,30 +60,20 @@ using KeypointsVector = std::vector; using MatchIndicesMap = std::map; /** - * @brief Generates point tracks from connected components in the keypoint - * matches graph. + * @brief Creates a list of tracks from 2d point correspondences. + * + * Creates a disjoint-set forest (DSF) and 2d tracks from pairwise matches. + * We create a singleton for union-find set elements from camera index of a + * detection and the index of that detection in that camera's keypoint list, + * i.e. (i,k). + * + * @param Map from (i1,i2) image pair indices to (K,2) matrix, for K + * correspondence indices, from each image. + * @param Length-N list of keypoints, for N images/cameras. */ -class DsfTrackGenerator { - public: - /** Default constructor. */ - DsfTrackGenerator() = default; - - /** - * @brief Creates a list of tracks from 2d point correspondences. - * - * Creates a disjoint-set forest (DSF) and 2d tracks from pairwise matches. - * We create a singleton for union-find set elements from camera index of a - * detection and the index of that detection in that camera's keypoint list, - * i.e. (i,k). - * - * @param Map from (i1,i2) image pair indices to (K,2) matrix, for K - * correspondence indices, from each image. - * @param Length-N list of keypoints, for N images/cameras. - */ - static std::vector tracksFromPairwiseMatches( - const MatchIndicesMap& matches, const KeypointsVector& keypoints, - bool verbose = false); -}; +std::vector tracksFromPairwiseMatches( + const MatchIndicesMap& matches, const KeypointsVector& keypoints, + bool verbose = false); } // namespace gtsfm diff --git a/gtsam/sfm/SfmTrack.h b/gtsam/sfm/SfmTrack.h index 1fe5aedf4..e5196e750 100644 --- a/gtsam/sfm/SfmTrack.h +++ b/gtsam/sfm/SfmTrack.h @@ -77,7 +77,7 @@ struct GTSAM_EXPORT SfmTrack2d { const SiftIndex& siftIndex(size_t idx) const { return siftIndices[idx]; } /** - * @brief Validates the track by checking that no two measurements are from + * @brief Check that no two measurements are from the same camera. * @returns boolean result of the validation. */ bool hasUniqueCameras() const { diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index da054711e..cbcbb1191 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -5,18 +5,17 @@ namespace gtsam { #include -class SfmTrack2d -{ +class SfmTrack2d { std::vector> measurements; SfmTrack2d(); - SfmTrack2d(std::vector &measurements); + SfmTrack2d(const std::vector& measurements); size_t numberMeasurements() const; pair measurement(size_t idx) const; pair siftIndex(size_t idx) const; void addMeasurement(size_t idx, const gtsam::Point2& m); gtsam::SfmMeasurement measurement(size_t idx) const; - bool hasUniqueCameras(); + bool hasUniqueCameras() const; }; virtual class SfmTrack : gtsam::SfmTrack2d { @@ -331,13 +330,10 @@ class MatchIndicesMap { gtsam::gtsfm::CorrespondenceIndices at(const pair& keypair) const; }; - -class Keypoints -{ +class Keypoints { Keypoints(const gtsam::gtsfm::KeypointCoordinates& coordinates); gtsam::gtsfm::KeypointCoordinates coordinates; -}; // check if this should be a method - +}; class KeypointsVector { KeypointsVector(); @@ -349,13 +345,9 @@ class KeypointsVector { gtsam::gtsfm::Keypoints at(const size_t& index) const; }; -class DsfTrackGenerator { - DsfTrackGenerator(); - const gtsam::SfmTrack2dVector tracksFromPairwiseMatches( +gtsam::SfmTrack2dVector tracksFromPairwiseMatches( const gtsam::gtsfm::MatchIndicesMap& matches_dict, - const gtsam::gtsfm::KeypointsVector& keypoints_list, - bool verbose = false); -}; + const gtsam::gtsfm::KeypointsVector& keypoints_list, bool verbose = false); } // namespace gtsfm } // namespace gtsam diff --git a/python/gtsam/tests/test_DsfTrackGenerator.py b/python/gtsam/tests/test_DsfTrackGenerator.py index df6e60c9f..690fa22e2 100644 --- a/python/gtsam/tests/test_DsfTrackGenerator.py +++ b/python/gtsam/tests/test_DsfTrackGenerator.py @@ -5,10 +5,11 @@ Authors: John Lambert import unittest +import gtsam import numpy as np from gtsam import (IndexPair, KeypointsVector, MatchIndicesMap, Point2, SfmMeasurementVector, SfmTrack2d) -from gtsam.gtsfm import DsfTrackGenerator, Keypoints +from gtsam.gtsfm import Keypoints from gtsam.utils.test_case import GtsamTestCase @@ -33,7 +34,7 @@ class TestDsfTrackGenerator(GtsamTestCase): matches_dict[IndexPair(0, 1)] = np.array([[0, 0], [1, 1]]) matches_dict[IndexPair(1, 2)] = np.array([[2, 0], [1, 1]]) - tracks = DsfTrackGenerator().tracksFromPairwiseMatches( + tracks = gtsam.gtsfm.tracksFromPairwiseMatches( matches_dict, keypoints_list, verbose=False, From 90c990dc3323badb1d7d3cde73c7da3792a5c0da Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 23 Oct 2022 16:26:41 -0700 Subject: [PATCH 117/118] Rename of test --- .../gtsam/tests/{test_dsf_map.py => test_DSFMap.py} | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) rename python/gtsam/tests/{test_dsf_map.py => test_DSFMap.py} (88%) diff --git a/python/gtsam/tests/test_dsf_map.py b/python/gtsam/tests/test_DSFMap.py similarity index 88% rename from python/gtsam/tests/test_dsf_map.py rename to python/gtsam/tests/test_DSFMap.py index 6cae98ff5..f973f7c99 100644 --- a/python/gtsam/tests/test_dsf_map.py +++ b/python/gtsam/tests/test_DSFMap.py @@ -15,8 +15,7 @@ from __future__ import print_function import unittest from typing import Tuple -import gtsam -from gtsam import IndexPair +from gtsam import DSFMapIndexPair, IndexPair, IndexPairSetAsArray from gtsam.utils.test_case import GtsamTestCase @@ -29,10 +28,10 @@ class TestDSFMap(GtsamTestCase): def key(index_pair) -> Tuple[int, int]: return index_pair.i(), index_pair.j() - dsf = gtsam.DSFMapIndexPair() - pair1 = gtsam.IndexPair(1, 18) + dsf = DSFMapIndexPair() + pair1 = IndexPair(1, 18) self.assertEqual(key(dsf.find(pair1)), key(pair1)) - pair2 = gtsam.IndexPair(2, 2) + pair2 = IndexPair(2, 2) # testing the merge feature of dsf dsf.merge(pair1, pair2) @@ -45,7 +44,7 @@ class TestDSFMap(GtsamTestCase): k'th detected keypoint in image i. For the data below, merging such measurements into feature tracks across frames should create 2 distinct sets. """ - dsf = gtsam.DSFMapIndexPair() + dsf = DSFMapIndexPair() dsf.merge(IndexPair(0, 1), IndexPair(1, 2)) dsf.merge(IndexPair(0, 1), IndexPair(3, 4)) dsf.merge(IndexPair(4, 5), IndexPair(6, 8)) @@ -56,7 +55,7 @@ class TestDSFMap(GtsamTestCase): for i in sets: set_keys = [] s = sets[i] - for val in gtsam.IndexPairSetAsArray(s): + for val in IndexPairSetAsArray(s): set_keys.append((val.i(), val.j())) merged_sets.add(tuple(set_keys)) From 32df824920145001ada8495677af43f43fccd6a7 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 23 Oct 2022 16:27:01 -0700 Subject: [PATCH 118/118] Vectorized interface --- gtsam/sfm/DsfTrackGenerator.h | 6 +-- gtsam/sfm/SfmTrack.h | 25 ++++++++++++ gtsam/sfm/sfm.i | 6 ++- python/gtsam/tests/test_DsfTrackGenerator.py | 42 +++++++++++++------- 4 files changed, 58 insertions(+), 21 deletions(-) diff --git a/gtsam/sfm/DsfTrackGenerator.h b/gtsam/sfm/DsfTrackGenerator.h index b02073d8b..14ec2302d 100644 --- a/gtsam/sfm/DsfTrackGenerator.h +++ b/gtsam/sfm/DsfTrackGenerator.h @@ -31,8 +31,6 @@ namespace gtsfm { typedef Eigen::MatrixX2i CorrespondenceIndices; // N x 2 array -using KeypointCoordinates = Eigen::MatrixX2d; - // Output of detections in an image. // Coordinate system convention: // 1. The x coordinate denotes the horizontal direction (+ve direction towards @@ -41,7 +39,7 @@ using KeypointCoordinates = Eigen::MatrixX2d; // 3. Origin is at the top left corner of the image. struct Keypoints { // The (x, y) coordinates of the features, of shape Nx2. - KeypointCoordinates coordinates; + Eigen::MatrixX2d coordinates; // Optional scale of the detections, of shape N. // Note: gtsam::Vector is typedef'd for Eigen::VectorXd. @@ -50,7 +48,7 @@ struct Keypoints { /// Optional confidences/responses for each detection, of shape N. boost::optional responses; - Keypoints(const KeypointCoordinates& coordinates) + Keypoints(const Eigen::MatrixX2d& coordinates) : coordinates(coordinates){}; // boost::none }; diff --git a/gtsam/sfm/SfmTrack.h b/gtsam/sfm/SfmTrack.h index e5196e750..c75199374 100644 --- a/gtsam/sfm/SfmTrack.h +++ b/gtsam/sfm/SfmTrack.h @@ -22,6 +22,7 @@ #include #include +#include #include #include #include @@ -90,6 +91,30 @@ struct GTSAM_EXPORT SfmTrack2d { bool all_cameras_unique = (i == track_cam_indices.end()); return all_cameras_unique; } + + /// @} + /// @name Vectorized Interface + /// @{ + + /// @brief Return the measurements as a 2D matrix + Eigen::MatrixX2d measurementMatrix() const { + Eigen::MatrixX2d m(numberMeasurements(), 2); + for (size_t i = 0; i < numberMeasurements(); i++) { + m.row(i) = measurement(i).second; + } + return m; + } + + /// @brief Return the camera indices of the measurements + Eigen::VectorXi indexVector() const { + Eigen::VectorXi v(numberMeasurements()); + for (size_t i = 0; i < numberMeasurements(); i++) { + v(i) = measurement(i).first; + } + return v; + } + + /// @} }; using SfmTrack2dVector = std::vector; diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index cbcbb1191..26dc20c3e 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -16,6 +16,8 @@ class SfmTrack2d { void addMeasurement(size_t idx, const gtsam::Point2& m); gtsam::SfmMeasurement measurement(size_t idx) const; bool hasUniqueCameras() const; + Eigen::MatrixX2d measurementMatrix() const; + Eigen::VectorXi indexVector() const; }; virtual class SfmTrack : gtsam::SfmTrack2d { @@ -331,8 +333,8 @@ class MatchIndicesMap { }; class Keypoints { - Keypoints(const gtsam::gtsfm::KeypointCoordinates& coordinates); - gtsam::gtsfm::KeypointCoordinates coordinates; + Keypoints(const Eigen::MatrixX2d& coordinates); + Eigen::MatrixX2d coordinates; }; class KeypointsVector { diff --git a/python/gtsam/tests/test_DsfTrackGenerator.py b/python/gtsam/tests/test_DsfTrackGenerator.py index 690fa22e2..e600227c9 100644 --- a/python/gtsam/tests/test_DsfTrackGenerator.py +++ b/python/gtsam/tests/test_DsfTrackGenerator.py @@ -7,8 +7,7 @@ import unittest import gtsam import numpy as np -from gtsam import (IndexPair, KeypointsVector, MatchIndicesMap, Point2, - SfmMeasurementVector, SfmTrack2d) +from gtsam import IndexPair, KeypointsVector, MatchIndicesMap, Point2, SfmMeasurementVector, SfmTrack2d from gtsam.gtsfm import Keypoints from gtsam.utils.test_case import GtsamTestCase @@ -43,29 +42,42 @@ class TestDsfTrackGenerator(GtsamTestCase): # Verify track 0. track0 = tracks[0] + assert track0.numberMeasurements() == 2 np.testing.assert_allclose(track0.measurements[0][1], Point2(10, 20)) np.testing.assert_allclose(track0.measurements[1][1], Point2(50, 60)) assert track0.measurements[0][0] == 0 assert track0.measurements[1][0] == 1 - assert track0.numberMeasurements() == 2 + np.testing.assert_allclose( + track0.measurementMatrix(), + [ + [10, 20], + [50, 60], + ], + ) + np.testing.assert_allclose(track0.indexVector(), [0, 1]) # Verify track 1. track1 = tracks[1] - np.testing.assert_allclose(track1.measurements[0][1], Point2(30, 40)) - np.testing.assert_allclose(track1.measurements[1][1], Point2(70, 80)) - np.testing.assert_allclose(track1.measurements[2][1], Point2(130, 140)) - assert track1.measurements[0][0] == 0 - assert track1.measurements[1][0] == 1 - assert track1.measurements[2][0] == 2 - assert track1.numberMeasurements() == 3 + np.testing.assert_allclose( + track1.measurementMatrix(), + [ + [30, 40], + [70, 80], + [130, 140], + ], + ) + np.testing.assert_allclose(track1.indexVector(), [0, 1, 2]) # Verify track 2. track2 = tracks[2] - np.testing.assert_allclose(track2.measurements[0][1], Point2(90, 100)) - np.testing.assert_allclose(track2.measurements[1][1], Point2(110, 120)) - assert track2.measurements[0][0] == 1 - assert track2.measurements[1][0] == 2 - assert track2.numberMeasurements() == 2 + np.testing.assert_allclose( + track2.measurementMatrix(), + [ + [90, 100], + [110, 120], + ], + ) + np.testing.assert_allclose(track2.indexVector(), [1, 2]) class TestSfmTrack2d(GtsamTestCase):