From 1dd3b180b1781a4be2af3f933e4b180d5cca559d Mon Sep 17 00:00:00 2001 From: Jeffrey Date: Mon, 28 Oct 2024 20:33:06 +0800 Subject: [PATCH 001/112] update testIncrementalFixedLagSmoother.cpp to reproduce the bug in marginalization --- gtsam/inference/VariableIndex.h | 6 +- .../tests/testIncrementalFixedLagSmoother.cpp | 97 ++++++++++++++++++- 2 files changed, 100 insertions(+), 3 deletions(-) diff --git a/gtsam/inference/VariableIndex.h b/gtsam/inference/VariableIndex.h index 207ded0ce..110c0bba4 100644 --- a/gtsam/inference/VariableIndex.h +++ b/gtsam/inference/VariableIndex.h @@ -87,9 +87,11 @@ class GTSAM_EXPORT VariableIndex { const FactorIndices& operator[](Key variable) const { KeyMap::const_iterator item = index_.find(variable); if(item == index_.end()) - throw std::invalid_argument("Requested non-existent variable from VariableIndex"); + throw std::invalid_argument("Requested non-existent variable '" + + DefaultKeyFormatter(variable) + + "' from VariableIndex"); else - return item->second; + return item->second; } /// Return true if no factors associated with a variable diff --git a/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp index 3454c352a..cb4dcbf9a 100644 --- a/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp @@ -49,6 +49,41 @@ bool check_smoother(const NonlinearFactorGraph& fullgraph, const Values& fullini return assert_equal(expected, actual); } +/* ************************************************************************* */ +void PrintSymbolicTreeHelper( + const ISAM2Clique::shared_ptr& clique, const std::string indent = "") { + + // Print the current clique + std::cout << indent << "P( "; + for(Key key: clique->conditional()->frontals()) { + std::cout << DefaultKeyFormatter(key) << " "; + } + if (clique->conditional()->nrParents() > 0) + std::cout << "| "; + for(Key key: clique->conditional()->parents()) { + std::cout << DefaultKeyFormatter(key) << " "; + } + std::cout << ")" << std::endl; + + // Recursively print all of the children + for(const ISAM2Clique::shared_ptr& child: clique->children) { + PrintSymbolicTreeHelper(child, indent + " "); + } +} + +/* ************************************************************************* */ +void PrintSymbolicTree(const ISAM2& isam, + const std::string& label) { + std::cout << label << std::endl; + if (!isam.roots().empty()) { + for(const ISAM2::sharedClique& root: isam.roots()) { + PrintSymbolicTreeHelper(root); + } + } else + std::cout << "{Empty Tree}" << std::endl; +} + + /* ************************************************************************* */ TEST( IncrementalFixedLagSmoother, Example ) { @@ -64,7 +99,7 @@ TEST( IncrementalFixedLagSmoother, Example ) // Create a Fixed-Lag Smoother typedef IncrementalFixedLagSmoother::KeyTimestampMap Timestamps; - IncrementalFixedLagSmoother smoother(7.0, ISAM2Params()); + IncrementalFixedLagSmoother smoother(9.0, ISAM2Params()); // Create containers to keep the full graph Values fullinit; @@ -158,6 +193,9 @@ TEST( IncrementalFixedLagSmoother, Example ) Values newValues; Timestamps newTimestamps; + // Add the odometry factor twice to ensure the removeFactor test below works, + // where we need to keep the connectivity of the graph. + newFactors.push_back(BetweenFactor(key1, key2, Point2(1.0, 0.0), odometerNoise)); newFactors.push_back(BetweenFactor(key1, key2, Point2(1.0, 0.0), odometerNoise)); newValues.insert(key2, Point2(double(i)+0.1, -0.1)); newTimestamps[key2] = double(i); @@ -210,6 +248,10 @@ TEST( IncrementalFixedLagSmoother, Example ) const NonlinearFactorGraph smootherFactorsBeforeRemove = smoother.getFactors(); + std::cout << "fullgraph.size() = " << fullgraph.size() << std::endl; + std::cout << "smootherFactorsBeforeRemove.size() = " + << smootherFactorsBeforeRemove.size() << std::endl; + // remove factor smoother.update(emptyNewFactors, emptyNewValues, emptyNewTimestamps,factorToRemove); @@ -231,6 +273,59 @@ TEST( IncrementalFixedLagSmoother, Example ) } } } + + { + PrintSymbolicTree(smoother.getISAM2(), "Bayes Tree Before marginalization test:"); + + i = 17; + while(i <= 200) { + Key key_0 = MakeKey(i); + Key key_1 = MakeKey(i-1); + Key key_2 = MakeKey(i-2); + Key key_3 = MakeKey(i-3); + Key key_4 = MakeKey(i-4); + Key key_5 = MakeKey(i-5); + Key key_6 = MakeKey(i-6); + Key key_7 = MakeKey(i-7); + Key key_8 = MakeKey(i-8); + + NonlinearFactorGraph newFactors; + Values newValues; + Timestamps newTimestamps; + + // To make a complex graph + newFactors.push_back(BetweenFactor(key_1, key_0, Point2(1.0, 0.0), odometerNoise)); + if (i % 2 == 0) + newFactors.push_back(BetweenFactor(key_2, key_1, Point2(1.0, 0.0), odometerNoise)); + if (i % 3 == 0) + newFactors.push_back(BetweenFactor(key_3, key_2, Point2(1.0, 0.0), odometerNoise)); + if (i % 4 == 0) + newFactors.push_back(BetweenFactor(key_4, key_3, Point2(1.0, 0.0), odometerNoise)); + if (i % 5 == 0) + newFactors.push_back(BetweenFactor(key_5, key_4, Point2(1.0, 0.0), odometerNoise)); + if (i % 6 == 0) + newFactors.push_back(BetweenFactor(key_6, key_5, Point2(1.0, 0.0), odometerNoise)); + if (i % 7 == 0) + newFactors.push_back(BetweenFactor(key_7, key_6, Point2(1.0, 0.0), odometerNoise)); + if (i % 8 == 0) + newFactors.push_back(BetweenFactor(key_8, key_7, Point2(1.0, 0.0), odometerNoise)); + + newValues.insert(key_0, Point2(double(i)+0.1, -0.1)); + newTimestamps[key_0] = double(i); + + fullgraph.push_back(newFactors); + fullinit.insert(newValues); + + // Update the smoother + smoother.update(newFactors, newValues, newTimestamps); + + // Check + CHECK(check_smoother(fullgraph, fullinit, smoother, key_0)); + PrintSymbolicTree(smoother.getISAM2(), "Bayes Tree marginalization test: i = " + std::to_string(i)); + + ++i; + } + } } /* ************************************************************************* */ From 896e52ca27baf823112a10c43d5442318dccd271 Mon Sep 17 00:00:00 2001 From: Jeffrey Date: Mon, 28 Oct 2024 23:38:04 +0800 Subject: [PATCH 002/112] Fix marginalization in IncrementalFixedLagSmoother. Add BayesTreeMarginalizationHelper.h and use the new helper to gather the additional keys to re-eliminate when marginalizing variables in IncrementalFixedLagSmoother. --- .../BayesTreeMarginalizationHelper.h | 174 ++++++++++++++++++ .../nonlinear/IncrementalFixedLagSmoother.cpp | 10 + .../tests/testIncrementalFixedLagSmoother.cpp | 15 +- 3 files changed, 196 insertions(+), 3 deletions(-) create mode 100644 gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h diff --git a/gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h b/gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h new file mode 100644 index 000000000..fe261d10e --- /dev/null +++ b/gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h @@ -0,0 +1,174 @@ +/* ---------------------------------------------------------------------------- + + * 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 BayesTreeMarginalizationHelper.h + * @brief Helper functions for marginalizing variables from a Bayes Tree. + * + * @author Jeffrey (Zhiwei Wang) + * @date Oct 28, 2024 + */ + +// \callgraph +#pragma once + +#include +#include +#include +#include "gtsam_unstable/dllexport.h" + +namespace gtsam { + +/** + * This class provides helper functions for marginalizing variables from a Bayes Tree. + */ +template +class GTSAM_UNSTABLE_EXPORT BayesTreeMarginalizationHelper { + +public: + using Clique = typename BayesTree::Clique; + using sharedClique = typename BayesTree::sharedClique; + + /** Get the additional keys that need to be re-eliminated when marginalizing + * the variables in @p marginalizableKeys from the Bayes tree @p bayesTree. + * + * @param[in] bayesTree The Bayes tree to be marginalized. + * @param[in] marginalizableKeys The keys to be marginalized. + * + * + * When marginalizing a variable @f$ \theta @f$ in a Bayes tree, some nodes + * may need to be re-eliminated. The variable to be marginalized should be + * eliminated first. + * + * 1. If @f$ \theta @f$ is already in a leaf node @f$ L @f$, and all other + * frontal variables within @f$ L @f$ are to be marginalized, then this + * node does not need to be re-eliminated; the entire node can be directly + * marginalized. + * + * 2. If @f$ \theta @f$ is in a leaf node @f$ L @f$, but @f$ L @f$ contains + * other frontal variables that do not need to be marginalized: + * a. If all other non-marginalized frontal variables are listed after + * @f$ \theta @f$ (each node contains a frontal list, with variables to + * be eliminated earlier in the list), then node @f$ L @f$ does not + * need to be re-eliminated. + * b. Otherwise, if there are non-marginalized nodes listed before + * @f$ \theta @f$, then node @f$ L @f$ needs to be re-eliminated, and + * correspondingly, all nodes between @f$ L @f$ and the root need to be + * re-eliminated. + * + * 3. If @f$ \theta @f$ is in an intermediate node @f$ M @f$ (non-leaf node), + * but none of @f$ M @f$'s child nodes depend on variable @f$ \theta @f$ + * (they only depend on other variables within @f$ M @f$), then during the + * process of marginalizing @f$ \theta @f$, @f$ M @f$ can be treated as a + * leaf node, and @f$ M @f$ should be processed following the same + * approach as for leaf nodes. + * + * In this case, the original elimination of @f$ \theta @f$ does not + * depend on the elimination results of variables in the child nodes. + * + * 4. If @f$ \theta @f$ is in an intermediate node @f$ M @f$ (non-leaf node), + * and there exist child nodes that depend on variable @f$ \theta @f$, + * then not only does node @f$ M @f$ need to be re-eliminated, but all + * child nodes dependent on @f$ \theta @f$, including descendant nodes + * recursively dependent on @f$ \theta @f$, also need to be re-eliminated. + * + * The frontal variables in child nodes were originally eliminated before + * @f$ \theta @f$ and their elimination results are relied upon by + * @f$ \theta @f$'s elimination. When re-eliminating, they should be + * eliminated after @f$ \theta @f$. + */ + static void gatherAdditionalKeysToReEliminate( + const BayesTree& bayesTree, + const KeyVector& marginalizableKeys, + std::set& additionalKeys) { + const bool debug = ISDEBUG("BayesTreeMarginalizationHelper"); + + std::set marginalizableKeySet(marginalizableKeys.begin(), marginalizableKeys.end()); + std::set checkedCliques; + + std::set dependentCliques; + for (const Key& key : marginalizableKeySet) { + sharedClique clique = bayesTree[key]; + if (checkedCliques.count(clique)) { + continue; + } + checkedCliques.insert(clique); + + bool is_leaf = clique->children.empty(); + bool need_reeliminate = false; + bool has_non_marginalizable_ahead = false; + for (Key i: clique->conditional()->frontals()) { + if (marginalizableKeySet.count(i)) { + if (has_non_marginalizable_ahead) { + need_reeliminate = true; + break; + } else { + // Check whether there're child nodes dependent on this key. + for(const sharedClique& child: clique->children) { + if (std::find(child->conditional()->beginParents(), + child->conditional()->endParents(), i) + != child->conditional()->endParents()) { + need_reeliminate = true; + break; + } + } + } + } else { + has_non_marginalizable_ahead = true; + } + } + + if (!need_reeliminate) { + continue; + } else { + // need to re-eliminate this clique and all its children that depend on + // a marginalizable key + for (Key i: clique->conditional()->frontals()) { + additionalKeys.insert(i); + for (const sharedClique& child: clique->children) { + if (!dependentCliques.count(child) && + std::find(child->conditional()->beginParents(), + child->conditional()->endParents(), i) + != child->conditional()->endParents()) { + dependentCliques.insert(child); + } + } + } + } + } + + // Recursively add the dependent keys + while (!dependentCliques.empty()) { + auto begin = dependentCliques.begin(); + sharedClique clique = *begin; + dependentCliques.erase(begin); + + for (Key key : clique->conditional()->frontals()) { + additionalKeys.insert(key); + } + + for (const sharedClique& child: clique->children) { + dependentCliques.insert(child); + } + } + + if (debug) { + std::cout << "BayesTreeMarginalizationHelper: Additional keys to re-eliminate: "; + for (const Key& key : additionalKeys) { + std::cout << DefaultKeyFormatter(key) << " "; + } + std::cout << std::endl; + } + } +}; +// BayesTreeMarginalizationHelper + +}/// namespace gtsam diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp index 52e56260d..9d27f5713 100644 --- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp @@ -20,11 +20,15 @@ */ #include +#include #include +// #define GTSAM_OLD_MARGINALIZATION + namespace gtsam { /* ************************************************************************* */ +#ifdef GTSAM_OLD_MARGINALIZATION void recursiveMarkAffectedKeys(const Key& key, const ISAM2Clique::shared_ptr& clique, std::set& additionalKeys) { @@ -45,6 +49,7 @@ void recursiveMarkAffectedKeys(const Key& key, } // If the key was not found in the separator/parents, then none of its children can have it either } +#endif /* ************************************************************************* */ void IncrementalFixedLagSmoother::print(const std::string& s, @@ -116,12 +121,17 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update( // Mark additional keys between the marginalized keys and the leaves std::set additionalKeys; +#ifdef GTSAM_OLD_MARGINALIZATION for(Key key: marginalizableKeys) { ISAM2Clique::shared_ptr clique = isam_[key]; for(const ISAM2Clique::shared_ptr& child: clique->children) { recursiveMarkAffectedKeys(key, child, additionalKeys); } } +#else + BayesTreeMarginalizationHelper::gatherAdditionalKeysToReEliminate( + isam_, marginalizableKeys, additionalKeys); +#endif KeyList additionalMarkedKeys(additionalKeys.begin(), additionalKeys.end()); // Update iSAM2 diff --git a/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp index cb4dcbf9a..cd2ba593b 100644 --- a/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp @@ -99,7 +99,7 @@ TEST( IncrementalFixedLagSmoother, Example ) // Create a Fixed-Lag Smoother typedef IncrementalFixedLagSmoother::KeyTimestampMap Timestamps; - IncrementalFixedLagSmoother smoother(9.0, ISAM2Params()); + IncrementalFixedLagSmoother smoother(12.0, ISAM2Params()); // Create containers to keep the full graph Values fullinit; @@ -226,6 +226,7 @@ TEST( IncrementalFixedLagSmoother, Example ) newFactors.push_back(BetweenFactor(key1, key2, Point2(1.0, 0.0), odometerNoise)); newValues.insert(key2, Point2(double(i)+0.1, -0.1)); newTimestamps[key2] = double(i); + ++i; fullgraph.push_back(newFactors); fullinit.insert(newValues); @@ -275,10 +276,12 @@ TEST( IncrementalFixedLagSmoother, Example ) } { + SETDEBUG("BayesTreeMarginalizationHelper", true); PrintSymbolicTree(smoother.getISAM2(), "Bayes Tree Before marginalization test:"); - i = 17; - while(i <= 200) { + // Do pressure test on marginalization. Enlarge max_i to enhance the test. + const int max_i = 500; + while(i <= max_i) { Key key_0 = MakeKey(i); Key key_1 = MakeKey(i-1); Key key_2 = MakeKey(i-2); @@ -288,6 +291,8 @@ TEST( IncrementalFixedLagSmoother, Example ) Key key_6 = MakeKey(i-6); Key key_7 = MakeKey(i-7); Key key_8 = MakeKey(i-8); + Key key_9 = MakeKey(i-9); + Key key_10 = MakeKey(i-10); NonlinearFactorGraph newFactors; Values newValues; @@ -309,6 +314,10 @@ TEST( IncrementalFixedLagSmoother, Example ) newFactors.push_back(BetweenFactor(key_7, key_6, Point2(1.0, 0.0), odometerNoise)); if (i % 8 == 0) newFactors.push_back(BetweenFactor(key_8, key_7, Point2(1.0, 0.0), odometerNoise)); + if (i % 9 == 0) + newFactors.push_back(BetweenFactor(key_9, key_8, Point2(1.0, 0.0), odometerNoise)); + if (i % 10 == 0) + newFactors.push_back(BetweenFactor(key_10, key_9, Point2(1.0, 0.0), odometerNoise)); newValues.insert(key_0, Point2(double(i)+0.1, -0.1)); newTimestamps[key_0] = double(i); From c6ba2b5fd852e29d16eb1d963b216ea98d3d0ff5 Mon Sep 17 00:00:00 2001 From: Jeffrey Date: Tue, 29 Oct 2024 15:13:09 +0800 Subject: [PATCH 003/112] update doc string in BayesTreeMarginalizationHelper.h --- .../BayesTreeMarginalizationHelper.h | 64 +++++++------------ 1 file changed, 23 insertions(+), 41 deletions(-) diff --git a/gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h b/gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h index fe261d10e..6f5ff425f 100644 --- a/gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h +++ b/gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h @@ -37,53 +37,33 @@ public: using Clique = typename BayesTree::Clique; using sharedClique = typename BayesTree::sharedClique; - /** Get the additional keys that need to be re-eliminated when marginalizing + /** Get the additional keys that need reelimination when marginalizing * the variables in @p marginalizableKeys from the Bayes tree @p bayesTree. * - * @param[in] bayesTree The Bayes tree to be marginalized. + * @param[in] bayesTree The Bayes tree. * @param[in] marginalizableKeys The keys to be marginalized. * * - * When marginalizing a variable @f$ \theta @f$ in a Bayes tree, some nodes - * may need to be re-eliminated. The variable to be marginalized should be - * eliminated first. + * When marginalizing a variable @f$ \theta @f$ from a Bayes tree, some + * nodes may need reelimination to ensure the variables to marginalize + * be eliminated first. * - * 1. If @f$ \theta @f$ is already in a leaf node @f$ L @f$, and all other - * frontal variables within @f$ L @f$ are to be marginalized, then this - * node does not need to be re-eliminated; the entire node can be directly - * marginalized. + * We should consider two cases: * - * 2. If @f$ \theta @f$ is in a leaf node @f$ L @f$, but @f$ L @f$ contains - * other frontal variables that do not need to be marginalized: - * a. If all other non-marginalized frontal variables are listed after - * @f$ \theta @f$ (each node contains a frontal list, with variables to - * be eliminated earlier in the list), then node @f$ L @f$ does not - * need to be re-eliminated. - * b. Otherwise, if there are non-marginalized nodes listed before - * @f$ \theta @f$, then node @f$ L @f$ needs to be re-eliminated, and - * correspondingly, all nodes between @f$ L @f$ and the root need to be - * re-eliminated. + * 1. If a child node relies on @f$ \theta @f$ (i.e., @f$ \theta @f$ + * is a parent / separator of the node), then the frontal + * variables of the child node need to be reeliminated. In + * addition, all the descendants of the child node also need to + * be reeliminated. * - * 3. If @f$ \theta @f$ is in an intermediate node @f$ M @f$ (non-leaf node), - * but none of @f$ M @f$'s child nodes depend on variable @f$ \theta @f$ - * (they only depend on other variables within @f$ M @f$), then during the - * process of marginalizing @f$ \theta @f$, @f$ M @f$ can be treated as a - * leaf node, and @f$ M @f$ should be processed following the same - * approach as for leaf nodes. + * 2. If other frontal variables in the same node with @f$ \theta @f$ + * are in front of @f$ \theta @f$ but not to be marginalized, then + * these variables also need to be reeliminated. * - * In this case, the original elimination of @f$ \theta @f$ does not - * depend on the elimination results of variables in the child nodes. + * These variables were eliminated before @f$ \theta @f$ in the original + * Bayes tree, and after reelimination they will be eliminated after + * @f$ \theta @f$ so that @f$ \theta @f$ can be marginalized safely. * - * 4. If @f$ \theta @f$ is in an intermediate node @f$ M @f$ (non-leaf node), - * and there exist child nodes that depend on variable @f$ \theta @f$, - * then not only does node @f$ M @f$ need to be re-eliminated, but all - * child nodes dependent on @f$ \theta @f$, including descendant nodes - * recursively dependent on @f$ \theta @f$, also need to be re-eliminated. - * - * The frontal variables in child nodes were originally eliminated before - * @f$ \theta @f$ and their elimination results are relied upon by - * @f$ \theta @f$'s elimination. When re-eliminating, they should be - * eliminated after @f$ \theta @f$. */ static void gatherAdditionalKeysToReEliminate( const BayesTree& bayesTree, @@ -102,20 +82,21 @@ public: } checkedCliques.insert(clique); - bool is_leaf = clique->children.empty(); bool need_reeliminate = false; bool has_non_marginalizable_ahead = false; for (Key i: clique->conditional()->frontals()) { if (marginalizableKeySet.count(i)) { if (has_non_marginalizable_ahead) { + // Case 2 in the docstring need_reeliminate = true; break; } else { - // Check whether there're child nodes dependent on this key. + // Check whether there's a child node dependent on this key. for(const sharedClique& child: clique->children) { if (std::find(child->conditional()->beginParents(), child->conditional()->endParents(), i) != child->conditional()->endParents()) { + // Case 1 in the docstring need_reeliminate = true; break; } @@ -127,10 +108,11 @@ public: } if (!need_reeliminate) { + // No variable needs to be reeliminated continue; } else { - // need to re-eliminate this clique and all its children that depend on - // a marginalizable key + // Need to reeliminate the current clique and all its children + // that rely on a marginalizable key. for (Key i: clique->conditional()->frontals()) { additionalKeys.insert(i); for (const sharedClique& child: clique->children) { From 67b0b78ea1eacdb2647fedab828d9dcff99aac10 Mon Sep 17 00:00:00 2001 From: Jeffrey Date: Thu, 31 Oct 2024 18:58:49 +0800 Subject: [PATCH 004/112] Update BayesTreeMarginalizationHelper: 1. Refactor code in BayesTreeMarginalizationHelper; 2. And avoid the unnecessary re-elimination of subtrees that only contain marginalizable variables; --- .../BayesTreeMarginalizationHelper.h | 350 +++++++++++++----- .../nonlinear/IncrementalFixedLagSmoother.cpp | 7 +- 2 files changed, 265 insertions(+), 92 deletions(-) diff --git a/gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h b/gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h index 6f5ff425f..04e4e0cac 100644 --- a/gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h +++ b/gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h @@ -20,6 +20,7 @@ // \callgraph #pragma once +#include #include #include #include @@ -37,109 +38,54 @@ public: using Clique = typename BayesTree::Clique; using sharedClique = typename BayesTree::sharedClique; - /** Get the additional keys that need reelimination when marginalizing - * the variables in @p marginalizableKeys from the Bayes tree @p bayesTree. + /** + * This function identifies variables that need to be re-eliminated before + * performing marginalization. * - * @param[in] bayesTree The Bayes tree. - * @param[in] marginalizableKeys The keys to be marginalized. + * Re-elimination is necessary for a clique containing marginalizable + * variables if: * + * 1. Some non-marginalizable variables appear before marginalizable ones + * in that clique; + * 2. Or it has a child node depending on a marginalizable variable AND the + * subtree rooted at that child contains non-marginalizables. * - * When marginalizing a variable @f$ \theta @f$ from a Bayes tree, some - * nodes may need reelimination to ensure the variables to marginalize - * be eliminated first. - * - * We should consider two cases: - * - * 1. If a child node relies on @f$ \theta @f$ (i.e., @f$ \theta @f$ - * is a parent / separator of the node), then the frontal - * variables of the child node need to be reeliminated. In - * addition, all the descendants of the child node also need to - * be reeliminated. - * - * 2. If other frontal variables in the same node with @f$ \theta @f$ - * are in front of @f$ \theta @f$ but not to be marginalized, then - * these variables also need to be reeliminated. - * - * These variables were eliminated before @f$ \theta @f$ in the original - * Bayes tree, and after reelimination they will be eliminated after - * @f$ \theta @f$ so that @f$ \theta @f$ can be marginalized safely. + * In addition, the subtrees under the aforementioned cliques that require + * re-elimination, which contain non-marginalizable variables in their root + * node, also need to be re-eliminated. * + * @param[in] bayesTree The Bayes tree + * @param[in] marginalizableKeys Keys to be marginalized + * @return Set of additional keys that need to be re-eliminated */ - static void gatherAdditionalKeysToReEliminate( + static std::set gatherAdditionalKeysToReEliminate( const BayesTree& bayesTree, - const KeyVector& marginalizableKeys, - std::set& additionalKeys) { + const KeyVector& marginalizableKeys) { const bool debug = ISDEBUG("BayesTreeMarginalizationHelper"); - std::set marginalizableKeySet(marginalizableKeys.begin(), marginalizableKeys.end()); - std::set checkedCliques; + std::set additionalKeys; + std::set marginalizableKeySet( + marginalizableKeys.begin(), marginalizableKeys.end()); + std::set dependentSubtrees; + CachedSearch cachedSearch; - std::set dependentCliques; - for (const Key& key : marginalizableKeySet) { - sharedClique clique = bayesTree[key]; - if (checkedCliques.count(clique)) { - continue; - } - checkedCliques.insert(clique); + // Check each clique that contains a marginalizable key + for (const sharedClique& clique : + getCliquesContainingKeys(bayesTree, marginalizableKeySet)) { - bool need_reeliminate = false; - bool has_non_marginalizable_ahead = false; - for (Key i: clique->conditional()->frontals()) { - if (marginalizableKeySet.count(i)) { - if (has_non_marginalizable_ahead) { - // Case 2 in the docstring - need_reeliminate = true; - break; - } else { - // Check whether there's a child node dependent on this key. - for(const sharedClique& child: clique->children) { - if (std::find(child->conditional()->beginParents(), - child->conditional()->endParents(), i) - != child->conditional()->endParents()) { - // Case 1 in the docstring - need_reeliminate = true; - break; - } - } - } - } else { - has_non_marginalizable_ahead = true; - } - } + if (needsReelimination(clique, marginalizableKeySet, &cachedSearch)) { + // Add frontal variables from current clique + addCliqueToKeySet(clique, &additionalKeys); - if (!need_reeliminate) { - // No variable needs to be reeliminated - continue; - } else { - // Need to reeliminate the current clique and all its children - // that rely on a marginalizable key. - for (Key i: clique->conditional()->frontals()) { - additionalKeys.insert(i); - for (const sharedClique& child: clique->children) { - if (!dependentCliques.count(child) && - std::find(child->conditional()->beginParents(), - child->conditional()->endParents(), i) - != child->conditional()->endParents()) { - dependentCliques.insert(child); - } - } - } + // Then gather dependent subtrees to be added later + gatherDependentSubtrees( + clique, marginalizableKeySet, &dependentSubtrees, &cachedSearch); } } - // Recursively add the dependent keys - while (!dependentCliques.empty()) { - auto begin = dependentCliques.begin(); - sharedClique clique = *begin; - dependentCliques.erase(begin); - - for (Key key : clique->conditional()->frontals()) { - additionalKeys.insert(key); - } - - for (const sharedClique& child: clique->children) { - dependentCliques.insert(child); - } + // Add the remaining dependent cliques + for (const sharedClique& subtree : dependentSubtrees) { + addSubtreeToKeySet(subtree, &additionalKeys); } if (debug) { @@ -149,6 +95,232 @@ public: } std::cout << std::endl; } + + return additionalKeys; + } + + protected: + + /** + * Gather the cliques containing any of the given keys. + * + * @param[in] bayesTree The Bayes tree + * @param[in] keysOfInterest Set of keys of interest + * @return Set of cliques that contain any of the given keys + */ + static std::set getCliquesContainingKeys( + const BayesTree& bayesTree, + const std::set& keysOfInterest) { + std::set cliques; + for (const Key& key : keysOfInterest) { + cliques.insert(bayesTree[key]); + } + return cliques; + } + + /** + * A struct to cache the results of the below two functions. + */ + struct CachedSearch { + std::unordered_map wholeMarginalizableCliques; + std::unordered_map wholeMarginalizableSubtrees; + }; + + /** + * Check if all variables in the clique are marginalizable. + * + * Note we use a cache map to avoid repeated searches. + */ + static bool isWholeCliqueMarginalizable( + const sharedClique& clique, + const std::set& marginalizableKeys, + CachedSearch* cache) { + auto it = cache->wholeMarginalizableCliques.find(clique.get()); + if (it != cache->wholeMarginalizableCliques.end()) { + return it->second; + } else { + bool ret = true; + for (Key key : clique->conditional()->frontals()) { + if (!marginalizableKeys.count(key)) { + ret = false; + break; + } + } + cache->wholeMarginalizableCliques.insert({clique.get(), ret}); + return ret; + } + } + + /** + * Check if all variables in the subtree are marginalizable. + * + * Note we use a cache map to avoid repeated searches. + */ + static bool isWholeSubtreeMarginalizable( + const sharedClique& subtree, + const std::set& marginalizableKeys, + CachedSearch* cache) { + auto it = cache->wholeMarginalizableSubtrees.find(subtree.get()); + if (it != cache->wholeMarginalizableSubtrees.end()) { + return it->second; + } else { + bool ret = true; + if (isWholeCliqueMarginalizable(subtree, marginalizableKeys, cache)) { + for (const sharedClique& child : subtree->children) { + if (!isWholeSubtreeMarginalizable(child, marginalizableKeys, cache)) { + ret = false; + break; + } + } + } else { + ret = false; + } + cache->wholeMarginalizableSubtrees.insert({subtree.get(), ret}); + return ret; + } + } + + /** + * Check if a clique contains variables that need reelimination due to + * elimination ordering conflicts. + * + * @param[in] clique The clique to check + * @param[in] marginalizableKeys Set of keys to be marginalized + * @return true if any variables in the clique need re-elimination + */ + static bool needsReelimination( + const sharedClique& clique, + const std::set& marginalizableKeys, + CachedSearch* cache) { + bool hasNonMarginalizableAhead = false; + + // Check each frontal variable in order + for (Key key : clique->conditional()->frontals()) { + if (marginalizableKeys.count(key)) { + // If we've seen non-marginalizable variables before this one, + // we need to reeliminate + if (hasNonMarginalizableAhead) { + return true; + } + + // Check if any child depends on this marginalizable key and the + // subtree rooted at that child contains non-marginalizables. + for (const sharedClique& child : clique->children) { + if (hasDependency(child, key) && + !isWholeSubtreeMarginalizable(child, marginalizableKeys, cache)) { + return true; + } + } + } else { + hasNonMarginalizableAhead = true; + } + } + return false; + } + + /** + * Gather all subtrees that depend on a marginalizable key and contain + * non-marginalizable variables in their root. + * + * @param[in] rootClique The starting clique + * @param[in] marginalizableKeys Set of keys to be marginalized + * @param[out] dependentSubtrees Pointer to set storing dependent cliques + */ + static void gatherDependentSubtrees( + const sharedClique& rootClique, + const std::set& marginalizableKeys, + std::set* dependentSubtrees, + CachedSearch* cache) { + for (Key key : rootClique->conditional()->frontals()) { + if (marginalizableKeys.count(key)) { + // Find children that depend on this key + for (const sharedClique& child : rootClique->children) { + if (!dependentSubtrees->count(child) && + hasDependency(child, key)) { + getSubtreesContainingNonMarginalizables( + child, marginalizableKeys, cache, dependentSubtrees); + } + } + } + } + } + + /** + * Gather all subtrees that contain non-marginalizable variables in its root. + */ + static void getSubtreesContainingNonMarginalizables( + const sharedClique& rootClique, + const std::set& marginalizableKeys, + CachedSearch* cache, + std::set* subtreesContainingNonMarginalizables) { + // If the root clique itself contains non-marginalizable variables, we + // just add it to subtreesContainingNonMarginalizables; + if (!isWholeCliqueMarginalizable(rootClique, marginalizableKeys, cache)) { + subtreesContainingNonMarginalizables->insert(rootClique); + return; + } + + // Otherwise, we need to recursively check the children + for (const sharedClique& child : rootClique->children) { + getSubtreesContainingNonMarginalizables( + child, marginalizableKeys, cache, + subtreesContainingNonMarginalizables); + } + } + + /** + * Add all frontal variables from a clique to a key set. + * + * @param[in] clique Clique to add keys from + * @param[out] additionalKeys Pointer to the output key set + */ + static void addCliqueToKeySet( + const sharedClique& clique, + std::set* additionalKeys) { + for (Key key : clique->conditional()->frontals()) { + additionalKeys->insert(key); + } + } + + /** + * Add all frontal variables from a subtree to a key set. + * + * @param[in] subRoot Root clique of the subtree + * @param[out] additionalKeys Pointer to the output key set + */ + static void addSubtreeToKeySet( + const sharedClique& subRoot, + std::set* additionalKeys) { + std::set cliques; + cliques.insert(subRoot); + while(!cliques.empty()) { + auto begin = cliques.begin(); + sharedClique clique = *begin; + cliques.erase(begin); + addCliqueToKeySet(clique, additionalKeys); + for (const sharedClique& child : clique->children) { + cliques.insert(child); + } + } + } + + /** + * Check if the clique depends on the given key. + * + * @param[in] clique Clique to check + * @param[in] key Key to check for dependencies + * @return true if clique depends on the key + */ + static bool hasDependency( + const sharedClique& clique, Key key) { + auto conditional = clique->conditional(); + if (std::find(conditional->beginParents(), + conditional->endParents(), key) + != conditional->endParents()) { + return true; + } else { + return false; + } } }; // BayesTreeMarginalizationHelper diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp index 9d27f5713..afe4fb3de 100644 --- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp @@ -120,8 +120,8 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update( } // Mark additional keys between the marginalized keys and the leaves - std::set additionalKeys; #ifdef GTSAM_OLD_MARGINALIZATION + std::set additionalKeys; for(Key key: marginalizableKeys) { ISAM2Clique::shared_ptr clique = isam_[key]; for(const ISAM2Clique::shared_ptr& child: clique->children) { @@ -129,8 +129,9 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update( } } #else - BayesTreeMarginalizationHelper::gatherAdditionalKeysToReEliminate( - isam_, marginalizableKeys, additionalKeys); + std::set additionalKeys = + BayesTreeMarginalizationHelper::gatherAdditionalKeysToReEliminate( + isam_, marginalizableKeys); #endif KeyList additionalMarkedKeys(additionalKeys.begin(), additionalKeys.end()); From 14c3467520851a0520b4a4ce8f8eadb806c4e82d Mon Sep 17 00:00:00 2001 From: Jeffrey Date: Thu, 31 Oct 2024 19:05:54 +0800 Subject: [PATCH 005/112] Remove old marginalization code in IncrementalFixedLagSmoother.cpp --- .../nonlinear/IncrementalFixedLagSmoother.cpp | 37 ------------------- 1 file changed, 37 deletions(-) diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp index afe4fb3de..238ff6b3d 100644 --- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp @@ -23,34 +23,8 @@ #include #include -// #define GTSAM_OLD_MARGINALIZATION - namespace gtsam { -/* ************************************************************************* */ -#ifdef GTSAM_OLD_MARGINALIZATION -void recursiveMarkAffectedKeys(const Key& key, - const ISAM2Clique::shared_ptr& clique, std::set& additionalKeys) { - - // Check if the separator keys of the current clique contain the specified key - if (std::find(clique->conditional()->beginParents(), - clique->conditional()->endParents(), key) - != clique->conditional()->endParents()) { - - // Mark the frontal keys of the current clique - for(Key i: clique->conditional()->frontals()) { - additionalKeys.insert(i); - } - - // Recursively mark all of the children - for(const ISAM2Clique::shared_ptr& child: clique->children) { - recursiveMarkAffectedKeys(key, child, additionalKeys); - } - } - // If the key was not found in the separator/parents, then none of its children can have it either -} -#endif - /* ************************************************************************* */ void IncrementalFixedLagSmoother::print(const std::string& s, const KeyFormatter& keyFormatter) const { @@ -119,20 +93,9 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update( std::cout << std::endl; } - // Mark additional keys between the marginalized keys and the leaves -#ifdef GTSAM_OLD_MARGINALIZATION - std::set additionalKeys; - for(Key key: marginalizableKeys) { - ISAM2Clique::shared_ptr clique = isam_[key]; - for(const ISAM2Clique::shared_ptr& child: clique->children) { - recursiveMarkAffectedKeys(key, child, additionalKeys); - } - } -#else std::set additionalKeys = BayesTreeMarginalizationHelper::gatherAdditionalKeysToReEliminate( isam_, marginalizableKeys); -#endif KeyList additionalMarkedKeys(additionalKeys.begin(), additionalKeys.end()); // Update iSAM2 From 1a5e711f0e49451b51b2e54757f3b894b8e4ee50 Mon Sep 17 00:00:00 2001 From: Jeffrey Date: Thu, 31 Oct 2024 21:52:45 +0800 Subject: [PATCH 006/112] Further optimize the implementation of BayesTreeMarginalizationHelper: Now we won't re-emilinate any unnecessary nodes (we re-emilinated whole subtrees in the previous commits, which is not optimal) --- .../BayesTreeMarginalizationHelper.h | 124 ++++++++---------- 1 file changed, 58 insertions(+), 66 deletions(-) diff --git a/gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h b/gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h index 04e4e0cac..724814fd0 100644 --- a/gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h +++ b/gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h @@ -21,6 +21,7 @@ #pragma once #include +#include #include #include #include @@ -50,9 +51,12 @@ public: * 2. Or it has a child node depending on a marginalizable variable AND the * subtree rooted at that child contains non-marginalizables. * - * In addition, the subtrees under the aforementioned cliques that require - * re-elimination, which contain non-marginalizable variables in their root - * node, also need to be re-eliminated. + * In addition, for any descendant node depending on a marginalizable + * variable, if the subtree rooted at that descendant contains + * non-marginalizable variables (i.e., it lies on a path from one of the + * aforementioned cliques that require re-elimination to a node containing + * non-marginalizable variables at the leaf side), then it also needs to + * be re-eliminated. * * @param[in] bayesTree The Bayes tree * @param[in] marginalizableKeys Keys to be marginalized @@ -66,7 +70,7 @@ public: std::set additionalKeys; std::set marginalizableKeySet( marginalizableKeys.begin(), marginalizableKeys.end()); - std::set dependentSubtrees; + std::set dependentCliques; CachedSearch cachedSearch; // Check each clique that contains a marginalizable key @@ -77,17 +81,14 @@ public: // Add frontal variables from current clique addCliqueToKeySet(clique, &additionalKeys); - // Then gather dependent subtrees to be added later - gatherDependentSubtrees( - clique, marginalizableKeySet, &dependentSubtrees, &cachedSearch); + // Then add the dependent cliques + for (const sharedClique& dependent : + gatherDependentCliques(clique, marginalizableKeySet, &cachedSearch)) { + addCliqueToKeySet(dependent, &additionalKeys); + } } } - // Add the remaining dependent cliques - for (const sharedClique& subtree : dependentSubtrees) { - addSubtreeToKeySet(subtree, &additionalKeys); - } - if (debug) { std::cout << "BayesTreeMarginalizationHelper: Additional keys to re-eliminate: "; for (const Key& key : additionalKeys) { @@ -219,53 +220,53 @@ public: } /** - * Gather all subtrees that depend on a marginalizable key and contain - * non-marginalizable variables in their root. + * Gather all dependent nodes that lie on a path from the root clique + * to a clique containing a non-marginalizable variable at the leaf side. * - * @param[in] rootClique The starting clique + * @param[in] rootClique The root clique * @param[in] marginalizableKeys Set of keys to be marginalized - * @param[out] dependentSubtrees Pointer to set storing dependent cliques */ - static void gatherDependentSubtrees( + static std::set gatherDependentCliques( const sharedClique& rootClique, const std::set& marginalizableKeys, - std::set* dependentSubtrees, CachedSearch* cache) { - for (Key key : rootClique->conditional()->frontals()) { - if (marginalizableKeys.count(key)) { - // Find children that depend on this key - for (const sharedClique& child : rootClique->children) { - if (!dependentSubtrees->count(child) && - hasDependency(child, key)) { - getSubtreesContainingNonMarginalizables( - child, marginalizableKeys, cache, dependentSubtrees); - } - } + std::vector dependentChildren; + dependentChildren.reserve(rootClique->children.size()); + for (const sharedClique& child : rootClique->children) { + if (hasDependency(child, marginalizableKeys)) { + dependentChildren.push_back(child); } } + return gatherDependentCliquesFromChildren(dependentChildren, marginalizableKeys, cache); } /** - * Gather all subtrees that contain non-marginalizable variables in its root. + * A helper function for the above gatherDependentCliques(). */ - static void getSubtreesContainingNonMarginalizables( - const sharedClique& rootClique, + static std::set gatherDependentCliquesFromChildren( + const std::vector& dependentChildren, const std::set& marginalizableKeys, - CachedSearch* cache, - std::set* subtreesContainingNonMarginalizables) { - // If the root clique itself contains non-marginalizable variables, we - // just add it to subtreesContainingNonMarginalizables; - if (!isWholeCliqueMarginalizable(rootClique, marginalizableKeys, cache)) { - subtreesContainingNonMarginalizables->insert(rootClique); - return; - } + CachedSearch* cache) { + std::deque descendants( + dependentChildren.begin(), dependentChildren.end()); + std::set dependentCliques; + while (!descendants.empty()) { + sharedClique descendant = descendants.front(); + descendants.pop_front(); - // Otherwise, we need to recursively check the children - for (const sharedClique& child : rootClique->children) { - getSubtreesContainingNonMarginalizables( - child, marginalizableKeys, cache, - subtreesContainingNonMarginalizables); + // If the subtree rooted at this descendant contains non-marginalizables, + // it must lie on a path from the root clique to a clique containing + // non-marginalizables at the leaf side. + if (!isWholeSubtreeMarginalizable(descendant, marginalizableKeys, cache)) { + dependentCliques.insert(descendant); + } + + // Add all children of the current descendant to the set descendants. + for (const sharedClique& child : descendant->children) { + descendants.push_back(child); + } } + return dependentCliques; } /** @@ -282,28 +283,6 @@ public: } } - /** - * Add all frontal variables from a subtree to a key set. - * - * @param[in] subRoot Root clique of the subtree - * @param[out] additionalKeys Pointer to the output key set - */ - static void addSubtreeToKeySet( - const sharedClique& subRoot, - std::set* additionalKeys) { - std::set cliques; - cliques.insert(subRoot); - while(!cliques.empty()) { - auto begin = cliques.begin(); - sharedClique clique = *begin; - cliques.erase(begin); - addCliqueToKeySet(clique, additionalKeys); - for (const sharedClique& child : clique->children) { - cliques.insert(child); - } - } - } - /** * Check if the clique depends on the given key. * @@ -322,6 +301,19 @@ public: return false; } } + + /** + * Check if the clique depends on any of the given keys. + */ + static bool hasDependency( + const sharedClique& clique, const std::set& keys) { + for (Key key : keys) { + if (hasDependency(clique, key)) { + return true; + } + } + return false; + } }; // BayesTreeMarginalizationHelper From 0d9c3a99583032e25e36b6135084aaa75159430c Mon Sep 17 00:00:00 2001 From: Jeffrey Date: Thu, 31 Oct 2024 22:23:22 +0800 Subject: [PATCH 007/112] Remove unused variable --- gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h | 1 - 1 file changed, 1 deletion(-) diff --git a/gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h b/gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h index 724814fd0..53d030624 100644 --- a/gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h +++ b/gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h @@ -70,7 +70,6 @@ public: std::set additionalKeys; std::set marginalizableKeySet( marginalizableKeys.begin(), marginalizableKeys.end()); - std::set dependentCliques; CachedSearch cachedSearch; // Check each clique that contains a marginalizable key From 06dac43cae843e5a2bc16447af9d3bde13e0f54d Mon Sep 17 00:00:00 2001 From: Jeffrey Date: Sat, 2 Nov 2024 17:14:01 +0800 Subject: [PATCH 008/112] Some refinement in BayesTreeMarginalizationHelper: 1. Skip subtrees that have already been visited when searching for dependent cliques; 2. Avoid copying shared_ptrs (which needs extra expensive atomic operations) in the searching. Use const Clique* instead of sharedClique whenever possible; 3. Use std::unordered_set instead of std::set to improve average searching speed. --- .../BayesTreeMarginalizationHelper.h | 167 +++++++++++------- .../nonlinear/IncrementalFixedLagSmoother.cpp | 2 +- 2 files changed, 104 insertions(+), 65 deletions(-) diff --git a/gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h b/gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h index 53d030624..1e367e55c 100644 --- a/gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h +++ b/gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h @@ -21,6 +21,7 @@ #pragma once #include +#include #include #include #include @@ -62,30 +63,18 @@ public: * @param[in] marginalizableKeys Keys to be marginalized * @return Set of additional keys that need to be re-eliminated */ - static std::set gatherAdditionalKeysToReEliminate( + static std::unordered_set + gatherAdditionalKeysToReEliminate( const BayesTree& bayesTree, const KeyVector& marginalizableKeys) { const bool debug = ISDEBUG("BayesTreeMarginalizationHelper"); - std::set additionalKeys; - std::set marginalizableKeySet( - marginalizableKeys.begin(), marginalizableKeys.end()); - CachedSearch cachedSearch; + std::unordered_set additionalCliques = + gatherAdditionalCliquesToReEliminate(bayesTree, marginalizableKeys); - // Check each clique that contains a marginalizable key - for (const sharedClique& clique : - getCliquesContainingKeys(bayesTree, marginalizableKeySet)) { - - if (needsReelimination(clique, marginalizableKeySet, &cachedSearch)) { - // Add frontal variables from current clique - addCliqueToKeySet(clique, &additionalKeys); - - // Then add the dependent cliques - for (const sharedClique& dependent : - gatherDependentCliques(clique, marginalizableKeySet, &cachedSearch)) { - addCliqueToKeySet(dependent, &additionalKeys); - } - } + std::unordered_set additionalKeys; + for (const Clique* clique : additionalCliques) { + addCliqueToKeySet(clique, &additionalKeys); } if (debug) { @@ -100,6 +89,41 @@ public: } protected: + /** + * This function identifies cliques that need to be re-eliminated before + * performing marginalization. + * See the docstring of @ref gatherAdditionalKeysToReEliminate(). + */ + static std::unordered_set + gatherAdditionalCliquesToReEliminate( + const BayesTree& bayesTree, + const KeyVector& marginalizableKeys) { + std::unordered_set additionalCliques; + std::unordered_set marginalizableKeySet( + marginalizableKeys.begin(), marginalizableKeys.end()); + CachedSearch cachedSearch; + + // Check each clique that contains a marginalizable key + for (const Clique* clique : + getCliquesContainingKeys(bayesTree, marginalizableKeySet)) { + if (additionalCliques.count(clique)) { + // The clique has already been visited. This can happen when an + // ancestor of the current clique also contain some marginalizable + // varaibles and it's processed beore the current. + continue; + } + + if (needsReelimination(clique, marginalizableKeySet, &cachedSearch)) { + // Add the current clique + additionalCliques.insert(clique); + + // Then add the dependent cliques + gatherDependentCliques(clique, marginalizableKeySet, &additionalCliques, + &cachedSearch); + } + } + return additionalCliques; + } /** * Gather the cliques containing any of the given keys. @@ -108,12 +132,12 @@ public: * @param[in] keysOfInterest Set of keys of interest * @return Set of cliques that contain any of the given keys */ - static std::set getCliquesContainingKeys( + static std::unordered_set getCliquesContainingKeys( const BayesTree& bayesTree, - const std::set& keysOfInterest) { - std::set cliques; + const std::unordered_set& keysOfInterest) { + std::unordered_set cliques; for (const Key& key : keysOfInterest) { - cliques.insert(bayesTree[key]); + cliques.insert(bayesTree[key].get()); } return cliques; } @@ -122,8 +146,8 @@ public: * A struct to cache the results of the below two functions. */ struct CachedSearch { - std::unordered_map wholeMarginalizableCliques; - std::unordered_map wholeMarginalizableSubtrees; + std::unordered_map wholeMarginalizableCliques; + std::unordered_map wholeMarginalizableSubtrees; }; /** @@ -132,10 +156,10 @@ public: * Note we use a cache map to avoid repeated searches. */ static bool isWholeCliqueMarginalizable( - const sharedClique& clique, - const std::set& marginalizableKeys, + const Clique* clique, + const std::unordered_set& marginalizableKeys, CachedSearch* cache) { - auto it = cache->wholeMarginalizableCliques.find(clique.get()); + auto it = cache->wholeMarginalizableCliques.find(clique); if (it != cache->wholeMarginalizableCliques.end()) { return it->second; } else { @@ -146,7 +170,7 @@ public: break; } } - cache->wholeMarginalizableCliques.insert({clique.get(), ret}); + cache->wholeMarginalizableCliques.insert({clique, ret}); return ret; } } @@ -157,17 +181,17 @@ public: * Note we use a cache map to avoid repeated searches. */ static bool isWholeSubtreeMarginalizable( - const sharedClique& subtree, - const std::set& marginalizableKeys, + const Clique* subtree, + const std::unordered_set& marginalizableKeys, CachedSearch* cache) { - auto it = cache->wholeMarginalizableSubtrees.find(subtree.get()); + auto it = cache->wholeMarginalizableSubtrees.find(subtree); if (it != cache->wholeMarginalizableSubtrees.end()) { return it->second; } else { bool ret = true; if (isWholeCliqueMarginalizable(subtree, marginalizableKeys, cache)) { for (const sharedClique& child : subtree->children) { - if (!isWholeSubtreeMarginalizable(child, marginalizableKeys, cache)) { + if (!isWholeSubtreeMarginalizable(child.get(), marginalizableKeys, cache)) { ret = false; break; } @@ -175,7 +199,7 @@ public: } else { ret = false; } - cache->wholeMarginalizableSubtrees.insert({subtree.get(), ret}); + cache->wholeMarginalizableSubtrees.insert({subtree, ret}); return ret; } } @@ -189,8 +213,8 @@ public: * @return true if any variables in the clique need re-elimination */ static bool needsReelimination( - const sharedClique& clique, - const std::set& marginalizableKeys, + const Clique* clique, + const std::unordered_set& marginalizableKeys, CachedSearch* cache) { bool hasNonMarginalizableAhead = false; @@ -206,8 +230,8 @@ public: // Check if any child depends on this marginalizable key and the // subtree rooted at that child contains non-marginalizables. for (const sharedClique& child : clique->children) { - if (hasDependency(child, key) && - !isWholeSubtreeMarginalizable(child, marginalizableKeys, cache)) { + if (hasDependency(child.get(), key) && + !isWholeSubtreeMarginalizable(child.get(), marginalizableKeys, cache)) { return true; } } @@ -225,47 +249,59 @@ public: * @param[in] rootClique The root clique * @param[in] marginalizableKeys Set of keys to be marginalized */ - static std::set gatherDependentCliques( - const sharedClique& rootClique, - const std::set& marginalizableKeys, + static void gatherDependentCliques( + const Clique* rootClique, + const std::unordered_set& marginalizableKeys, + std::unordered_set* additionalCliques, CachedSearch* cache) { - std::vector dependentChildren; + std::vector dependentChildren; dependentChildren.reserve(rootClique->children.size()); for (const sharedClique& child : rootClique->children) { - if (hasDependency(child, marginalizableKeys)) { - dependentChildren.push_back(child); + if (additionalCliques->count(child.get())) { + // This child has already been visited. This can happen if the + // child itself contains a marginalizable variable and it's + // processed before the current rootClique. + continue; + } + if (hasDependency(child.get(), marginalizableKeys)) { + dependentChildren.push_back(child.get()); } } - return gatherDependentCliquesFromChildren(dependentChildren, marginalizableKeys, cache); + gatherDependentCliquesFromChildren( + dependentChildren, marginalizableKeys, additionalCliques, cache); } /** * A helper function for the above gatherDependentCliques(). */ - static std::set gatherDependentCliquesFromChildren( - const std::vector& dependentChildren, - const std::set& marginalizableKeys, + static void gatherDependentCliquesFromChildren( + const std::vector& dependentChildren, + const std::unordered_set& marginalizableKeys, + std::unordered_set* additionalCliques, CachedSearch* cache) { - std::deque descendants( + std::deque descendants( dependentChildren.begin(), dependentChildren.end()); - std::set dependentCliques; while (!descendants.empty()) { - sharedClique descendant = descendants.front(); + const Clique* descendant = descendants.front(); descendants.pop_front(); // If the subtree rooted at this descendant contains non-marginalizables, // it must lie on a path from the root clique to a clique containing // non-marginalizables at the leaf side. if (!isWholeSubtreeMarginalizable(descendant, marginalizableKeys, cache)) { - dependentCliques.insert(descendant); - } + additionalCliques->insert(descendant); - // Add all children of the current descendant to the set descendants. - for (const sharedClique& child : descendant->children) { - descendants.push_back(child); + // Add children of the current descendant to the set descendants. + for (const sharedClique& child : descendant->children) { + if (additionalCliques->count(child.get())) { + // This child has already been visited. + continue; + } else { + descendants.push_back(child.get()); + } + } } } - return dependentCliques; } /** @@ -275,8 +311,8 @@ public: * @param[out] additionalKeys Pointer to the output key set */ static void addCliqueToKeySet( - const sharedClique& clique, - std::set* additionalKeys) { + const Clique* clique, + std::unordered_set* additionalKeys) { for (Key key : clique->conditional()->frontals()) { additionalKeys->insert(key); } @@ -290,8 +326,8 @@ public: * @return true if clique depends on the key */ static bool hasDependency( - const sharedClique& clique, Key key) { - auto conditional = clique->conditional(); + const Clique* clique, Key key) { + auto& conditional = clique->conditional(); if (std::find(conditional->beginParents(), conditional->endParents(), key) != conditional->endParents()) { @@ -305,12 +341,15 @@ public: * Check if the clique depends on any of the given keys. */ static bool hasDependency( - const sharedClique& clique, const std::set& keys) { - for (Key key : keys) { - if (hasDependency(clique, key)) { + const Clique* clique, const std::unordered_set& keys) { + auto& conditional = clique->conditional(); + for (auto it = conditional->beginParents(); + it != conditional->endParents(); ++it) { + if (keys.count(*it)) { return true; } } + return false; } }; diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp index 238ff6b3d..0cd9ecbac 100644 --- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp @@ -93,7 +93,7 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update( std::cout << std::endl; } - std::set additionalKeys = + std::unordered_set additionalKeys = BayesTreeMarginalizationHelper::gatherAdditionalKeysToReEliminate( isam_, marginalizableKeys); KeyList additionalMarkedKeys(additionalKeys.begin(), additionalKeys.end()); From eca2bb5d8a916043608866792c13e7cb86cae10a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 30 Oct 2024 10:27:51 -0700 Subject: [PATCH 009/112] epipolarLine --- gtsam/geometry/FundamentalMatrix.cpp | 28 ++++++++++++++++++++++++++++ gtsam/geometry/FundamentalMatrix.h | 7 +++++++ 2 files changed, 35 insertions(+) diff --git a/gtsam/geometry/FundamentalMatrix.cpp b/gtsam/geometry/FundamentalMatrix.cpp index 5e8c26e62..837ba7263 100644 --- a/gtsam/geometry/FundamentalMatrix.cpp +++ b/gtsam/geometry/FundamentalMatrix.cpp @@ -74,6 +74,20 @@ Matrix3 FundamentalMatrix::matrix() const { V_.transpose().matrix(); } +Vector3 FundamentalMatrix::epipolarLine(const Point2& p, + OptionalJacobian<3, 7> H) { + Vector3 point(p.x(), p.y(), 1); // Create a point in homogeneous coordinates + Vector3 line = matrix() * point; // Compute the epipolar line + + if (H) { + // Compute the Jacobian if requested + throw std::runtime_error( + "FundamentalMatrix::epipolarLine: Jacobian not implemented yet."); + } + + return line; // Return the epipolar line +} + void FundamentalMatrix::print(const std::string& s) const { std::cout << s << matrix() << std::endl; } @@ -116,6 +130,20 @@ Matrix3 SimpleFundamentalMatrix::matrix() const { return Ka().transpose().inverse() * E_.matrix() * Kb().inverse(); } +Vector3 SimpleFundamentalMatrix::epipolarLine(const Point2& p, + OptionalJacobian<3, 7> H) { + Vector3 point(p.x(), p.y(), 1); // Create a point in homogeneous coordinates + Vector3 line = matrix() * point; // Compute the epipolar line + + if (H) { + // Compute the Jacobian if requested + throw std::runtime_error( + "SimpleFundamentalMatrix::epipolarLine: Jacobian not implemented yet."); + } + + return line; // Return the epipolar line +} + void SimpleFundamentalMatrix::print(const std::string& s) const { std::cout << s << " E:\n" << E_.matrix() << "\nfa: " << fa_ << "\nfb: " << fb_ diff --git a/gtsam/geometry/FundamentalMatrix.h b/gtsam/geometry/FundamentalMatrix.h index c114c2b5b..df2c2881a 100644 --- a/gtsam/geometry/FundamentalMatrix.h +++ b/gtsam/geometry/FundamentalMatrix.h @@ -7,6 +7,7 @@ #pragma once +#include #include #include #include @@ -86,6 +87,9 @@ class GTSAM_EXPORT FundamentalMatrix { /// Return the fundamental matrix representation Matrix3 matrix() const; + /// Computes the epipolar line in a (left) for a given point in b (right) + Vector3 epipolarLine(const Point2& p, OptionalJacobian<3, 7> H = {}); + /// @name Testable /// @{ /// Print the FundamentalMatrix @@ -161,6 +165,9 @@ class GTSAM_EXPORT SimpleFundamentalMatrix { /// F = Ka^(-T) * E * Kb^(-1) Matrix3 matrix() const; + /// Computes the epipolar line in a (left) for a given point in b (right) + Vector3 epipolarLine(const Point2& p, OptionalJacobian<3, 7> H = {}); + /// @name Testable /// @{ /// Print the SimpleFundamentalMatrix From 45fc039d075868c9b038b5a3d5aaebf4aba40cba Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 30 Oct 2024 14:54:57 -0700 Subject: [PATCH 010/112] EssentialMatrixFactor5 --- gtsam/slam/EssentialMatrixFactor.h | 147 +++++++++++++++--- .../slam/tests/testEssentialMatrixFactor.cpp | 33 +++- 2 files changed, 150 insertions(+), 30 deletions(-) diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index b19bb09ab..c8f94681c 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -38,7 +38,6 @@ class EssentialMatrixFactor : public NoiseModelFactorN { typedef EssentialMatrixFactor This; public: - // Provide access to the Matrix& version of evaluateError: using Base::evaluateError; @@ -93,8 +92,8 @@ class EssentialMatrixFactor : public NoiseModelFactorN { } /// vector of errors returns 1D vector - Vector evaluateError( - const EssentialMatrix& E, OptionalMatrixType H) const override { + Vector evaluateError(const EssentialMatrix& E, + OptionalMatrixType H) const override { Vector error(1); error << E.error(vA_, vB_, H); return error; @@ -118,7 +117,6 @@ class EssentialMatrixFactor2 typedef EssentialMatrixFactor2 This; public: - // Provide access to the Matrix& version of evaluateError: using Base::evaluateError; @@ -178,9 +176,9 @@ class EssentialMatrixFactor2 * @param E essential matrix * @param d inverse depth d */ - Vector evaluateError( - const EssentialMatrix& E, const double& d, - OptionalMatrixType DE, OptionalMatrixType Dd) const override { + Vector evaluateError(const EssentialMatrix& E, const double& d, + OptionalMatrixType DE, + OptionalMatrixType Dd) const override { // We have point x,y in image 1 // Given a depth Z, the corresponding 3D point P1 = Z*(x,y,1) = (x,y,1)/d // We then convert to second camera by P2 = 1R2'*(P1-1T2) @@ -241,7 +239,6 @@ class EssentialMatrixFactor3 : public EssentialMatrixFactor2 { Rot3 cRb_; ///< Rotation from body to camera frame public: - // Provide access to the Matrix& version of evaluateError: using Base::evaluateError; @@ -292,9 +289,9 @@ class EssentialMatrixFactor3 : public EssentialMatrixFactor2 { * @param E essential matrix * @param d inverse depth d */ - Vector evaluateError( - const EssentialMatrix& E, const double& d, - OptionalMatrixType DE, OptionalMatrixType Dd) const override { + Vector evaluateError(const EssentialMatrix& E, const double& d, + OptionalMatrixType DE, + OptionalMatrixType Dd) const override { if (!DE) { // Convert E from body to camera frame EssentialMatrix cameraE = cRb_ * E; @@ -304,8 +301,9 @@ class EssentialMatrixFactor3 : public EssentialMatrixFactor2 { // Version with derivatives Matrix D_e_cameraE, D_cameraE_E; // 2*5, 5*5 EssentialMatrix cameraE = E.rotate(cRb_, D_cameraE_E); - // Using the pointer version of evaluateError since the Base class (EssentialMatrixFactor2) - // does not have the matrix reference version of evaluateError + // Using the pointer version of evaluateError since the Base class + // (EssentialMatrixFactor2) does not have the matrix reference version of + // evaluateError Vector e = Base::evaluateError(cameraE, d, &D_e_cameraE, Dd); *DE = D_e_cameraE * D_cameraE_E; // (2*5) * (5*5) return e; @@ -327,7 +325,7 @@ class EssentialMatrixFactor3 : public EssentialMatrixFactor2 { * Even with a prior, we can only optimize 2 DoF in the calibration. So the * prior should have a noise model with very low sigma in the remaining * dimensions. This has been tested to work on Cal3_S2. With Cal3Bundler, it - * works only with a strong prior (low sigma noisemodel) on all degrees of + * works only with a strong prior (low sigma noise model) on all degrees of * freedom. */ template @@ -343,13 +341,12 @@ class EssentialMatrixFactor4 typedef Eigen::Matrix JacobianCalibration; public: - - // Provide access to the Matrix& version of evaluateError: + // Provide access to the Matrix& version of evaluateError: using Base::evaluateError; /** * Constructor - * @param keyE Essential Matrix (from camera B to A) variable key + * @param keyE Essential Matrix aEb variable key * @param keyK Calibration variable key (common for both cameras) * @param pA point in first camera, in pixel coordinates * @param pB point in second camera, in pixel coordinates @@ -385,32 +382,32 @@ class EssentialMatrixFactor4 * @param H2 optional jacobian of error w.r.t K * @return * Vector 1D vector of algebraic error */ - Vector evaluateError( - const EssentialMatrix& E, const CALIBRATION& K, - OptionalMatrixType H1, OptionalMatrixType H2) const override { + Vector evaluateError(const EssentialMatrix& E, const CALIBRATION& K, + OptionalMatrixType HE, + OptionalMatrixType HK) const override { // converting from pixel coordinates to normalized coordinates cA and cB JacobianCalibration cA_H_K; // dcA/dK JacobianCalibration cB_H_K; // dcB/dK - Point2 cA = K.calibrate(pA_, H2 ? &cA_H_K : 0, OptionalNone); - Point2 cB = K.calibrate(pB_, H2 ? &cB_H_K : 0, OptionalNone); + Point2 cA = K.calibrate(pA_, HK ? &cA_H_K : 0, OptionalNone); + Point2 cB = K.calibrate(pB_, HK ? &cB_H_K : 0, OptionalNone); // convert to homogeneous coordinates Vector3 vA = EssentialMatrix::Homogeneous(cA); Vector3 vB = EssentialMatrix::Homogeneous(cB); - if (H2) { + if (HK) { // compute the jacobian of error w.r.t K // error function f = vA.T * E * vB // H2 = df/dK = vB.T * E.T * dvA/dK + vA.T * E * dvB/dK // where dvA/dK = dvA/dcA * dcA/dK, dVB/dK = dvB/dcB * dcB/dK // and dvA/dcA = dvB/dcB = [[1, 0], [0, 1], [0, 0]] - *H2 = vB.transpose() * E.matrix().transpose().leftCols<2>() * cA_H_K + + *HK = vB.transpose() * E.matrix().transpose().leftCols<2>() * cA_H_K + vA.transpose() * E.matrix().leftCols<2>() * cB_H_K; } Vector error(1); - error << E.error(vA, vB, H1); + error << E.error(vA, vB, HE); return error; } @@ -420,4 +417,104 @@ class EssentialMatrixFactor4 }; // EssentialMatrixFactor4 +/** + * Binary factor that optimizes for E and two calibrations Ka and Kb using the + * algebraic epipolar error (Ka^-1 pA)'E (Kb^-1 pB). The calibrations are + * assumed different for the two images. Don'tt use this factor with same + * calibration unknown, as Jacobians will be incorrect... + * + * Note: even stronger caveats about having priors on calibration apply. + */ +template +class EssentialMatrixFactor5 + : public NoiseModelFactorN { + private: + Point2 pA_, pB_; ///< points in pixel coordinates + + typedef NoiseModelFactorN Base; + typedef EssentialMatrixFactor5 This; + + static constexpr int DimK = FixedDimension::value; + typedef Eigen::Matrix JacobianCalibration; + + public: + // Provide access to the Matrix& version of evaluateError: + using Base::evaluateError; + + /** + * Constructor + * @param keyE Essential Matrix aEb variable key + * @param keyKa Calibration variable key for camera A + * @param keyKb Calibration variable key for camera B + * @param pA point in first camera, in pixel coordinates + * @param pB point in second camera, in pixel coordinates + * @param model noise model is about dot product in ideal, homogeneous + * coordinates + */ + EssentialMatrixFactor5(Key keyE, Key keyKa, Key keyKb, const Point2& pA, + const Point2& pB, const SharedNoiseModel& model) + : Base(model, keyE, keyKa, keyKb), pA_(pA), pB_(pB) {} + + /// @return a deep copy of this factor + gtsam::NonlinearFactor::shared_ptr clone() const override { + return std::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + /// print + void print( + const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { + Base::print(s); + std::cout << " EssentialMatrixFactor5 with measurements\n (" + << pA_.transpose() << ")' and (" << pB_.transpose() << ")'" + << std::endl; + } + + /** + * @brief Calculate the algebraic epipolar error pA' (Ka^-1)' E Kb pB. + * + * @param E essential matrix for key keyE + * @param Ka calibration for camera A for key keyKa + * @param Kb calibration for camera B for key keyKb + * @param H1 optional jacobian of error w.r.t E + * @param H2 optional jacobian of error w.r.t Ka + * @param H3 optional jacobian of error w.r.t Kb + * @return * Vector 1D vector of algebraic error + */ + Vector evaluateError(const EssentialMatrix& E, const CALIBRATION& Ka, + const CALIBRATION& Kb, OptionalMatrixType HE, + OptionalMatrixType HKa, + OptionalMatrixType HKb) const override { + // converting from pixel coordinates to normalized coordinates cA and cB + JacobianCalibration cA_H_Ka; // dcA/dKa + JacobianCalibration cB_H_Kb; // dcB/dKb + Point2 cA = Ka.calibrate(pA_, HKa ? &cA_H_Ka : 0, OptionalNone); + Point2 cB = Kb.calibrate(pB_, HKb ? &cB_H_Kb : 0, OptionalNone); + + // convert to homogeneous coordinates + Vector3 vA = EssentialMatrix::Homogeneous(cA); + Vector3 vB = EssentialMatrix::Homogeneous(cB); + + if (HKa) { + // compute the jacobian of error w.r.t Ka + *HKa = vB.transpose() * E.matrix().transpose().leftCols<2>() * cA_H_Ka; + } + + if (HKb) { + // compute the jacobian of error w.r.t Kb + *HKb = vA.transpose() * E.matrix().leftCols<2>() * cB_H_Kb; + } + + Vector error(1); + error << E.error(vA, vB, HE); + + return error; + } + + public: + GTSAM_MAKE_ALIGNED_OPERATOR_NEW +}; +// EssentialMatrixFactor5 + } // namespace gtsam diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 0f9c2ef9f..609f38187 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -14,8 +14,8 @@ #include #include #include -#include #include +#include #include using namespace std::placeholders; @@ -101,7 +101,8 @@ TEST(EssentialMatrixFactor, ExpressionFactor) { Key key(1); for (size_t i = 0; i < 5; i++) { std::function)> f = - std::bind(&EssentialMatrix::error, std::placeholders::_1, vA(i), vB(i), std::placeholders::_2); + std::bind(&EssentialMatrix::error, std::placeholders::_1, vA(i), vB(i), + std::placeholders::_2); Expression E_(key); // leaf expression Expression expr(f, E_); // unary expression @@ -127,10 +128,11 @@ TEST(EssentialMatrixFactor, ExpressionFactorRotationOnly) { Key key(1); for (size_t i = 0; i < 5; i++) { std::function)> f = - std::bind(&EssentialMatrix::error, std::placeholders::_1, vA(i), vB(i), std::placeholders::_2); + std::bind(&EssentialMatrix::error, std::placeholders::_1, vA(i), vB(i), + std::placeholders::_2); std::function, - OptionalJacobian<5, 2>)> + OptionalJacobian<5, 3>, + OptionalJacobian<5, 2>)> g; Expression R_(key); Expression d_(trueDirection); @@ -529,6 +531,27 @@ TEST(EssentialMatrixFactor4, minimizationWithStrongCal3BundlerPrior) { 1e-6); } +//************************************************************************* +TEST(EssentialMatrixFactor5, factor) { + Key keyE(1), keyKa(2), keyKb(3); + for (size_t i = 0; i < 5; i++) { + EssentialMatrixFactor5 factor(keyE, keyKa, keyKb, pA(i), pB(i), + model1); + + // Check evaluation + Vector1 expected; + expected << 0; + Vector actual = factor.evaluateError(trueE, trueK, trueK); + EXPECT(assert_equal(expected, actual, 1e-7)); + + Values truth; + truth.insert(keyE, trueE); + truth.insert(keyKa, trueK); + truth.insert(keyKb, trueK); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, truth, 1e-6, 1e-7); + } +} + } // namespace example1 //************************************************************************* From f8d00f82f16e16110b5d0884b56550f24cd35c74 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 30 Oct 2024 16:06:56 -0700 Subject: [PATCH 011/112] Allow for global calibration --- gtsam/sfm/TransferFactor.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/gtsam/sfm/TransferFactor.h b/gtsam/sfm/TransferFactor.h index c8b249ddc..b73a71aa4 100644 --- a/gtsam/sfm/TransferFactor.h +++ b/gtsam/sfm/TransferFactor.h @@ -236,6 +236,8 @@ class EssentialTransferFactorK /** * @brief Constructor that accepts a vector of point triplets. * + * @note Calibrations are assumed all different, keys are derived from edges. + * * @param edge1 First EdgeKey specifying E1: (a, c) or (c, a) * @param edge2 Second EdgeKey specifying E2: (b, c) or (c, b) * @param triplets A vector of triplets containing (pa, pb, pc) From ce196d962fbd62283cda8295cbc93f7cb6722657 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 30 Oct 2024 16:07:22 -0700 Subject: [PATCH 012/112] Wrap EMFs --- gtsam/slam/EssentialMatrixFactor.h | 4 +-- gtsam/slam/slam.i | 50 +++++++++++++++++++++++++++--- 2 files changed, 47 insertions(+), 7 deletions(-) diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index c8f94681c..7a0b46906 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -354,7 +354,7 @@ class EssentialMatrixFactor4 * coordinates */ EssentialMatrixFactor4(Key keyE, Key keyK, const Point2& pA, const Point2& pB, - const SharedNoiseModel& model) + const SharedNoiseModel& model = nullptr) : Base(model, keyE, keyK), pA_(pA), pB_(pB) {} /// @return a deep copy of this factor @@ -452,7 +452,7 @@ class EssentialMatrixFactor5 * coordinates */ EssentialMatrixFactor5(Key keyE, Key keyKa, Key keyKb, const Point2& pA, - const Point2& pB, const SharedNoiseModel& model) + const Point2& pB, const SharedNoiseModel& model = nullptr) : Base(model, keyE, keyKa, keyKb), pA_(pA), pB_(pB) {} /// @return a deep copy of this factor diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index 97dcfcae7..a226f06ec 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -107,7 +107,7 @@ typedef gtsam::GeneralSFMFactor, gtsam::Point3> GeneralSFMFactorPoseCal3Unified; -template virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor { GeneralSFMFactor2(const gtsam::Point2& measured, @@ -221,15 +221,55 @@ typedef gtsam::PoseRotationPrior PoseRotationPrior3D; #include virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor { - EssentialMatrixFactor(size_t key, const gtsam::Point2& pA, - const gtsam::Point2& pB, - const gtsam::noiseModel::Base* noiseModel); + EssentialMatrixFactor(size_t key, + const gtsam::Point2& pA, const gtsam::Point2& pB, + const gtsam::noiseModel::Base* model); void print(string s = "", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::EssentialMatrixFactor& other, double tol) const; gtsam::Vector evaluateError(const gtsam::EssentialMatrix& E) const; }; +virtual class EssentialMatrixFactor2 : gtsam::NoiseModelFactor { + EssentialMatrixFactor2(size_t key1, size_t key2, + const gtsam::Point2& pA, const gtsam::Point2& pB, + const gtsam::noiseModel::Base* model); + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + gtsam::Vector evaluateError(const gtsam::EssentialMatrix& E, double d) const; +}; + +virtual class EssentialMatrixFactor3 : gtsam::EssentialMatrixFactor2 { + EssentialMatrixFactor3(size_t key1, size_t key2, + const gtsam::Point2& pA, const gtsam::Point2& pB, + const gtsam::Rot3& cRb, const gtsam::noiseModel::Base* model); + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + gtsam::Vector evaluateError(const gtsam::EssentialMatrix& E, double d) const; +}; + +template +virtual class EssentialMatrixFactor4 : gtsam::NoiseModelFactor { + EssentialMatrixFactor4(size_t keyE, size_t keyK, + const gtsam::Point2& pA, const gtsam::Point2& pB, + const gtsam::noiseModel::Base* model = nullptr); + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + gtsam::Vector evaluateError(const gtsam::EssentialMatrix& E, const CALIBRATION& K) const; +}; + +template +virtual class EssentialMatrixFactor5 : gtsam::NoiseModelFactor { + EssentialMatrixFactor5(size_t keyE, size_t keyKa, size_t keyKb, + const gtsam::Point2& pA, const gtsam::Point2& pB, + const gtsam::noiseModel::Base* model = nullptr); + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + gtsam::Vector evaluateError(const gtsam::EssentialMatrix& E, + const CALIBRATION& Ka, const CALIBRATION& Kb) const; +}; + #include virtual class EssentialMatrixConstraint : gtsam::NoiseModelFactor { EssentialMatrixConstraint(size_t key1, size_t key2, const gtsam::EssentialMatrix &measuredE, From 360dc4138c9d51f2683d8bc8af51eb692b76dc7e Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 30 Oct 2024 16:07:40 -0700 Subject: [PATCH 013/112] Compare 3 more cases --- python/gtsam/examples/ViewGraphComparison.py | 114 ++++++++++++------- 1 file changed, 70 insertions(+), 44 deletions(-) diff --git a/python/gtsam/examples/ViewGraphComparison.py b/python/gtsam/examples/ViewGraphComparison.py index 1eb43cec8..37558a46c 100644 --- a/python/gtsam/examples/ViewGraphComparison.py +++ b/python/gtsam/examples/ViewGraphComparison.py @@ -32,7 +32,7 @@ from gtsam import ( K = gtsam.symbol_shorthand.K # Methods to compare -methods = ["SimpleF", "Fundamental", "Essential+Ks", "Calibrated"] +methods = ["SimpleF", "Fundamental", "Essential+Ks", "Essential+K", "Calibrated", "Binary+Ks", "Binary+K"] # Formatter function for printing keys @@ -76,8 +76,8 @@ def simulate_data(points, poses, cal, rng, noise_std): return measurements -# Function to compute ground truth matrices def compute_ground_truth(method, poses, cal): + """Function to compute ground truth edge variables.""" E1 = EssentialMatrix.FromPose3(poses[0].between(poses[1])) E2 = EssentialMatrix.FromPose3(poses[0].between(poses[2])) F1 = FundamentalMatrix(cal.K(), E1, cal.K()) @@ -90,58 +90,80 @@ def compute_ground_truth(method, poses, cal): SF1 = SimpleFundamentalMatrix(E1, f, f, c, c) SF2 = SimpleFundamentalMatrix(E2, f, f, c, c) return SF1, SF2 - elif method == "Essential+Ks" or method == "Calibrated": - return E1, E2 else: - raise ValueError(f"Unknown method {method}") + return E1, E2 def build_factor_graph(method, num_cameras, measurements, cal): """build the factor graph""" graph = NonlinearFactorGraph() + # Determine the FactorClass based on the method if method == "Fundamental": FactorClass = gtsam.TransferFactorFundamentalMatrix elif method == "SimpleF": FactorClass = gtsam.TransferFactorSimpleFundamentalMatrix - elif method == "Essential+Ks": + elif method in ["Essential+Ks", "Essential+K"]: FactorClass = gtsam.EssentialTransferFactorKCal3f - # add priors on all calibrations: + elif method == "Binary+K": + FactorClass = gtsam.EssentialMatrixFactor4Cal3f + elif method == "Binary+Ks": + FactorClass = gtsam.EssentialMatrixFactor5Cal3f + elif method == "Calibrated": + FactorClass = gtsam.EssentialTransferFactorCal3f + else: + raise ValueError(f"Unknown method {method}") + + # Add priors on calibrations if necessary + if method in ["Essential+Ks", "Binary+Ks"]: for i in range(num_cameras): model = gtsam.noiseModel.Isotropic.Sigma(1, 10.0) graph.addPriorCal3f(K(i), cal, model) - elif method == "Calibrated": - FactorClass = gtsam.EssentialTransferFactorCal3f - # No priors on calibration needed - else: - raise ValueError(f"Unknown method {method}") + elif method in ["Essential+K", "Binary+K"]: + model = gtsam.noiseModel.Isotropic.Sigma(1, 10.0) + graph.addPriorCal3f(K(0), cal, model) z = measurements # shorthand for a in range(num_cameras): b = (a + 1) % num_cameras # Next camera c = (a + 2) % num_cameras # Camera after next - - # Vectors to collect tuples for each factor - tuples1 = [] - tuples2 = [] - tuples3 = [] - - # Collect data for the three factors - for j in range(len(measurements[0])): - tuples1.append((z[a][j], z[b][j], z[c][j])) - tuples2.append((z[a][j], z[c][j], z[b][j])) - tuples3.append((z[c][j], z[b][j], z[a][j])) - - # Add transfer factors between views a, b, and c. - if method in ["Calibrated"]: - graph.add(FactorClass(EdgeKey(a, c), EdgeKey(b, c), tuples1, cal)) - graph.add(FactorClass(EdgeKey(a, b), EdgeKey(b, c), tuples2, cal)) - graph.add(FactorClass(EdgeKey(a, c), EdgeKey(a, b), tuples3, cal)) + if method in ["Binary+Ks", "Binary+K"]: + # Add binary Essential Matrix factors + ab, ac = EdgeKey(a, b).key(), EdgeKey(a, c).key() + for j in range(len(measurements[0])): + if method == "Binary+Ks": + graph.add(FactorClass(ab, K(a), K(b), z[a][j], z[b][j])) + graph.add(FactorClass(ac, K(a), K(c), z[a][j], z[c][j])) + else: # Binary+K + graph.add(FactorClass(ab, K(0), z[a][j], z[b][j])) + graph.add(FactorClass(ac, K(0), z[a][j], z[c][j])) else: - graph.add(FactorClass(EdgeKey(a, c), EdgeKey(b, c), tuples1)) - graph.add(FactorClass(EdgeKey(a, b), EdgeKey(b, c), tuples2)) - graph.add(FactorClass(EdgeKey(a, c), EdgeKey(a, b), tuples3)) + # Add transfer factors between views a, b, and c + + # Vectors to collect tuples for each factor + tuples1 = [] + tuples2 = [] + tuples3 = [] + + for j in range(len(measurements[0])): + tuples1.append((z[a][j], z[b][j], z[c][j])) + tuples2.append((z[a][j], z[c][j], z[b][j])) + tuples3.append((z[c][j], z[b][j], z[a][j])) + + # Add transfer factors between views a, b, and c. + if method in ["Calibrated"]: + graph.add(FactorClass(EdgeKey(a, c), EdgeKey(b, c), tuples1, cal)) + graph.add(FactorClass(EdgeKey(a, b), EdgeKey(b, c), tuples2, cal)) + graph.add(FactorClass(EdgeKey(a, c), EdgeKey(a, b), tuples3, cal)) + elif method == "Essential+K": + graph.add(FactorClass(EdgeKey(a, c), EdgeKey(b, c), K(0), tuples1)) + graph.add(FactorClass(EdgeKey(a, b), EdgeKey(b, c), K(0), tuples2)) + graph.add(FactorClass(EdgeKey(a, c), EdgeKey(a, b), K(0), tuples3)) + else: + graph.add(FactorClass(EdgeKey(a, c), EdgeKey(b, c), tuples1)) + graph.add(FactorClass(EdgeKey(a, b), EdgeKey(b, c), tuples2)) + graph.add(FactorClass(EdgeKey(a, c), EdgeKey(a, b), tuples3)) return graph @@ -159,22 +181,23 @@ def get_initial_estimate(method, num_cameras, ground_truth, cal): initialEstimate.insert(EdgeKey(a, b).key(), F1) initialEstimate.insert(EdgeKey(a, c).key(), F2) total_dimension += F1.dim() + F2.dim() - elif method in ["Essential+Ks", "Calibrated"]: + elif method in ["Essential+Ks", "Essential+K", "Binary+Ks", "Binary+K", "Calibrated"]: E1, E2 = ground_truth for a in range(num_cameras): - b = (a + 1) % num_cameras # Next camera - c = (a + 2) % num_cameras # Camera after next + b = (a + 1) % num_cameras + c = (a + 2) % num_cameras initialEstimate.insert(EdgeKey(a, b).key(), E1) initialEstimate.insert(EdgeKey(a, c).key(), E2) total_dimension += E1.dim() + E2.dim() - else: - raise ValueError(f"Unknown method {method}") - if method == "Essential+Ks": - # Insert initial calibrations + # Insert initial calibrations + if method in ["Essential+Ks", "Binary+Ks"]: for i in range(num_cameras): initialEstimate.insert(K(i), cal) total_dimension += cal.dim() + elif method in ["Essential+K", "Binary+K"]: + initialEstimate.insert(K(0), cal) + total_dimension += cal.dim() print(f"Total dimension of the problem: {total_dimension}") return initialEstimate @@ -205,7 +228,7 @@ def compute_distances(method, result, ground_truth, num_cameras, cal): key_ab = EdgeKey(a, b).key() key_ac = EdgeKey(a, c).key() - if method in ["Essential+Ks", "Calibrated"]: + if method in ["Essential+Ks", "Essential+K", "Binary+Ks", "Binary+K", "Calibrated"]: E_est_ab = result.atEssentialMatrix(key_ab) E_est_ac = result.atEssentialMatrix(key_ac) @@ -218,15 +241,18 @@ def compute_distances(method, result, ground_truth, num_cameras, cal): SF_est_ac = result.atSimpleFundamentalMatrix(key_ac).matrix() F_est_ab = FundamentalMatrix(SF_est_ab) F_est_ac = FundamentalMatrix(SF_est_ac) - elif method == "Essential+Ks": - # Retrieve calibrations from result: + elif method in ["Essential+Ks", "Binary+Ks"]: + # Retrieve calibrations from result for each camera cal_a = result.atCal3f(K(a)) cal_b = result.atCal3f(K(b)) cal_c = result.atCal3f(K(c)) - - # Convert estimated EssentialMatrices to FundamentalMatrices F_est_ab = FundamentalMatrix(cal_a.K(), E_est_ab, cal_b.K()) F_est_ac = FundamentalMatrix(cal_a.K(), E_est_ac, cal_c.K()) + elif method in ["Essential+K", "Binary+K"]: + # Use single shared calibration + cal_shared = result.atCal3f(K(0)) + F_est_ab = FundamentalMatrix(cal_shared.K(), E_est_ab, cal_shared.K()) + F_est_ac = FundamentalMatrix(cal_shared.K(), E_est_ac, cal_shared.K()) elif method == "Calibrated": # Use ground truth calibration F_est_ab = FundamentalMatrix(cal.K(), E_est_ab, cal.K()) From 183774556851cb098e58c8ab38b180b863d298ba Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 5 Nov 2024 15:06:25 -0500 Subject: [PATCH 014/112] Trying to optimize well --- python/gtsam/examples/ViewGraphComparison.py | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/python/gtsam/examples/ViewGraphComparison.py b/python/gtsam/examples/ViewGraphComparison.py index 37558a46c..7a9f77d09 100644 --- a/python/gtsam/examples/ViewGraphComparison.py +++ b/python/gtsam/examples/ViewGraphComparison.py @@ -117,10 +117,10 @@ def build_factor_graph(method, num_cameras, measurements, cal): # Add priors on calibrations if necessary if method in ["Essential+Ks", "Binary+Ks"]: for i in range(num_cameras): - model = gtsam.noiseModel.Isotropic.Sigma(1, 10.0) + model = gtsam.noiseModel.Isotropic.Sigma(1, 1000.0) graph.addPriorCal3f(K(i), cal, model) elif method in ["Essential+K", "Binary+K"]: - model = gtsam.noiseModel.Isotropic.Sigma(1, 10.0) + model = gtsam.noiseModel.Isotropic.Sigma(1, 1000.0) graph.addPriorCal3f(K(0), cal, model) z = measurements # shorthand @@ -186,6 +186,8 @@ def get_initial_estimate(method, num_cameras, ground_truth, cal): for a in range(num_cameras): b = (a + 1) % num_cameras c = (a + 2) % num_cameras + # initialEstimate.insert(EdgeKey(a, b).key(), E1.retract(0.1 * np.ones((5, 1)))) + # initialEstimate.insert(EdgeKey(a, c).key(), E2.retract(0.1 * np.ones((5, 1)))) initialEstimate.insert(EdgeKey(a, b).key(), E1) initialEstimate.insert(EdgeKey(a, c).key(), E2) total_dimension += E1.dim() + E2.dim() @@ -206,8 +208,9 @@ def get_initial_estimate(method, num_cameras, ground_truth, cal): def optimize(graph, initialEstimate, method): """optimize the graph""" params = LevenbergMarquardtParams() - params.setlambdaInitial(1e10) # Initialize lambda to a high value - params.setlambdaUpperBound(1e10) + if method not in ["Calibrated", "Binary+K", "Binary+Ks"]: + params.setlambdaInitial(1e10) # Initialize lambda to a high value + params.setlambdaUpperBound(1e10) # params.setAbsoluteErrorTol(0.1) params.setVerbosityLM("SUMMARY") optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate, params) @@ -373,6 +376,8 @@ def main(): # Compute final error final_error = graph.error(result) + if method in ["Binary+K", "Binary+Ks"]: + final_error *= cal.f() * cal.f() # Store results results[method]["distances"].extend(distances) From 5f667b44255b0e6181b620833ba8097f596cd0b4 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 5 Nov 2024 17:24:13 -0500 Subject: [PATCH 015/112] Rename factor --- gtsam/sfm/TransferFactor.h | 18 ++++++++++++++++++ gtsam/sfm/sfm.i | 7 +++++-- gtsam/sfm/tests/testTransferFactor.cpp | 2 +- 3 files changed, 24 insertions(+), 3 deletions(-) diff --git a/gtsam/sfm/TransferFactor.h b/gtsam/sfm/TransferFactor.h index b73a71aa4..0338762be 100644 --- a/gtsam/sfm/TransferFactor.h +++ b/gtsam/sfm/TransferFactor.h @@ -253,6 +253,24 @@ class EssentialTransferFactorK TransferEdges(edge1, edge2), triplets_(triplets) {} + /** + * @brief Constructor that accepts a vector of point triplets. + * + * @note Calibrations are assumed all same, using given key `keyK`. + * + * @param edge1 First EdgeKey specifying E1: (a, c) or (c, a) + * @param edge2 Second EdgeKey specifying E2: (b, c) or (c, b) + * @param keyK Calibration key for all views. + * @param triplets A vector of triplets containing (pa, pb, pc) + * @param model An optional SharedNoiseModel + */ + EssentialTransferFactorK(EdgeKey edge1, EdgeKey edge2, Key keyK, + const std::vector& triplets, + const SharedNoiseModel& model = nullptr) + : Base(model, edge1, edge2, keyK, keyK, keyK), + TransferEdges(edge1, edge2), + triplets_(triplets) {} + /// Transfer points pa and pb to view c and evaluate error. Vector2 TransferError(const Matrix3& Eca, const K& Ka, const Point2& pa, const Matrix3& Ecb, const K& Kb, const Point2& pb, diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index a15b73ea1..24f24f1bb 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -98,8 +98,11 @@ virtual class EssentialTransferFactor : gtsam::NoiseModelFactor { template virtual class EssentialTransferFactorK : gtsam::NoiseModelFactor { EssentialTransferFactorK(gtsam::EdgeKey edge1, gtsam::EdgeKey edge2, - const std::vector>& triplets, - const gtsam::noiseModel::Base* model = nullptr); + const std::vector>& triplets, + const gtsam::noiseModel::Base* model = nullptr); + EssentialTransferFactorK(gtsam::EdgeKey edge1, gtsam::EdgeKey edge2, size_t keyK, + const std::vector>& triplets, + const gtsam::noiseModel::Base* model = nullptr); }; #include diff --git a/gtsam/sfm/tests/testTransferFactor.cpp b/gtsam/sfm/tests/testTransferFactor.cpp index ced3d2ce7..b34bdf832 100644 --- a/gtsam/sfm/tests/testTransferFactor.cpp +++ b/gtsam/sfm/tests/testTransferFactor.cpp @@ -154,7 +154,7 @@ TEST(TransferFactor, MultipleTuples) { } //************************************************************************* -// Test for EssentialTransferFactor +// Test for EssentialTransferFactorK TEST(EssentialTransferFactor, Jacobians) { using namespace fixture; From 8b968c14018caab9daa5838a03d0c57829c6eb21 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 6 Nov 2024 09:08:16 -0500 Subject: [PATCH 016/112] use visitWith to not create a new tree --- gtsam/discrete/DecisionTreeFactor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp index 45574f641..4e7a7342e 100644 --- a/gtsam/discrete/DecisionTreeFactor.cpp +++ b/gtsam/discrete/DecisionTreeFactor.cpp @@ -239,7 +239,7 @@ namespace gtsam { }; // Go through the tree - this->apply(op); + this->visitWith(op); return probs; } From 9535ae2fc974a61fe52a0312439c64b6e71cfdad Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Wed, 6 Nov 2024 09:51:06 +0100 Subject: [PATCH 017/112] Add missing #if for clang --- gtsam/linear/VectorValues.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index 7440c0b2b..52e2fac22 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -22,8 +22,10 @@ #include // assert_throw needs a semicolon in Release mode. +#if defined(__clang__) #pragma clang diagnostic push #pragma clang diagnostic ignored "-Wextra-semi-stmt" +#endif namespace gtsam { @@ -417,5 +419,6 @@ namespace gtsam { } // \namespace gtsam +#if defined(__clang__) #pragma clang diagnostic pop - +#endif From d21f191219a945c91546e8d77bb0badc3f877446 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 6 Nov 2024 15:06:46 -0500 Subject: [PATCH 018/112] use a fixed size min-heap to find the pruning threshold --- gtsam/discrete/DecisionTreeFactor.cpp | 57 ++++++++++++++++++++++----- 1 file changed, 48 insertions(+), 9 deletions(-) diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp index 4e7a7342e..9b541bbf0 100644 --- a/gtsam/discrete/DecisionTreeFactor.cpp +++ b/gtsam/discrete/DecisionTreeFactor.cpp @@ -353,18 +353,57 @@ namespace gtsam { DecisionTreeFactor DecisionTreeFactor::prune(size_t maxNrAssignments) const { const size_t N = maxNrAssignments; - // Get the probabilities in the decision tree so we can threshold. - std::vector probabilities = this->probabilities(); + // Set of all keys + std::set allKeys(keys().begin(), keys().end()); + std::vector min_heap; - // The number of probabilities can be lower than max_leaves - if (probabilities.size() <= N) { - return *this; - } + auto op = [&](const Assignment& a, double p) { + // Get all the keys in the current assignment + std::set assignment_keys; + for (auto&& [k, _] : a) { + assignment_keys.insert(k); + } - std::sort(probabilities.begin(), probabilities.end(), - std::greater{}); + // Find the keys missing in the assignment + std::vector diff; + std::set_difference(allKeys.begin(), allKeys.end(), + assignment_keys.begin(), assignment_keys.end(), + std::back_inserter(diff)); - double threshold = probabilities[N - 1]; + // Compute the total number of assignments in the (pruned) subtree + size_t nrAssignments = 1; + for (auto&& k : diff) { + nrAssignments *= cardinalities_.at(k); + } + + if (min_heap.empty()) { + for (size_t i = 0; i < std::min(nrAssignments, N); ++i) { + min_heap.push_back(p); + } + std::make_heap(min_heap.begin(), min_heap.end(), + std::greater{}); + + } else { + // If p is larger than the smallest element, + // then we insert into the max heap. + if (p > min_heap.at(0)) { + for (size_t i = 0; i < std::min(nrAssignments, N); ++i) { + if (min_heap.size() == N) { + std::pop_heap(min_heap.begin(), min_heap.end(), + std::greater{}); + min_heap.pop_back(); + } + min_heap.push_back(p); + std::make_heap(min_heap.begin(), min_heap.end(), + std::greater{}); + } + } + } + return p; + }; + this->visitWith(op); + + double threshold = min_heap.at(0); // Now threshold the decision tree size_t total = 0; From 9666725473c9dfbf7b16f3447c400306ace9f783 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 6 Nov 2024 16:37:04 -0500 Subject: [PATCH 019/112] implement a min-heap to record the top N probabilities for pruning --- gtsam/discrete/DecisionTreeFactor.cpp | 74 ++++++++++++++++++++++----- 1 file changed, 60 insertions(+), 14 deletions(-) diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp index 9b541bbf0..c8efc5fa5 100644 --- a/gtsam/discrete/DecisionTreeFactor.cpp +++ b/gtsam/discrete/DecisionTreeFactor.cpp @@ -349,13 +349,67 @@ namespace gtsam { : DiscreteFactor(keys.indices(), keys.cardinalities()), AlgebraicDecisionTree(keys, table) {} + /** + * @brief Min-Heap class to help with pruning. + * The `top` element is always the smallest value. + */ + class MinHeap { + std::vector v_; + + public: + /// Default constructor + MinHeap() {} + + /// Push value onto the heap + void push(double x) { + v_.push_back(x); + std::make_heap(v_.begin(), v_.end(), std::greater{}); + } + + /// Push value `x`, `n` number of times. + void push(double x, size_t n) { + v_.insert(v_.end(), n, x); + std::make_heap(v_.begin(), v_.end(), std::greater{}); + } + + /// Pop the top value of the heap. + double pop() { + std::pop_heap(v_.begin(), v_.end(), std::greater{}); + double x = v_.back(); + v_.pop_back(); + return x; + } + + /// Return the top value of the heap without popping it. + double top() { return v_.at(0); } + + /** + * @brief Print the heap as a sequence. + * + * @param s A string to prologue the output. + */ + void print(const std::string& s = "") { + std::cout << (s.empty() ? "" : s + " "); + for (size_t i = 0; i < v_.size() - 1; i++) { + std::cout << v_.at(i) << ","; + } + std::cout << v_.at(v_.size() - 1) << std::endl; + } + + /// Return true if heap is empty. + bool empty() const { return v_.empty(); } + + /// Return the size of the heap. + size_t size() const { return v_.size(); } + }; + /* ************************************************************************ */ DecisionTreeFactor DecisionTreeFactor::prune(size_t maxNrAssignments) const { const size_t N = maxNrAssignments; // Set of all keys std::set allKeys(keys().begin(), keys().end()); - std::vector min_heap; + MinHeap min_heap; auto op = [&](const Assignment& a, double p) { // Get all the keys in the current assignment @@ -377,25 +431,17 @@ namespace gtsam { } if (min_heap.empty()) { - for (size_t i = 0; i < std::min(nrAssignments, N); ++i) { - min_heap.push_back(p); - } - std::make_heap(min_heap.begin(), min_heap.end(), - std::greater{}); + min_heap.push(p, std::min(nrAssignments, N)); } else { // If p is larger than the smallest element, // then we insert into the max heap. - if (p > min_heap.at(0)) { + if (p > min_heap.top()) { for (size_t i = 0; i < std::min(nrAssignments, N); ++i) { if (min_heap.size() == N) { - std::pop_heap(min_heap.begin(), min_heap.end(), - std::greater{}); - min_heap.pop_back(); + min_heap.pop(); } - min_heap.push_back(p); - std::make_heap(min_heap.begin(), min_heap.end(), - std::greater{}); + min_heap.push(p); } } } @@ -403,7 +449,7 @@ namespace gtsam { }; this->visitWith(op); - double threshold = min_heap.at(0); + double threshold = min_heap.top(); // Now threshold the decision tree size_t total = 0; From ae43b2ade7a6f88332146aca59a27a17f8eb17b8 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 6 Nov 2024 19:23:26 -0500 Subject: [PATCH 020/112] make MinHeap more efficient by calling push_heap instead of make_heap --- gtsam/discrete/DecisionTreeFactor.cpp | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp index c8efc5fa5..caedab713 100644 --- a/gtsam/discrete/DecisionTreeFactor.cpp +++ b/gtsam/discrete/DecisionTreeFactor.cpp @@ -363,13 +363,15 @@ namespace gtsam { /// Push value onto the heap void push(double x) { v_.push_back(x); - std::make_heap(v_.begin(), v_.end(), std::greater{}); + std::push_heap(v_.begin(), v_.end(), std::greater{}); } /// Push value `x`, `n` number of times. void push(double x, size_t n) { - v_.insert(v_.end(), n, x); - std::make_heap(v_.begin(), v_.end(), std::greater{}); + for (size_t i = 0; i < n; ++i) { + v_.push_back(x); + std::push_heap(v_.begin(), v_.end(), std::greater{}); + } } /// Pop the top value of the heap. @@ -390,10 +392,11 @@ namespace gtsam { */ void print(const std::string& s = "") { std::cout << (s.empty() ? "" : s + " "); - for (size_t i = 0; i < v_.size() - 1; i++) { - std::cout << v_.at(i) << ","; + for (size_t i = 0; i < v_.size(); i++) { + std::cout << v_.at(i); + if (v_.size() > 1 && i < v_.size() - 1) std::cout << ", "; } - std::cout << v_.at(v_.size() - 1) << std::endl; + std::cout << std::endl; } /// Return true if heap is empty. From c4cf82668cdcdb20d0059b4f5753da89d8279dc7 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Fri, 8 Nov 2024 00:01:30 +0100 Subject: [PATCH 021/112] Bump min required cmake version to 3.9 In the top-level cmake file. It was 3.5 and recent cmake versions complain about it becoming deprecate. Also, the wrap directory already required 3.9 anyhow... --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 617accc2e..e3b462eec 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.9) if (POLICY CMP0082) cmake_policy(SET CMP0082 NEW) # install from sub-directories immediately endif() From 733e919570159e18cc49f524598ff6a1a2ed72ef Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Gonzalve?= Date: Sat, 9 Nov 2024 20:20:37 +0100 Subject: [PATCH 022/112] Make operator-> const Having this operator non-const is awkward as operator* is const. This sometimes leads to writing: (*obj).data; instead of obj->data; which is unexpected.... --- gtsam/nonlinear/Values.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index 21e8e281f..2fe64d9c9 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -189,7 +189,7 @@ namespace gtsam { const_iterator_type it_; deref_iterator(const_iterator_type it) : it_(it) {} ConstKeyValuePair operator*() const { return {it_->first, *(it_->second)}; } - std::unique_ptr operator->() { + std::unique_ptr operator->() const { return std::make_unique(it_->first, *(it_->second)); } bool operator==(const deref_iterator& other) const { From baa275bf51ed63dd61f3da2c90167491a2f1806e Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 26 Nov 2024 09:53:28 -0800 Subject: [PATCH 023/112] address comments --- gtsam/slam/EssentialMatrixFactor.h | 32 +++++++++------- .../slam/tests/testEssentialMatrixFactor.cpp | 37 +++++++++++++++---- 2 files changed, 47 insertions(+), 22 deletions(-) diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index 7a0b46906..5fdf138cc 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -420,10 +420,13 @@ class EssentialMatrixFactor4 /** * Binary factor that optimizes for E and two calibrations Ka and Kb using the * algebraic epipolar error (Ka^-1 pA)'E (Kb^-1 pB). The calibrations are - * assumed different for the two images. Don'tt use this factor with same - * calibration unknown, as Jacobians will be incorrect... + * assumed different for the two images, but if you use the same key for Ka and + * Kb, the sum of the two K Jacobians equals that of the K Jacobian for + * EssentialMatrix4. If you know there is a single global calibration, use + * that factor instead. * - * Note: even stronger caveats about having priors on calibration apply. + * Note: see the comment about priors from EssentialMatrixFactor4: even stronger + * caveats about having priors on calibration apply here. */ template class EssentialMatrixFactor5 @@ -442,17 +445,18 @@ class EssentialMatrixFactor5 using Base::evaluateError; /** - * Constructor - * @param keyE Essential Matrix aEb variable key - * @param keyKa Calibration variable key for camera A - * @param keyKb Calibration variable key for camera B - * @param pA point in first camera, in pixel coordinates - * @param pB point in second camera, in pixel coordinates - * @param model noise model is about dot product in ideal, homogeneous + * Constructor + * @param keyE Essential Matrix aEb variable key + * @param keyKa Calibration variable key for camera A + * @param keyKb Calibration variable key for camera B + * @param pA point in first camera, in pixel coordinates + * @param pB point in second camera, in pixel coordinates + * @param model noise model is about dot product in ideal, homogeneous * coordinates */ EssentialMatrixFactor5(Key keyE, Key keyKa, Key keyKb, const Point2& pA, - const Point2& pB, const SharedNoiseModel& model = nullptr) + const Point2& pB, + const SharedNoiseModel& model = nullptr) : Base(model, keyE, keyKa, keyKb), pA_(pA), pB_(pB) {} /// @return a deep copy of this factor @@ -492,17 +496,17 @@ class EssentialMatrixFactor5 Point2 cA = Ka.calibrate(pA_, HKa ? &cA_H_Ka : 0, OptionalNone); Point2 cB = Kb.calibrate(pB_, HKb ? &cB_H_Kb : 0, OptionalNone); - // convert to homogeneous coordinates + // Convert to homogeneous coordinates. Vector3 vA = EssentialMatrix::Homogeneous(cA); Vector3 vB = EssentialMatrix::Homogeneous(cB); if (HKa) { - // compute the jacobian of error w.r.t Ka + // Compute the jacobian of error w.r.t Ka. *HKa = vB.transpose() * E.matrix().transpose().leftCols<2>() * cA_H_Ka; } if (HKb) { - // compute the jacobian of error w.r.t Kb + // Compute the jacobian of error w.r.t Kb. *HKb = vA.transpose() * E.matrix().leftCols<2>() * cB_H_Kb; } diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index 609f38187..11558e38e 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -117,8 +117,8 @@ TEST(EssentialMatrixFactor, ExpressionFactor) { // Check evaluation Vector expected(1); expected << 0; - vector Hactual(1); - Vector actual = factor.unwhitenedError(values, Hactual); + vector actualH(1); + Vector actual = factor.unwhitenedError(values, actualH); EXPECT(assert_equal(expected, actual, 1e-7)); } } @@ -151,8 +151,8 @@ TEST(EssentialMatrixFactor, ExpressionFactorRotationOnly) { // Check evaluation Vector expected(1); expected << 0; - vector Hactual(1); - Vector actual = factor.unwhitenedError(values, Hactual); + vector actualH(1); + Vector actual = factor.unwhitenedError(values, actualH); EXPECT(assert_equal(expected, actual, 1e-7)); } } @@ -213,9 +213,9 @@ TEST(EssentialMatrixFactor2, factor) { const Point2 pi = PinholeBase::Project(P2); Point2 expected(pi - pB(i)); - Matrix Hactual1, Hactual2; + Matrix actualH1, actualH2; double d(baseline / P1.z()); - Vector actual = factor.evaluateError(trueE, d, Hactual1, Hactual2); + Vector actual = factor.evaluateError(trueE, d, actualH1, actualH2); EXPECT(assert_equal(expected, actual, 1e-7)); Values val; @@ -278,9 +278,9 @@ TEST(EssentialMatrixFactor3, factor) { const Point2 pi = camera2.project(P1); Point2 expected(pi - pB(i)); - Matrix Hactual1, Hactual2; + Matrix actualH1, actualH2; double d(baseline / P1.z()); - Vector actual = factor.evaluateError(bodyE, d, Hactual1, Hactual2); + Vector actual = factor.evaluateError(bodyE, d, actualH1, actualH2); EXPECT(assert_equal(expected, actual, 1e-7)); Values val; @@ -552,6 +552,27 @@ TEST(EssentialMatrixFactor5, factor) { } } +//************************************************************************* +// Test that if we use the same keys for Ka and Kb the sum of the two K +// Jacobians equals that of the single K Jacobian for EssentialMatrix4 +TEST(EssentialMatrixFactor5, SameKeys) { + Key keyE(1), keyK(2); + for (size_t i = 0; i < 5; i++) { + EssentialMatrixFactor4 factor4(keyE, keyK, pA(i), pB(i), model1); + EssentialMatrixFactor5 factor5(keyE, keyK, keyK, pA(i), pB(i), + model1); + + Values truth; + truth.insert(keyE, trueE); + truth.insert(keyK, trueK); + + // Check Jacobians + Matrix H0, H1, H2; + factor4.evaluateError(trueE, trueK, nullptr, &H0); + factor5.evaluateError(trueE, trueK, trueK, nullptr, &H1, &H2); + EXPECT(assert_equal(H0, H1 + H2, 1e-9)); + } +} } // namespace example1 //************************************************************************* From d38b577284182e85c3285eca415ac4c622494fd5 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 1 Dec 2024 18:55:55 -0500 Subject: [PATCH 024/112] update CMake for finding gperftools --- cmake/FindGooglePerfTools.cmake | 35 ++++++++++++++------------------- 1 file changed, 15 insertions(+), 20 deletions(-) diff --git a/cmake/FindGooglePerfTools.cmake b/cmake/FindGooglePerfTools.cmake index 01243257b..d85c01389 100644 --- a/cmake/FindGooglePerfTools.cmake +++ b/cmake/FindGooglePerfTools.cmake @@ -1,35 +1,30 @@ # -*- cmake -*- -# - Find Google perftools -# Find the Google perftools includes and libraries -# This module defines -# GOOGLE_PERFTOOLS_INCLUDE_DIR, where to find heap-profiler.h, etc. -# GOOGLE_PERFTOOLS_FOUND, If false, do not try to use Google perftools. +# - Find GPerfTools (formerly Google perftools) +# Find the GPerfTools libraries +# If false, do not try to use Google perftools. # also defined for general use are # TCMALLOC_LIBRARY, where to find the tcmalloc library. -FIND_PATH(GOOGLE_PERFTOOLS_INCLUDE_DIR google/heap-profiler.h -/usr/local/include -/usr/include -) - SET(TCMALLOC_NAMES ${TCMALLOC_NAMES} tcmalloc) -FIND_LIBRARY(TCMALLOC_LIBRARY +find_library(TCMALLOC_LIBRARY NAMES ${TCMALLOC_NAMES} PATHS /usr/lib /usr/local/lib - ) +) +find_library(GPERFTOOLS_PROFILER + NAMES profiler + PATHS /usr/lib /usr/local/lib +) -IF (TCMALLOC_LIBRARY AND GOOGLE_PERFTOOLS_INCLUDE_DIR) +IF (TCMALLOC_LIBRARY AND GPERFTOOLS_PROFILER) SET(TCMALLOC_LIBRARIES ${TCMALLOC_LIBRARY}) SET(GOOGLE_PERFTOOLS_FOUND "YES") -ELSE (TCMALLOC_LIBRARY AND GOOGLE_PERFTOOLS_INCLUDE_DIR) +ELSE (TCMALLOC_LIBRARY AND GPERFTOOLS_PROFILER) SET(GOOGLE_PERFTOOLS_FOUND "NO") -ENDIF (TCMALLOC_LIBRARY AND GOOGLE_PERFTOOLS_INCLUDE_DIR) +ENDIF (TCMALLOC_LIBRARY AND GPERFTOOLS_PROFILER) IF (GOOGLE_PERFTOOLS_FOUND) - IF (NOT GOOGLE_PERFTOOLS_FIND_QUIETLY) - MESSAGE(STATUS "Found Google perftools: ${GOOGLE_PERFTOOLS_LIBRARIES}") - ENDIF (NOT GOOGLE_PERFTOOLS_FIND_QUIETLY) + MESSAGE(STATUS "Found Google perftools: ${GPERFTOOLS_PROFILER}") ELSE (GOOGLE_PERFTOOLS_FOUND) IF (GOOGLE_PERFTOOLS_FIND_REQUIRED) MESSAGE(FATAL_ERROR "Could not find Google perftools library") @@ -38,5 +33,5 @@ ENDIF (GOOGLE_PERFTOOLS_FOUND) MARK_AS_ADVANCED( TCMALLOC_LIBRARY - GOOGLE_PERFTOOLS_INCLUDE_DIR - ) + GPERFTOOLS_PROFILER +) From be4964927c657e5e3daab55ec7b36cbe38166000 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 1 Dec 2024 18:57:40 -0500 Subject: [PATCH 025/112] rename TCMALLOC_LIBRARY to GPERFTOOLS_TCMALLOC --- cmake/FindGooglePerfTools.cmake | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/cmake/FindGooglePerfTools.cmake b/cmake/FindGooglePerfTools.cmake index d85c01389..f7caa7c1c 100644 --- a/cmake/FindGooglePerfTools.cmake +++ b/cmake/FindGooglePerfTools.cmake @@ -3,11 +3,12 @@ # - Find GPerfTools (formerly Google perftools) # Find the GPerfTools libraries # If false, do not try to use Google perftools. -# also defined for general use are -# TCMALLOC_LIBRARY, where to find the tcmalloc library. +# Also defined for general use are +# - GPERFTOOLS_TCMALLOC: where to find the tcmalloc library +# - GPERFTOOLS_PROFILER: where to find the profiler library SET(TCMALLOC_NAMES ${TCMALLOC_NAMES} tcmalloc) -find_library(TCMALLOC_LIBRARY +find_library(GPERFTOOLS_TCMALLOC NAMES ${TCMALLOC_NAMES} PATHS /usr/lib /usr/local/lib ) @@ -16,12 +17,12 @@ find_library(GPERFTOOLS_PROFILER PATHS /usr/lib /usr/local/lib ) -IF (TCMALLOC_LIBRARY AND GPERFTOOLS_PROFILER) - SET(TCMALLOC_LIBRARIES ${TCMALLOC_LIBRARY}) +IF (GPERFTOOLS_TCMALLOC AND GPERFTOOLS_PROFILER) + SET(TCMALLOC_LIBRARIES ${GPERFTOOLS_TCMALLOC}) SET(GOOGLE_PERFTOOLS_FOUND "YES") -ELSE (TCMALLOC_LIBRARY AND GPERFTOOLS_PROFILER) +ELSE (GPERFTOOLS_TCMALLOC AND GPERFTOOLS_PROFILER) SET(GOOGLE_PERFTOOLS_FOUND "NO") -ENDIF (TCMALLOC_LIBRARY AND GPERFTOOLS_PROFILER) +ENDIF (GPERFTOOLS_TCMALLOC AND GPERFTOOLS_PROFILER) IF (GOOGLE_PERFTOOLS_FOUND) MESSAGE(STATUS "Found Google perftools: ${GPERFTOOLS_PROFILER}") @@ -32,6 +33,6 @@ ELSE (GOOGLE_PERFTOOLS_FOUND) ENDIF (GOOGLE_PERFTOOLS_FOUND) MARK_AS_ADVANCED( - TCMALLOC_LIBRARY + GPERFTOOLS_TCMALLOC GPERFTOOLS_PROFILER ) From 486feeb385345d13698e67a0ff4f20161ad8de57 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 1 Dec 2024 18:58:06 -0500 Subject: [PATCH 026/112] link gperftools libraries --- cmake/HandlePerfTools.cmake | 1 + 1 file changed, 1 insertion(+) diff --git a/cmake/HandlePerfTools.cmake b/cmake/HandlePerfTools.cmake index 499caf80a..6c455d952 100644 --- a/cmake/HandlePerfTools.cmake +++ b/cmake/HandlePerfTools.cmake @@ -2,3 +2,4 @@ ############################################################################### # Find Google perftools find_package(GooglePerfTools) +target_link_libraries(${PROJECT_NAME} ${GPERFTOOLS_TCMALLOC} ${GPERFTOOLS_PROFILER}) \ No newline at end of file From 8473911926e8605d5733bcd34493022cf5bef497 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 2 Dec 2024 09:52:20 -0500 Subject: [PATCH 027/112] computeThreshold as an individual function --- gtsam/discrete/DecisionTreeFactor.cpp | 30 +++++++++++++++++++-------- gtsam/discrete/DecisionTreeFactor.h | 11 ++++++++++ 2 files changed, 32 insertions(+), 9 deletions(-) diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp index caedab713..9ec3b0ac5 100644 --- a/gtsam/discrete/DecisionTreeFactor.cpp +++ b/gtsam/discrete/DecisionTreeFactor.cpp @@ -407,11 +407,9 @@ namespace gtsam { }; /* ************************************************************************ */ - DecisionTreeFactor DecisionTreeFactor::prune(size_t maxNrAssignments) const { - const size_t N = maxNrAssignments; - + double DecisionTreeFactor::computeThreshold(const size_t N) const { // Set of all keys - std::set allKeys(keys().begin(), keys().end()); + std::set allKeys = this->labels(); MinHeap min_heap; auto op = [&](const Assignment& a, double p) { @@ -433,18 +431,25 @@ namespace gtsam { nrAssignments *= cardinalities_.at(k); } + // If min-heap is empty, fill it initially. + // This is because there is nothing at the top. if (min_heap.empty()) { min_heap.push(p, std::min(nrAssignments, N)); } else { - // If p is larger than the smallest element, - // then we insert into the max heap. - if (p > min_heap.top()) { - for (size_t i = 0; i < std::min(nrAssignments, N); ++i) { + for (size_t i = 0; i < std::min(nrAssignments, N); ++i) { + // If p is larger than the smallest element, + // then we insert into the min heap. + // We check against the top each time because the + // heap maintains the smallest element at the top. + if (p > min_heap.top()) { if (min_heap.size() == N) { min_heap.pop(); } min_heap.push(p); + } else { + // p is <= min value so move to the next one + break; } } } @@ -452,7 +457,14 @@ namespace gtsam { }; this->visitWith(op); - double threshold = min_heap.top(); + return min_heap.top(); + } + + /* ************************************************************************ */ + DecisionTreeFactor DecisionTreeFactor::prune(size_t maxNrAssignments) const { + const size_t N = maxNrAssignments; + + double threshold = computeThreshold(N); // Now threshold the decision tree size_t total = 0; diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h index 7af729f3e..a8ab2644f 100644 --- a/gtsam/discrete/DecisionTreeFactor.h +++ b/gtsam/discrete/DecisionTreeFactor.h @@ -224,6 +224,17 @@ namespace gtsam { /// Get all the probabilities in order of assignment values std::vector probabilities() const; + /** + * @brief Compute the probability value which is the threshold above which + * only `N` leaves are present. + * + * This is used for pruning out the smaller probabilities. + * + * @param N The number of leaves to keep post pruning. + * @return double + */ + double computeThreshold(const size_t N) const; + /** * @brief Prune the decision tree of discrete variables. * From 1091b9cd2d03258d0f79144b92c4f432628d128a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 2 Dec 2024 09:53:01 -0500 Subject: [PATCH 028/112] test for computeThreshold --- .../discrete/tests/testDecisionTreeFactor.cpp | 45 +++++++++++++++---- 1 file changed, 37 insertions(+), 8 deletions(-) diff --git a/gtsam/discrete/tests/testDecisionTreeFactor.cpp b/gtsam/discrete/tests/testDecisionTreeFactor.cpp index a41d06c2b..756a0cebe 100644 --- a/gtsam/discrete/tests/testDecisionTreeFactor.cpp +++ b/gtsam/discrete/tests/testDecisionTreeFactor.cpp @@ -140,11 +140,46 @@ TEST(DecisionTreeFactor, enumerate) { EXPECT(actual == expected); } +namespace pruning_fixture { + +DiscreteKey A(1, 2), B(2, 2), C(3, 2); +DecisionTreeFactor f(A& B& C, "1 5 3 7 2 6 4 8"); + +DiscreteKey D(4, 2); +DecisionTreeFactor factor( + D& C & B & A, + "0.0 0.0 0.0 0.60658897 0.61241912 0.61241969 0.61247685 0.61247742 0.0 " + "0.0 0.0 0.99995287 1.0 1.0 1.0 1.0"); + +} // namespace pruning_fixture + +/* ************************************************************************* */ +// Check if computing the correct threshold works. +TEST(DecisionTreeFactor, ComputeThreshold) { + using namespace pruning_fixture; + + // Only keep the leaves with the top 5 values. + double threshold = f.computeThreshold(5); + EXPECT_DOUBLES_EQUAL(4.0, threshold, 1e-9); + + // Check for more extreme pruning where we only keep the top 2 leaves + threshold = f.computeThreshold(2); + EXPECT_DOUBLES_EQUAL(7.0, threshold, 1e-9); + + threshold = factor.computeThreshold(5); + EXPECT_DOUBLES_EQUAL(0.99995287, threshold, 1e-9); + + threshold = factor.computeThreshold(3); + EXPECT_DOUBLES_EQUAL(1.0, threshold, 1e-9); + + threshold = factor.computeThreshold(6); + EXPECT_DOUBLES_EQUAL(0.61247742, threshold, 1e-9); +} + /* ************************************************************************* */ // Check pruning of the decision tree works as expected. TEST(DecisionTreeFactor, Prune) { - DiscreteKey A(1, 2), B(2, 2), C(3, 2); - DecisionTreeFactor f(A & B & C, "1 5 3 7 2 6 4 8"); + using namespace pruning_fixture; // Only keep the leaves with the top 5 values. size_t maxNrAssignments = 5; @@ -160,12 +195,6 @@ TEST(DecisionTreeFactor, Prune) { DecisionTreeFactor expected2(A & B & C, "0 0 0 7 0 0 0 8"); EXPECT(assert_equal(expected2, pruned2)); - DiscreteKey D(4, 2); - DecisionTreeFactor factor( - D & C & B & A, - "0.0 0.0 0.0 0.60658897 0.61241912 0.61241969 0.61247685 0.61247742 0.0 " - "0.0 0.0 0.99995287 1.0 1.0 1.0 1.0"); - DecisionTreeFactor expected3(D & C & B & A, "0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 " "0.999952870000 1.0 1.0 1.0 1.0"); From 3980a439c0fc655ac73a2763f60109b5e6a9270f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 2 Dec 2024 09:53:33 -0500 Subject: [PATCH 029/112] link gperftools in the top CMakeLists.txt --- cmake/HandlePerfTools.cmake | 3 +-- gtsam/CMakeLists.txt | 4 ++++ 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/cmake/HandlePerfTools.cmake b/cmake/HandlePerfTools.cmake index 6c455d952..4b5e5c8b3 100644 --- a/cmake/HandlePerfTools.cmake +++ b/cmake/HandlePerfTools.cmake @@ -1,5 +1,4 @@ ############################################################################### # Find Google perftools -find_package(GooglePerfTools) -target_link_libraries(${PROJECT_NAME} ${GPERFTOOLS_TCMALLOC} ${GPERFTOOLS_PROFILER}) \ No newline at end of file +find_package(GooglePerfTools) \ No newline at end of file diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index 1fc8e4570..dc9f40ed7 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -147,6 +147,10 @@ if (GTSAM_USE_EIGEN_MKL) target_include_directories(gtsam PUBLIC ${MKL_INCLUDE_DIR}) endif() +if (GOOGLE_PERFTOOLS_FOUND) + target_link_libraries(gtsam PRIVATE ${GPERFTOOLS_TCMALLOC} ${GPERFTOOLS_PROFILER}) +endif() + # Add includes for source directories 'BEFORE' boost and any system include # paths so that the compiler uses GTSAM headers in our source directory instead # of any previously installed GTSAM headers. From de1c2d78fd2f14fdd25004edd31cae21d18f6435 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 2 Dec 2024 09:54:32 -0500 Subject: [PATCH 030/112] rename GOOGLE_PERFTOOLS to GPERFTOOLS --- cmake/FindGooglePerfTools.cmake | 12 ++++++------ cmake/HandleAllocators.cmake | 2 +- gtsam/CMakeLists.txt | 2 +- 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/cmake/FindGooglePerfTools.cmake b/cmake/FindGooglePerfTools.cmake index f7caa7c1c..f73f449ec 100644 --- a/cmake/FindGooglePerfTools.cmake +++ b/cmake/FindGooglePerfTools.cmake @@ -19,18 +19,18 @@ find_library(GPERFTOOLS_PROFILER IF (GPERFTOOLS_TCMALLOC AND GPERFTOOLS_PROFILER) SET(TCMALLOC_LIBRARIES ${GPERFTOOLS_TCMALLOC}) - SET(GOOGLE_PERFTOOLS_FOUND "YES") + SET(GPERFTOOLS_FOUND "YES") ELSE (GPERFTOOLS_TCMALLOC AND GPERFTOOLS_PROFILER) - SET(GOOGLE_PERFTOOLS_FOUND "NO") + SET(GPERFTOOLS_FOUND "NO") ENDIF (GPERFTOOLS_TCMALLOC AND GPERFTOOLS_PROFILER) -IF (GOOGLE_PERFTOOLS_FOUND) - MESSAGE(STATUS "Found Google perftools: ${GPERFTOOLS_PROFILER}") -ELSE (GOOGLE_PERFTOOLS_FOUND) +IF (GPERFTOOLS_FOUND) + MESSAGE(STATUS "Found Gperftools: ${GPERFTOOLS_PROFILER}") +ELSE (GPERFTOOLS_FOUND) IF (GOOGLE_PERFTOOLS_FIND_REQUIRED) MESSAGE(FATAL_ERROR "Could not find Google perftools library") ENDIF (GOOGLE_PERFTOOLS_FIND_REQUIRED) -ENDIF (GOOGLE_PERFTOOLS_FOUND) +ENDIF (GPERFTOOLS_FOUND) MARK_AS_ADVANCED( GPERFTOOLS_TCMALLOC diff --git a/cmake/HandleAllocators.cmake b/cmake/HandleAllocators.cmake index 63411b17b..38297bbba 100644 --- a/cmake/HandleAllocators.cmake +++ b/cmake/HandleAllocators.cmake @@ -7,7 +7,7 @@ else() list(APPEND possible_allocators BoostPool STL) set(preferred_allocator STL) endif() -if(GOOGLE_PERFTOOLS_FOUND) +if(GPERFTOOLS_FOUND) list(APPEND possible_allocators tcmalloc) endif() diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index dc9f40ed7..9a4b6ac3a 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -147,7 +147,7 @@ if (GTSAM_USE_EIGEN_MKL) target_include_directories(gtsam PUBLIC ${MKL_INCLUDE_DIR}) endif() -if (GOOGLE_PERFTOOLS_FOUND) +if (GPERFTOOLS_FOUND) target_link_libraries(gtsam PRIVATE ${GPERFTOOLS_TCMALLOC} ${GPERFTOOLS_PROFILER}) endif() From 94e31c99df1f9fb68eb043f5a39830694cb698df Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 2 Dec 2024 09:54:56 -0500 Subject: [PATCH 031/112] expose Rot3::expmap in wrapper --- gtsam/geometry/geometry.i | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 42d2fe550..a8af78f2f 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -360,6 +360,7 @@ class Rot3 { // Standard Interface static gtsam::Rot3 Expmap(gtsam::Vector v); static gtsam::Vector Logmap(const gtsam::Rot3& p); + gtsam::Rot3 expmap(const gtsam::Vector& v); gtsam::Vector logmap(const gtsam::Rot3& p); gtsam::Matrix matrix() const; gtsam::Matrix transpose() const; From 557d7d58278735c2d352ffe941d596be7c4b1836 Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Fri, 29 Nov 2024 19:25:16 -0800 Subject: [PATCH 032/112] wrap BatchFixedLagSmoother.getFactors() --- gtsam/nonlinear/nonlinear.i | 3 +++ 1 file changed, 3 insertions(+) diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 09c234630..5d15c1163 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -715,6 +715,9 @@ virtual class BatchFixedLagSmoother : gtsam::FixedLagSmoother { void print(string s = "BatchFixedLagSmoother:\n") const; gtsam::LevenbergMarquardtParams params() const; + + gtsam::NonlinearFactorGraph getFactors() const; + template From a9c75d8ef4f2bfd8f188c9f02caad076331b1930 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 2 Dec 2024 12:07:30 -0500 Subject: [PATCH 033/112] add pruned flag to avoid extra pruning --- gtsam/hybrid/HybridBayesTree.cpp | 8 +++++--- gtsam/hybrid/HybridGaussianConditional.cpp | 12 +++++++----- gtsam/hybrid/HybridGaussianConditional.h | 11 +++++++++-- 3 files changed, 21 insertions(+), 10 deletions(-) diff --git a/gtsam/hybrid/HybridBayesTree.cpp b/gtsam/hybrid/HybridBayesTree.cpp index 193635a21..1b633e024 100644 --- a/gtsam/hybrid/HybridBayesTree.cpp +++ b/gtsam/hybrid/HybridBayesTree.cpp @@ -210,9 +210,11 @@ void HybridBayesTree::prune(const size_t maxNrLeaves) { if (conditional->isHybrid()) { auto hybridGaussianCond = conditional->asHybrid(); - // Imperative - clique->conditional() = std::make_shared( - hybridGaussianCond->prune(parentData.prunedDiscreteProbs)); + if (!hybridGaussianCond->pruned()) { + // Imperative + clique->conditional() = std::make_shared( + hybridGaussianCond->prune(parentData.prunedDiscreteProbs)); + } } return parentData; } diff --git a/gtsam/hybrid/HybridGaussianConditional.cpp b/gtsam/hybrid/HybridGaussianConditional.cpp index 1bec42810..54346679e 100644 --- a/gtsam/hybrid/HybridGaussianConditional.cpp +++ b/gtsam/hybrid/HybridGaussianConditional.cpp @@ -120,7 +120,7 @@ struct HybridGaussianConditional::Helper { /* *******************************************************************************/ HybridGaussianConditional::HybridGaussianConditional( - const DiscreteKeys &discreteParents, Helper &&helper) + const DiscreteKeys &discreteParents, Helper &&helper, bool pruned) : BaseFactor(discreteParents, FactorValuePairs( [&](const GaussianFactorValuePair @@ -130,7 +130,8 @@ HybridGaussianConditional::HybridGaussianConditional( }, std::move(helper.pairs))), BaseConditional(*helper.nrFrontals), - negLogConstant_(helper.minNegLogConstant) {} + negLogConstant_(helper.minNegLogConstant), + pruned_(pruned) {} HybridGaussianConditional::HybridGaussianConditional( const DiscreteKey &discreteParent, @@ -166,8 +167,9 @@ HybridGaussianConditional::HybridGaussianConditional( : HybridGaussianConditional(discreteParents, Helper(conditionals)) {} HybridGaussianConditional::HybridGaussianConditional( - const DiscreteKeys &discreteParents, const FactorValuePairs &pairs) - : HybridGaussianConditional(discreteParents, Helper(pairs)) {} + const DiscreteKeys &discreteParents, const FactorValuePairs &pairs, + bool pruned) + : HybridGaussianConditional(discreteParents, Helper(pairs), pruned) {} /* *******************************************************************************/ const HybridGaussianConditional::Conditionals @@ -331,7 +333,7 @@ HybridGaussianConditional::shared_ptr HybridGaussianConditional::prune( FactorValuePairs prunedConditionals = factors().apply(pruner); return std::make_shared(discreteKeys(), - prunedConditionals); + prunedConditionals, true); } /* *******************************************************************************/ diff --git a/gtsam/hybrid/HybridGaussianConditional.h b/gtsam/hybrid/HybridGaussianConditional.h index c485fafce..0d2b1323c 100644 --- a/gtsam/hybrid/HybridGaussianConditional.h +++ b/gtsam/hybrid/HybridGaussianConditional.h @@ -68,6 +68,9 @@ class GTSAM_EXPORT HybridGaussianConditional ///< Take advantage of the neg-log space so everything is a minimization double negLogConstant_; + /// Flag to indicate if the conditional has been pruned. + bool pruned_ = false; + public: /// @name Constructors /// @{ @@ -150,9 +153,10 @@ class GTSAM_EXPORT HybridGaussianConditional * * @param discreteParents the discrete parents. Will be placed last. * @param conditionalPairs Decision tree of GaussianFactor/scalar pairs. + * @param pruned Flag indicating if conditional has been pruned. */ HybridGaussianConditional(const DiscreteKeys &discreteParents, - const FactorValuePairs &pairs); + const FactorValuePairs &pairs, bool pruned = false); /// @} /// @name Testable @@ -233,6 +237,9 @@ class GTSAM_EXPORT HybridGaussianConditional HybridGaussianConditional::shared_ptr prune( const DecisionTreeFactor &discreteProbs) const; + /// Return true if the conditional has already been pruned. + bool pruned() const { return pruned_; } + /// @} private: @@ -241,7 +248,7 @@ class GTSAM_EXPORT HybridGaussianConditional /// Private constructor that uses helper struct above. HybridGaussianConditional(const DiscreteKeys &discreteParents, - Helper &&helper); + Helper &&helper, bool pruned = false); /// Check whether `given` has values for all frontal keys. bool allFrontalsGiven(const VectorValues &given) const; From beff3fa26992645bd49c045215823ea49dbf9448 Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Mon, 2 Dec 2024 09:48:09 -0800 Subject: [PATCH 034/112] add error(HybridValues) to NonlinearFactor fixes the python build --- gtsam/nonlinear/NonlinearFactor.h | 1 + gtsam/nonlinear/nonlinear.i | 1 + 2 files changed, 2 insertions(+) diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 725117748..2d3a13766 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -22,6 +22,7 @@ #pragma once #include +#include #include #include #include diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 5d15c1163..b684b1aad 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -127,6 +127,7 @@ virtual class NonlinearFactor : gtsam::Factor { // NonlinearFactor bool equals(const gtsam::NonlinearFactor& other, double tol) const; double error(const gtsam::Values& c) const; + double error(const gtsam::HybridValues& c) const; size_t dim() const; bool active(const gtsam::Values& c) const; gtsam::GaussianFactor* linearize(const gtsam::Values& c) const; From bbdeaa400fe5305d86eb40a12a7b7b17516a6c0d Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sun, 1 Dec 2024 12:45:26 -0500 Subject: [PATCH 035/112] Remove the asserts as they have already been checked in Eigen --- gtsam/base/Vector.h | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index f9a5cf079..4ef374234 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -16,6 +16,7 @@ * @author Frank Dellaert * @author Alex Hagiopol * @author Varun Agrawal + * @author Fan Jiang */ // \callgraph @@ -193,14 +194,12 @@ GTSAM_EXPORT Vector ediv_(const Vector &a, const Vector &b); */ template inline double dot(const V1 &a, const V2& b) { - assert (b.size()==a.size()); return a.dot(b); } /** compatibility version for ublas' inner_prod() */ template inline double inner_prod(const V1 &a, const V2& b) { - assert (b.size()==a.size()); return a.dot(b); } From 2aa22005f11e8b50d5dd5332e9e59104c1996b4c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 6 Dec 2024 11:32:12 -0500 Subject: [PATCH 036/112] add CMake switch for GPerfTools --- cmake/FindGooglePerfTools.cmake | 2 ++ 1 file changed, 2 insertions(+) diff --git a/cmake/FindGooglePerfTools.cmake b/cmake/FindGooglePerfTools.cmake index f73f449ec..4af268528 100644 --- a/cmake/FindGooglePerfTools.cmake +++ b/cmake/FindGooglePerfTools.cmake @@ -36,3 +36,5 @@ MARK_AS_ADVANCED( GPERFTOOLS_TCMALLOC GPERFTOOLS_PROFILER ) + +option(GTSAM_ENABLE_GPERFTOOLS "Enable/Disable Gperftools" OFF) From c00de92854fce0038bf5e5ac3b49bbf45784a77e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 6 Dec 2024 11:32:34 -0500 Subject: [PATCH 037/112] use the switch --- gtsam/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index 9a4b6ac3a..87440d19f 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -147,7 +147,7 @@ if (GTSAM_USE_EIGEN_MKL) target_include_directories(gtsam PUBLIC ${MKL_INCLUDE_DIR}) endif() -if (GPERFTOOLS_FOUND) +if (GTSAM_ENABLE_GPERFTOOLS AND GPERFTOOLS_FOUND) target_link_libraries(gtsam PRIVATE ${GPERFTOOLS_TCMALLOC} ${GPERFTOOLS_PROFILER}) endif() From ddd95f4a51549d3c19cdca36d488da1323430676 Mon Sep 17 00:00:00 2001 From: Jade Turner Date: Sat, 7 Dec 2024 00:33:21 +0800 Subject: [PATCH 038/112] [build] Allow disabling tests and examples When you have gtsam as a dependency in CMake these would get built. This doesn't make sense and increases build times, allowing them to be disabled fixes this. Signed-off-by: Jade Turner --- CMakeLists.txt | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index e3b462eec..ffedf56e4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -112,10 +112,17 @@ add_subdirectory(CppUnitLite) add_subdirectory(gtsam) # Build Tests -add_subdirectory(tests) +option(BUILD_TESTS "Builds unit tests" ON) +if (BUILD_TESTS) + add_subdirectory(tests) +endif() + # Build examples -add_subdirectory(examples) +option(BUILD_EXAMPLES "Builds examples" ON) +if (BUILD_EXAMPLES) + add_subdirectory(examples) +endif() # Build timing add_subdirectory(timing) From 947f5b172d95d2438adfbcdf679a5817b0b93939 Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Sat, 7 Dec 2024 20:21:46 -0800 Subject: [PATCH 039/112] fix CHECK_EQUAL --- CppUnitLite/Test.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CppUnitLite/Test.h b/CppUnitLite/Test.h index 691068148..9ee0d10eb 100644 --- a/CppUnitLite/Test.h +++ b/CppUnitLite/Test.h @@ -129,7 +129,7 @@ protected: result_.addFailure(Failure(name_, __FILE__, __LINE__, #expected, #actual)); } #define CHECK_EQUAL(expected,actual)\ -{ if ((expected) == (actual)) return; result_.addFailure(Failure(name_, __FILE__, __LINE__, std::to_string(expected), std::to_string(actual))); } +{ if (!((expected) == (actual))) { result_.addFailure(Failure(name_, __FILE__, __LINE__, std::to_string(expected), std::to_string(actual))); } } #define LONGS_EQUAL(expected,actual)\ { long actualTemp = actual; \ From 9b93411d69b1cde30c720e80e644f096b6d7d6cb Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 8 Dec 2024 11:45:10 -0500 Subject: [PATCH 040/112] put Ring struct in a separate file --- gtsam/discrete/AlgebraicDecisionTree.h | 24 ++------------- gtsam/discrete/DiscreteConditional.cpp | 3 +- gtsam/discrete/Ring.h | 37 +++++++++++++++++++++++ gtsam/discrete/TableFactor.h | 16 +--------- gtsam/discrete/tests/testDecisionTree.cpp | 9 +----- 5 files changed, 43 insertions(+), 46 deletions(-) create mode 100644 gtsam/discrete/Ring.h diff --git a/gtsam/discrete/AlgebraicDecisionTree.h b/gtsam/discrete/AlgebraicDecisionTree.h index 9948b0be6..383346ab1 100644 --- a/gtsam/discrete/AlgebraicDecisionTree.h +++ b/gtsam/discrete/AlgebraicDecisionTree.h @@ -20,12 +20,12 @@ #include #include +#include -#include +#include #include #include #include -#include #include namespace gtsam { @@ -55,26 +55,6 @@ namespace gtsam { public: using Base = DecisionTree; - /** The Real ring with addition and multiplication */ - struct Ring { - static inline double zero() { return 0.0; } - static inline double one() { return 1.0; } - static inline double add(const double& a, const double& b) { - return a + b; - } - static inline double max(const double& a, const double& b) { - return std::max(a, b); - } - static inline double mul(const double& a, const double& b) { - return a * b; - } - static inline double div(const double& a, const double& b) { - return a / b; - } - static inline double id(const double& x) { return x; } - static inline double negate(const double& x) { return -x; } - }; - AlgebraicDecisionTree(double leaf = 1.0) : Base(leaf) {} // Explicitly non-explicit constructor diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp index 5ab0c59ec..c44905d79 100644 --- a/gtsam/discrete/DiscreteConditional.cpp +++ b/gtsam/discrete/DiscreteConditional.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include @@ -104,7 +105,7 @@ DiscreteConditional DiscreteConditional::operator*( // Finally, add parents to keys, in order for (auto&& dk : parents) discreteKeys.push_back(dk); - ADT product = ADT::apply(other, ADT::Ring::mul); + ADT product = ADT::apply(other, Ring::mul); return DiscreteConditional(newFrontals.size(), discreteKeys, product); } diff --git a/gtsam/discrete/Ring.h b/gtsam/discrete/Ring.h new file mode 100644 index 000000000..cf7c6424a --- /dev/null +++ b/gtsam/discrete/Ring.h @@ -0,0 +1,37 @@ +/* ---------------------------------------------------------------------------- + + * 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 Ring.h + * @brief Real Ring definition + * @author Varun Agrawal + * @date Dec 8, 2024 + */ + +#pragma once + +#include + +/** The Real ring with addition and multiplication */ +struct Ring { + static inline double zero() { return 0.0; } + static inline double one() { return 1.0; } + static inline double add(const double& a, const double& b) { return a + b; } + static inline double max(const double& a, const double& b) { + return std::max(a, b); + } + static inline double mul(const double& a, const double& b) { return a * b; } + static inline double div(const double& a, const double& b) { + return (a == 0 || b == 0) ? 0 : (a / b); + } + static inline double id(const double& x) { return x; } + static inline double negate(const double& x) { return -x; } +}; diff --git a/gtsam/discrete/TableFactor.h b/gtsam/discrete/TableFactor.h index f0ecd66a3..7015353e1 100644 --- a/gtsam/discrete/TableFactor.h +++ b/gtsam/discrete/TableFactor.h @@ -19,6 +19,7 @@ #include #include +#include #include #include @@ -99,21 +100,6 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor { using Binary = std::function; public: - /** The Real ring with addition and multiplication */ - struct Ring { - static inline double zero() { return 0.0; } - static inline double one() { return 1.0; } - static inline double add(const double& a, const double& b) { return a + b; } - static inline double max(const double& a, const double& b) { - return std::max(a, b); - } - static inline double mul(const double& a, const double& b) { return a * b; } - static inline double div(const double& a, const double& b) { - return (a == 0 || b == 0) ? 0 : (a / b); - } - static inline double id(const double& x) { return x; } - }; - /// @name Standard Constructors /// @{ diff --git a/gtsam/discrete/tests/testDecisionTree.cpp b/gtsam/discrete/tests/testDecisionTree.cpp index 526001b51..c664fe6b5 100644 --- a/gtsam/discrete/tests/testDecisionTree.cpp +++ b/gtsam/discrete/tests/testDecisionTree.cpp @@ -23,6 +23,7 @@ #include #include #include +#include #include #include @@ -124,14 +125,6 @@ struct traits
: public Testable
{}; GTSAM_CONCEPT_TESTABLE_INST(DT) -struct Ring { - static inline int zero() { return 0; } - static inline int one() { return 1; } - static inline int id(const int& a) { return a; } - static inline int add(const int& a, const int& b) { return a + b; } - static inline int mul(const int& a, const int& b) { return a * b; } -}; - /* ************************************************************************** */ // Check that creating decision trees respects key order. TEST(DecisionTree, ConstructorOrder) { From 7bf8ee167d0405d5822c021ef5b3d2ccc4942ffa Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 8 Dec 2024 11:51:55 -0500 Subject: [PATCH 041/112] update DecisionTreeFactor.h --- gtsam/discrete/DecisionTreeFactor.h | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h index a8ab2644f..9a3cde96d 100644 --- a/gtsam/discrete/DecisionTreeFactor.h +++ b/gtsam/discrete/DecisionTreeFactor.h @@ -21,11 +21,12 @@ #include #include #include +#include #include #include -#include #include +#include #include #include #include @@ -145,7 +146,7 @@ namespace gtsam { /// multiply two factors DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override { - return apply(f, ADT::Ring::mul); + return apply(f, Ring::mul); } static double safe_div(const double& a, const double& b); @@ -160,22 +161,22 @@ namespace gtsam { /// Create new factor by summing all values with the same separator values shared_ptr sum(size_t nrFrontals) const { - return combine(nrFrontals, ADT::Ring::add); + return combine(nrFrontals, Ring::add); } /// Create new factor by summing all values with the same separator values shared_ptr sum(const Ordering& keys) const { - return combine(keys, ADT::Ring::add); + return combine(keys, Ring::add); } /// Create new factor by maximizing over all values with the same separator. shared_ptr max(size_t nrFrontals) const { - return combine(nrFrontals, ADT::Ring::max); + return combine(nrFrontals, Ring::max); } /// Create new factor by maximizing over all values with the same separator. shared_ptr max(const Ordering& keys) const { - return combine(keys, ADT::Ring::max); + return combine(keys, Ring::max); } /// @} From 699757d381758cc32c5da96e4b021b42c4e283c0 Mon Sep 17 00:00:00 2001 From: Jade Turner Date: Mon, 9 Dec 2024 15:17:16 +0800 Subject: [PATCH 042/112] Revert "[build] Allow disabling tests and examples" This reverts commit ddd95f4a51549d3c19cdca36d488da1323430676. --- CMakeLists.txt | 11 ++--------- 1 file changed, 2 insertions(+), 9 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index ffedf56e4..e3b462eec 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -112,17 +112,10 @@ add_subdirectory(CppUnitLite) add_subdirectory(gtsam) # Build Tests -option(BUILD_TESTS "Builds unit tests" ON) -if (BUILD_TESTS) - add_subdirectory(tests) -endif() - +add_subdirectory(tests) # Build examples -option(BUILD_EXAMPLES "Builds examples" ON) -if (BUILD_EXAMPLES) - add_subdirectory(examples) -endif() +add_subdirectory(examples) # Build timing add_subdirectory(timing) From f8d9da1b8edeebdba2acdfc2100da85d6b8ed2e0 Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Mon, 9 Dec 2024 06:57:35 -0800 Subject: [PATCH 043/112] actually fix CHECK_EQUAL --- CppUnitLite/Test.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CppUnitLite/Test.h b/CppUnitLite/Test.h index 9ee0d10eb..c3fa83234 100644 --- a/CppUnitLite/Test.h +++ b/CppUnitLite/Test.h @@ -129,7 +129,7 @@ protected: result_.addFailure(Failure(name_, __FILE__, __LINE__, #expected, #actual)); } #define CHECK_EQUAL(expected,actual)\ -{ if (!((expected) == (actual))) { result_.addFailure(Failure(name_, __FILE__, __LINE__, std::to_string(expected), std::to_string(actual))); } } +{ if (!((expected) == (actual))) { result_.addFailure(Failure(name_, __FILE__, __LINE__, std::to_string(expected), std::to_string(actual))); return; } } #define LONGS_EQUAL(expected,actual)\ { long actualTemp = actual; \ From a425e6e3d4c80f5553d045e54d4e7d7f60a7d258 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 9 Dec 2024 16:36:43 -0500 Subject: [PATCH 044/112] better docs --- gtsam/linear/KalmanFilter.h | 226 +++++++++++++++++++++++------------- 1 file changed, 146 insertions(+), 80 deletions(-) diff --git a/gtsam/linear/KalmanFilter.h b/gtsam/linear/KalmanFilter.h index 65fac877a..7fde9975d 100644 --- a/gtsam/linear/KalmanFilter.h +++ b/gtsam/linear/KalmanFilter.h @@ -11,10 +11,10 @@ /** * @file KalmanFilter.h - * @brief Simple linear Kalman filter. Implemented using factor graphs, i.e., does Cholesky-based SRIF, really. + * @brief Simple linear Kalman filter implemented using factor graphs, i.e., + * performs Cholesky or QR-based SRIF (Square-Root Information Filter). * @date Sep 3, 2011 - * @author Stephen Williams - * @author Frank Dellaert + * @authors Stephen Williams, Frank Dellaert */ #pragma once @@ -32,120 +32,186 @@ namespace gtsam { /** * Kalman Filter class * - * Knows how to maintain a Gaussian density under linear-Gaussian motion and - * measurement models. It uses the square-root information form, as usual in GTSAM. + * Maintains a Gaussian density under linear-Gaussian motion and + * measurement models using the square-root information form. * - * The filter is functional, in that it does not have state: you call init() to create - * an initial state, then predict() and update() that create new states out of an old state. + * The filter is functional; it does not maintain internal state. Instead: + * - Use `init()` to create an initial filter state, + * - Call `predict()` and `update()` to create new states. */ class GTSAM_EXPORT KalmanFilter { - -public: - + public: /** - * This Kalman filter is a Square-root Information filter - * The type below allows you to specify the factorization variant. + * @enum Factorization + * @brief Specifies the factorization variant to use. */ - enum Factorization { - QR, CHOLESKY - }; + enum Factorization { QR, CHOLESKY }; /** - * The Kalman filter state is simply a GaussianDensity + * @typedef State + * @brief The Kalman filter state, represented as a shared pointer to a + * GaussianDensity. */ typedef GaussianDensity::shared_ptr State; -private: - - const size_t n_; /** dimensionality of state */ - const Matrix I_; /** identity matrix of size n*n */ - const GaussianFactorGraph::Eliminate function_; /** algorithm */ - - State solve(const GaussianFactorGraph& factorGraph) const; - State fuse(const State& p, GaussianFactor::shared_ptr newFactor) const; - -public: - - // Constructor - KalmanFilter(size_t n, Factorization method = - KALMANFILTER_DEFAULT_FACTORIZATION) : - n_(n), I_(Matrix::Identity(n_, n_)), function_( - method == QR ? GaussianFactorGraph::Eliminate(EliminateQR) : - GaussianFactorGraph::Eliminate(EliminateCholesky)) { - } + private: + const size_t n_; ///< Dimensionality of the state. + const Matrix I_; ///< Identity matrix of size \f$ n \times n \f$. + const GaussianFactorGraph::Eliminate + function_; ///< Elimination algorithm used. /** - * Create initial state, i.e., prior density at time k=0 - * In Kalman Filter notation, these are x_{0|0} and P_{0|0} - * @param x0 estimate at time 0 - * @param P0 covariance at time 0, given as a diagonal Gaussian 'model' + * Solve the factor graph. + * @param factorGraph The Gaussian factor graph to solve. + * @return The resulting Kalman filter state. + */ + State solve(const GaussianFactorGraph& factorGraph) const; + + /** + * Fuse two states. + * @param p The prior state. + * @param newFactor The new factor to incorporate. + * @return The resulting fused state. + */ + State fuse(const State& p, GaussianFactor::shared_ptr newFactor) const; + + public: + /** + * Constructor. + * @param n Dimensionality of the state. + * @param method Factorization method (default: QR unless compile-flag set). + */ + KalmanFilter(size_t n, + Factorization method = KALMANFILTER_DEFAULT_FACTORIZATION) + : n_(n), + I_(Matrix::Identity(n_, n_)), + function_(method == QR + ? GaussianFactorGraph::Eliminate(EliminateQR) + : GaussianFactorGraph::Eliminate(EliminateCholesky)) {} + + /** + * Create the initial state (prior density at time \f$ k=0 \f$). + * + * In Kalman Filter notation: + * - \f$ x_{0|0} \f$: Initial state estimate. + * - \f$ P_{0|0} \f$: Initial covariance matrix. + * + * @param x0 Estimate of the state at time 0 (\f$ x_{0|0} \f$). + * @param P0 Covariance matrix (\f$ P_{0|0} \f$), given as a diagonal Gaussian + * model. + * @return Initial Kalman filter state. */ State init(const Vector& x0, const SharedDiagonal& P0) const; - /// version of init with a full covariance matrix + /** + * Create the initial state with a full covariance matrix. + * @param x0 Initial state estimate. + * @param P0 Full covariance matrix. + * @return Initial Kalman filter state. + */ State init(const Vector& x0, const Matrix& P0) const; - /// print + /** + * Print the Kalman filter details. + * @param s Optional string prefix. + */ void print(const std::string& s = "") const; - /** Return step index k, starts at 0, incremented at each predict. */ - static Key step(const State& p) { - return p->firstFrontalKey(); - } + /** + * Return the step index \f$ k \f$ (starts at 0, incremented at each predict + * step). + * @param p The current state. + * @return Step index. + */ + static Key step(const State& p) { return p->firstFrontalKey(); } /** - * Predict the state P(x_{t+1}|Z^t) - * In Kalman Filter notation, this is x_{t+1|t} and P_{t+1|t} - * Details and parameters: - * In a linear Kalman Filter, the motion model is f(x_{t}) = F*x_{t} + B*u_{t} + w - * where F is the state transition model/matrix, B is the control input model, - * and w is zero-mean, Gaussian white noise with covariance Q. + * Predict the next state \f$ P(x_{k+1}|Z^k) \f$. + * + * In Kalman Filter notation: + * - \f$ x_{k+1|k} \f$: Predicted state. + * - \f$ P_{k+1|k} \f$: Predicted covariance. + * + * Motion model: + * \f[ + * x_{k+1} = F \cdot x_k + B \cdot u_k + w + * \f] + * where \f$ w \f$ is zero-mean Gaussian noise with covariance \f$ Q \f$. + * + * @param p Previous state (\f$ x_k \f$). + * @param F State transition matrix (\f$ F \f$). + * @param B Control input matrix (\f$ B \f$). + * @param u Control vector (\f$ u_k \f$). + * @param modelQ Noise model (\f$ Q \f$, diagonal Gaussian). + * @return Predicted state (\f$ x_{k+1|k} \f$). */ State predict(const State& p, const Matrix& F, const Matrix& B, - const Vector& u, const SharedDiagonal& modelQ) const; + const Vector& u, const SharedDiagonal& modelQ) const; - /* - * Version of predict with full covariance - * Q is normally derived as G*w*G^T where w models uncertainty of some - * physical property, such as velocity or acceleration, and G is derived from physics. - * This version allows more realistic models than a diagonal covariance matrix. + /** + * Predict the next state with a full covariance matrix. + * + *@note Q is normally derived as G*w*G^T where w models uncertainty of some + * physical property, such as velocity or acceleration, and G is derived from + * physics. This version allows more realistic models than a diagonal matrix. + * + * @param p Previous state. + * @param F State transition matrix. + * @param B Control input matrix. + * @param u Control vector. + * @param Q Full covariance matrix (\f$ Q \f$). + * @return Predicted state. */ State predictQ(const State& p, const Matrix& F, const Matrix& B, - const Vector& u, const Matrix& Q) const; + const Vector& u, const Matrix& Q) const; /** - * Predict the state P(x_{t+1}|Z^t) - * In Kalman Filter notation, this is x_{t+1|t} and P_{t+1|t} - * After the call, that is the density that can be queried. - * Details and parameters: - * This version of predict takes GaussianFactor motion model [A0 A1 b] - * with an optional noise model. + * Predict the next state using a GaussianFactor motion model. + * @param p Previous state. + * @param A0 Factor matrix. + * @param A1 Factor matrix. + * @param b Constant term vector. + * @param model Noise model (optional). + * @return Predicted state. */ State predict2(const State& p, const Matrix& A0, const Matrix& A1, - const Vector& b, const SharedDiagonal& model) const; + const Vector& b, const SharedDiagonal& model = nullptr) const; /** - * Update Kalman filter with a measurement - * For the Kalman Filter, the measurement function, h(x_{t}) = z_{t} - * will be of the form h(x_{t}) = H*x_{t} + v - * where H is the observation model/matrix, and v is zero-mean, - * Gaussian white noise with covariance R. + * Update the Kalman filter with a measurement. + * + * Observation model: + * \f[ + * z_k = H \cdot x_k + v + * \f] + * where \f$ v \f$ is zero-mean Gaussian noise with covariance R. * In this version, R is restricted to diagonal Gaussians (model parameter) + * + * @param p Previous state. + * @param H Observation matrix. + * @param z Measurement vector. + * @param model Noise model (diagonal Gaussian). + * @return Updated state. */ State update(const State& p, const Matrix& H, const Vector& z, - const SharedDiagonal& model) const; + const SharedDiagonal& model) const; - /* - * Version of update with full covariance - * Q is normally derived as G*w*G^T where w models uncertainty of some - * physical property, such as velocity or acceleration, and G is derived from physics. - * This version allows more realistic models than a diagonal covariance matrix. + /** + * Update the Kalman filter with a measurement using a full covariance matrix. + * @param p Previous state. + * @param H Observation matrix. + * @param z Measurement vector. + * @param R Full covariance matrix. + * @return Updated state. */ State updateQ(const State& p, const Matrix& H, const Vector& z, - const Matrix& Q) const; + const Matrix& R) const; + + /** + * Return the dimensionality of the state. + * @return Dimensionality of the state. + */ + size_t dim() const { return n_; } }; -} // \namespace gtsam - -/* ************************************************************************* */ - +} // namespace gtsam \ No newline at end of file From 1ae3fc2ad80ff27e499ea49ac942eff6ffef0910 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 9 Dec 2024 16:38:15 -0500 Subject: [PATCH 045/112] Use LLT rather than inverse --- gtsam/linear/KalmanFilter.cpp | 120 ++++++++++++++++++++-------------- 1 file changed, 70 insertions(+), 50 deletions(-) diff --git a/gtsam/linear/KalmanFilter.cpp b/gtsam/linear/KalmanFilter.cpp index ded6bc5e3..12a43093c 100644 --- a/gtsam/linear/KalmanFilter.cpp +++ b/gtsam/linear/KalmanFilter.cpp @@ -20,26 +20,35 @@ * @author Frank Dellaert */ -#include +#include #include #include -#include -#include - +#include using namespace std; namespace gtsam { -/* ************************************************************************* */ -// Auxiliary function to solve factor graph and return pointer to root conditional -KalmanFilter::State // -KalmanFilter::solve(const GaussianFactorGraph& factorGraph) const { +// In the code below we often need to multiply matrices with R, the Cholesky +// factor of the information matrix Q^{-1}, when given a covariance matrix Q. +// We can do this efficiently using Eigen, as we have: +// Q = L L^T +// => Q^{-1} = L^{-T}L^{-1} = R^T R +// => L^{-1} = R +// Rather than explicitly calculate R and do R*A, we should use L.solve(A) +// The following macro makes L available in the scope where it is used: +#define FACTORIZE_Q_INTO_L(Q) \ + Eigen::LLT llt(Q); \ + auto L = llt.matrixL(); +/* ************************************************************************* */ +// Auxiliary function to solve factor graph and return pointer to root +// conditional +KalmanFilter::State KalmanFilter::solve( + const GaussianFactorGraph& factorGraph) const { // Eliminate the graph using the provided Eliminate function Ordering ordering(factorGraph.keys()); - const auto bayesNet = // - factorGraph.eliminateSequential(ordering, function_); + const auto bayesNet = factorGraph.eliminateSequential(ordering, function_); // As this is a filter, all we need is the posterior P(x_t). // This is the last GaussianConditional in the resulting BayesNet @@ -49,9 +58,8 @@ KalmanFilter::solve(const GaussianFactorGraph& factorGraph) const { /* ************************************************************************* */ // Auxiliary function to create a small graph for predict or update and solve -KalmanFilter::State // -KalmanFilter::fuse(const State& p, GaussianFactor::shared_ptr newFactor) const { - +KalmanFilter::State KalmanFilter::fuse( + const State& p, GaussianFactor::shared_ptr newFactor) const { // Create a factor graph GaussianFactorGraph factorGraph; factorGraph.push_back(p); @@ -63,20 +71,27 @@ KalmanFilter::fuse(const State& p, GaussianFactor::shared_ptr newFactor) const { /* ************************************************************************* */ KalmanFilter::State KalmanFilter::init(const Vector& x0, - const SharedDiagonal& P0) const { - + const SharedDiagonal& P0) const { // Create a factor graph f(x0), eliminate it into P(x0) GaussianFactorGraph factorGraph; - factorGraph.emplace_shared(0, I_, x0, P0); // |x-x0|^2_diagSigma + factorGraph.emplace_shared(0, I_, x0, + P0); // |x-x0|^2_P0 return solve(factorGraph); } /* ************************************************************************* */ -KalmanFilter::State KalmanFilter::init(const Vector& x, const Matrix& P0) const { - +KalmanFilter::State KalmanFilter::init(const Vector& x0, + const Matrix& P0) const { // Create a factor graph f(x0), eliminate it into P(x0) GaussianFactorGraph factorGraph; - factorGraph.emplace_shared(0, x, P0); // 0.5*(x-x0)'*inv(Sigma)*(x-x0) + + // Perform Cholesky decomposition of P0 + FACTORIZE_Q_INTO_L(P0) + + // Premultiply I and x0 with R + const Matrix R = L.solve(I_); // = R + const Vector b = L.solve(x0); // = R*x0 + factorGraph.emplace_shared(0, R, b); return solve(factorGraph); } @@ -87,21 +102,23 @@ void KalmanFilter::print(const string& s) const { /* ************************************************************************* */ KalmanFilter::State KalmanFilter::predict(const State& p, const Matrix& F, - const Matrix& B, const Vector& u, const SharedDiagonal& model) const { - + const Matrix& B, const Vector& u, + const SharedDiagonal& model) const { // The factor related to the motion model is defined as - // f2(x_{t},x_{t+1}) = (F*x_{t} + B*u - x_{t+1}) * Q^-1 * (F*x_{t} + B*u - x_{t+1})^T + // f2(x_{k},x_{k+1}) = + // (F*x_{k} + B*u - x_{k+1}) * Q^-1 * (F*x_{k} + B*u - x_{k+1})^T Key k = step(p); return fuse(p, - std::make_shared(k, -F, k + 1, I_, B * u, model)); + std::make_shared(k, -F, k + 1, I_, B * u, model)); } /* ************************************************************************* */ KalmanFilter::State KalmanFilter::predictQ(const State& p, const Matrix& F, - const Matrix& B, const Vector& u, const Matrix& Q) const { - + const Matrix& B, const Vector& u, + const Matrix& Q) const { #ifndef NDEBUG - DenseIndex n = F.cols(); + const DenseIndex n = dim(); + assert(F.cols() == n); assert(F.rows() == n); assert(B.rows() == n); assert(B.cols() == u.size()); @@ -109,50 +126,53 @@ KalmanFilter::State KalmanFilter::predictQ(const State& p, const Matrix& F, assert(Q.cols() == n); #endif - // The factor related to the motion model is defined as - // f2(x_{t},x_{t+1}) = (F*x_{t} + B*u - x_{t+1}) * Q^-1 * (F*x_{t} + B*u - x_{t+1})^T - // See documentation in HessianFactor, we have A1 = -F, A2 = I_, b = B*u: - // TODO: starts to seem more elaborate than straight-up KF equations? - Matrix M = Q.inverse(), Ft = trans(F); - Matrix G12 = -Ft * M, G11 = -G12 * F, G22 = M; - Vector b = B * u, g2 = M * b, g1 = -Ft * g2; - double f = dot(b, g2); - Key k = step(p); - return fuse(p, - std::make_shared(k, k + 1, G11, G12, g1, G22, g2, f)); + // Perform Cholesky decomposition of Q + FACTORIZE_Q_INTO_L(Q) + + // Premultiply A1 = -F and A2 = I, b = B * u with R + const Matrix A1 = -L.solve(F); // = -R*F + const Matrix A2 = L.solve(I_); // = R + const Vector b = L.solve(B * u); // = R * (B * u) + return predict2(p, A1, A2, b); } /* ************************************************************************* */ KalmanFilter::State KalmanFilter::predict2(const State& p, const Matrix& A0, - const Matrix& A1, const Vector& b, const SharedDiagonal& model) const { + const Matrix& A1, const Vector& b, + const SharedDiagonal& model) const { // Nhe factor related to the motion model is defined as - // f2(x_{t},x_{t+1}) = |A0*x_{t} + A1*x_{t+1} - b|^2 + // f2(x_{k},x_{k+1}) = |A0*x_{k} + A1*x_{k+1} - b|^2 Key k = step(p); return fuse(p, std::make_shared(k, A0, k + 1, A1, b, model)); } /* ************************************************************************* */ KalmanFilter::State KalmanFilter::update(const State& p, const Matrix& H, - const Vector& z, const SharedDiagonal& model) const { + const Vector& z, + const SharedDiagonal& model) const { // The factor related to the measurements would be defined as - // f2 = (h(x_{t}) - z_{t}) * R^-1 * (h(x_{t}) - z_{t})^T - // = (x_{t} - z_{t}) * R^-1 * (x_{t} - z_{t})^T + // f2 = (h(x_{k}) - z_{k}) * R^-1 * (h(x_{k}) - z_{k})^T + // = (x_{k} - z_{k}) * R^-1 * (x_{k} - z_{k})^T Key k = step(p); return fuse(p, std::make_shared(k, H, z, model)); } /* ************************************************************************* */ KalmanFilter::State KalmanFilter::updateQ(const State& p, const Matrix& H, - const Vector& z, const Matrix& Q) const { + const Vector& z, + const Matrix& Q) const { Key k = step(p); - Matrix M = Q.inverse(), Ht = trans(H); - Matrix G = Ht * M * H; - Vector g = Ht * M * z; - double f = dot(z, M * z); - return fuse(p, std::make_shared(k, G, g, f)); + + // Perform Cholesky decomposition of Q + FACTORIZE_Q_INTO_L(Q) + + // Pre-multiply H and z with R, respectively + const Matrix RtH = L.solve(H); // = R * H + const Vector b = L.solve(z); // = R * z + + return fuse(p, std::make_shared(k, RtH, b)); } /* ************************************************************************* */ -} // \namespace gtsam - +} // namespace gtsam From 88b36da602c9697ca86db5d42921fa6d058cc1af Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 9 Dec 2024 16:55:15 -0500 Subject: [PATCH 046/112] make evaluate a common base method --- gtsam/discrete/DecisionTreeFactor.h | 9 ++------- gtsam/discrete/DiscreteFactor.h | 15 ++++++++++++++- gtsam/discrete/TableFactor.cpp | 3 ++- gtsam/discrete/TableFactor.h | 10 ++-------- gtsam_unstable/discrete/AllDiff.cpp | 2 +- gtsam_unstable/discrete/AllDiff.h | 2 +- gtsam_unstable/discrete/BinaryAllDiff.h | 2 +- gtsam_unstable/discrete/Domain.cpp | 2 +- gtsam_unstable/discrete/Domain.h | 2 +- gtsam_unstable/discrete/SingleValue.cpp | 2 +- gtsam_unstable/discrete/SingleValue.h | 2 +- 11 files changed, 27 insertions(+), 24 deletions(-) diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h index 9a3cde96d..8f3dd85a6 100644 --- a/gtsam/discrete/DecisionTreeFactor.h +++ b/gtsam/discrete/DecisionTreeFactor.h @@ -130,14 +130,9 @@ namespace gtsam { /// @name Standard Interface /// @{ - /// Calculate probability for given values `x`, + /// Calculate probability for given values, /// is just look up in AlgebraicDecisionTree. - double evaluate(const Assignment& values) const { - return ADT::operator()(values); - } - - /// Evaluate probability distribution, sugar. - double operator()(const DiscreteValues& values) const override { + double operator()(const Assignment& values) const override { return ADT::operator()(values); } diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h index 19af5bd13..5bc3c463f 100644 --- a/gtsam/discrete/DiscreteFactor.h +++ b/gtsam/discrete/DiscreteFactor.h @@ -92,8 +92,21 @@ class GTSAM_EXPORT DiscreteFactor : public Factor { size_t cardinality(Key j) const { return cardinalities_.at(j); } + /** + * @brief Calculate probability for given values. + * Calls specialized evaluation under the hood. + * + * Note: Uses Assignment as it is the base class of DiscreteValues. + * + * @param values Discrete assignment. + * @return double + */ + double evaluate(const Assignment& values) const { + return operator()(values); + } + /// Find value for given assignment of values to variables - virtual double operator()(const DiscreteValues&) const = 0; + virtual double operator()(const Assignment& values) const = 0; /// Error is just -log(value) virtual double error(const DiscreteValues& values) const; diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp index f4e023a4d..32cba84ed 100644 --- a/gtsam/discrete/TableFactor.cpp +++ b/gtsam/discrete/TableFactor.cpp @@ -133,7 +133,7 @@ bool TableFactor::equals(const DiscreteFactor& other, double tol) const { } /* ************************************************************************ */ -double TableFactor::operator()(const DiscreteValues& values) const { +double TableFactor::operator()(const Assignment& values) const { // a b c d => D * (C * (B * (a) + b) + c) + d uint64_t idx = 0, card = 1; for (auto it = sorted_dkeys_.rbegin(); it != sorted_dkeys_.rend(); ++it) { @@ -180,6 +180,7 @@ DecisionTreeFactor TableFactor::toDecisionTreeFactor() const { for (auto i = 0; i < sparse_table_.size(); i++) { table.push_back(sparse_table_.coeff(i)); } + // NOTE(Varun): This constructor is really expensive!! DecisionTreeFactor f(dkeys, table); return f; } diff --git a/gtsam/discrete/TableFactor.h b/gtsam/discrete/TableFactor.h index 7015353e1..53495d078 100644 --- a/gtsam/discrete/TableFactor.h +++ b/gtsam/discrete/TableFactor.h @@ -155,14 +155,8 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor { // /// @name Standard Interface // /// @{ - /// Calculate probability for given values `x`, - /// is just look up in TableFactor. - double evaluate(const DiscreteValues& values) const { - return operator()(values); - } - - /// Evaluate probability distribution, sugar. - double operator()(const DiscreteValues& values) const override; + /// Evaluate probability distribution, is just look up in TableFactor. + double operator()(const Assignment& values) const override; /// Calculate error for DiscreteValues `x`, is -log(probability). double error(const DiscreteValues& values) const override; diff --git a/gtsam_unstable/discrete/AllDiff.cpp b/gtsam_unstable/discrete/AllDiff.cpp index 2bd9e6dfd..a450605b3 100644 --- a/gtsam_unstable/discrete/AllDiff.cpp +++ b/gtsam_unstable/discrete/AllDiff.cpp @@ -26,7 +26,7 @@ void AllDiff::print(const std::string& s, const KeyFormatter& formatter) const { } /* ************************************************************************* */ -double AllDiff::operator()(const DiscreteValues& values) const { +double AllDiff::operator()(const Assignment& values) const { std::set taken; // record values taken by keys for (Key dkey : keys_) { size_t value = values.at(dkey); // get the value for that key diff --git a/gtsam_unstable/discrete/AllDiff.h b/gtsam_unstable/discrete/AllDiff.h index d7a63eae0..6fe43568e 100644 --- a/gtsam_unstable/discrete/AllDiff.h +++ b/gtsam_unstable/discrete/AllDiff.h @@ -45,7 +45,7 @@ class GTSAM_UNSTABLE_EXPORT AllDiff : public Constraint { } /// Calculate value = expensive ! - double operator()(const DiscreteValues& values) const override; + double operator()(const Assignment& values) const override; /// Convert into a decisiontree, can be *very* expensive ! DecisionTreeFactor toDecisionTreeFactor() const override; diff --git a/gtsam_unstable/discrete/BinaryAllDiff.h b/gtsam_unstable/discrete/BinaryAllDiff.h index 18b335092..a55865c77 100644 --- a/gtsam_unstable/discrete/BinaryAllDiff.h +++ b/gtsam_unstable/discrete/BinaryAllDiff.h @@ -47,7 +47,7 @@ class BinaryAllDiff : public Constraint { } /// Calculate value - double operator()(const DiscreteValues& values) const override { + double operator()(const Assignment& values) const override { return (double)(values.at(keys_[0]) != values.at(keys_[1])); } diff --git a/gtsam_unstable/discrete/Domain.cpp b/gtsam_unstable/discrete/Domain.cpp index bbbc87667..752228c18 100644 --- a/gtsam_unstable/discrete/Domain.cpp +++ b/gtsam_unstable/discrete/Domain.cpp @@ -30,7 +30,7 @@ string Domain::base1Str() const { } /* ************************************************************************* */ -double Domain::operator()(const DiscreteValues& values) const { +double Domain::operator()(const Assignment& values) const { return contains(values.at(key())); } diff --git a/gtsam_unstable/discrete/Domain.h b/gtsam_unstable/discrete/Domain.h index 7f7b717c2..13249d733 100644 --- a/gtsam_unstable/discrete/Domain.h +++ b/gtsam_unstable/discrete/Domain.h @@ -82,7 +82,7 @@ class GTSAM_UNSTABLE_EXPORT Domain : public Constraint { bool contains(size_t value) const { return values_.count(value) > 0; } /// Calculate value - double operator()(const DiscreteValues& values) const override; + double operator()(const Assignment& values) const override; /// Convert into a decisiontree DecisionTreeFactor toDecisionTreeFactor() const override; diff --git a/gtsam_unstable/discrete/SingleValue.cpp b/gtsam_unstable/discrete/SingleValue.cpp index 6b78f38f5..9762aec0f 100644 --- a/gtsam_unstable/discrete/SingleValue.cpp +++ b/gtsam_unstable/discrete/SingleValue.cpp @@ -22,7 +22,7 @@ void SingleValue::print(const string& s, const KeyFormatter& formatter) const { } /* ************************************************************************* */ -double SingleValue::operator()(const DiscreteValues& values) const { +double SingleValue::operator()(const Assignment& values) const { return (double)(values.at(keys_[0]) == value_); } diff --git a/gtsam_unstable/discrete/SingleValue.h b/gtsam_unstable/discrete/SingleValue.h index 3f7f22d6a..93fe38aaa 100644 --- a/gtsam_unstable/discrete/SingleValue.h +++ b/gtsam_unstable/discrete/SingleValue.h @@ -55,7 +55,7 @@ class GTSAM_UNSTABLE_EXPORT SingleValue : public Constraint { } /// Calculate value - double operator()(const DiscreteValues& values) const override; + double operator()(const Assignment& values) const override; /// Convert into a decisiontree DecisionTreeFactor toDecisionTreeFactor() const override; From e0fedda712808b557ef229b2ec4cda50f25fb743 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 9 Dec 2024 17:22:51 -0500 Subject: [PATCH 047/112] make the Unary and Binary ops common --- gtsam/discrete/DecisionTreeFactor.cpp | 12 ++++++------ gtsam/discrete/DecisionTreeFactor.h | 15 ++++++++++----- gtsam/discrete/DiscreteFactor.h | 5 +++++ gtsam/discrete/TableFactor.h | 4 ---- 4 files changed, 21 insertions(+), 15 deletions(-) diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp index 9ec3b0ac5..776d4bd90 100644 --- a/gtsam/discrete/DecisionTreeFactor.cpp +++ b/gtsam/discrete/DecisionTreeFactor.cpp @@ -83,7 +83,7 @@ namespace gtsam { } /* ************************************************************************ */ - DecisionTreeFactor DecisionTreeFactor::apply(ADT::Unary op) const { + DecisionTreeFactor DecisionTreeFactor::apply(Unary op) const { // apply operand ADT result = ADT::apply(op); // Make a new factor @@ -91,7 +91,7 @@ namespace gtsam { } /* ************************************************************************ */ - DecisionTreeFactor DecisionTreeFactor::apply(ADT::UnaryAssignment op) const { + DecisionTreeFactor DecisionTreeFactor::apply(UnaryAssignment op) const { // apply operand ADT result = ADT::apply(op); // Make a new factor @@ -100,7 +100,7 @@ namespace gtsam { /* ************************************************************************ */ DecisionTreeFactor DecisionTreeFactor::apply(const DecisionTreeFactor& f, - ADT::Binary op) const { + Binary op) const { map cs; // new cardinalities // make unique key-cardinality map for (Key j : keys()) cs[j] = cardinality(j); @@ -118,8 +118,8 @@ namespace gtsam { } /* ************************************************************************ */ - DecisionTreeFactor::shared_ptr DecisionTreeFactor::combine( - size_t nrFrontals, ADT::Binary op) const { + DecisionTreeFactor::shared_ptr DecisionTreeFactor::combine(size_t nrFrontals, + Binary op) const { if (nrFrontals > size()) { throw invalid_argument( "DecisionTreeFactor::combine: invalid number of frontal " @@ -146,7 +146,7 @@ namespace gtsam { /* ************************************************************************ */ DecisionTreeFactor::shared_ptr DecisionTreeFactor::combine( - const Ordering& frontalKeys, ADT::Binary op) const { + const Ordering& frontalKeys, Binary op) const { if (frontalKeys.size() > size()) { throw invalid_argument( "DecisionTreeFactor::combine: invalid number of frontal " diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h index 8f3dd85a6..9172180b1 100644 --- a/gtsam/discrete/DecisionTreeFactor.h +++ b/gtsam/discrete/DecisionTreeFactor.h @@ -51,6 +51,11 @@ namespace gtsam { typedef std::shared_ptr shared_ptr; typedef AlgebraicDecisionTree ADT; + // Needed since we have definitions in both DiscreteFactor and DecisionTree + using Base::Binary; + using Base::Unary; + using Base::UnaryAssignment; + /// @name Standard Constructors /// @{ @@ -182,21 +187,21 @@ namespace gtsam { * Apply unary operator (*this) "op" f * @param op a unary operator that operates on AlgebraicDecisionTree */ - DecisionTreeFactor apply(ADT::Unary op) const; + DecisionTreeFactor apply(Unary op) const; /** * Apply unary operator (*this) "op" f * @param op a unary operator that operates on AlgebraicDecisionTree. Takes * both the assignment and the value. */ - DecisionTreeFactor apply(ADT::UnaryAssignment op) const; + DecisionTreeFactor apply(UnaryAssignment op) const; /** * Apply binary operator (*this) "op" f * @param f the second argument for op * @param op a binary operator that operates on AlgebraicDecisionTree */ - DecisionTreeFactor apply(const DecisionTreeFactor& f, ADT::Binary op) const; + DecisionTreeFactor apply(const DecisionTreeFactor& f, Binary op) const; /** * Combine frontal variables using binary operator "op" @@ -204,7 +209,7 @@ namespace gtsam { * @param op a binary operator that operates on AlgebraicDecisionTree * @return shared pointer to newly created DecisionTreeFactor */ - shared_ptr combine(size_t nrFrontals, ADT::Binary op) const; + shared_ptr combine(size_t nrFrontals, Binary op) const; /** * Combine frontal variables in an Ordering using binary operator "op" @@ -212,7 +217,7 @@ namespace gtsam { * @param op a binary operator that operates on AlgebraicDecisionTree * @return shared pointer to newly created DecisionTreeFactor */ - shared_ptr combine(const Ordering& keys, ADT::Binary op) const; + shared_ptr combine(const Ordering& keys, Binary op) const; /// Enumerate all values into a map from values to double. std::vector> enumerate() const; diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h index 5bc3c463f..7175f7608 100644 --- a/gtsam/discrete/DiscreteFactor.h +++ b/gtsam/discrete/DiscreteFactor.h @@ -46,6 +46,11 @@ class GTSAM_EXPORT DiscreteFactor : public Factor { using Values = DiscreteValues; ///< backwards compatibility + using Unary = std::function; + using UnaryAssignment = + std::function&, const double&)>; + using Binary = std::function; + protected: /// Map of Keys and their cardinalities. std::map cardinalities_; diff --git a/gtsam/discrete/TableFactor.h b/gtsam/discrete/TableFactor.h index 53495d078..e7ed2294f 100644 --- a/gtsam/discrete/TableFactor.h +++ b/gtsam/discrete/TableFactor.h @@ -94,10 +94,6 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor { typedef std::shared_ptr shared_ptr; typedef Eigen::SparseVector::InnerIterator SparseIt; typedef std::vector> AssignValList; - using Unary = std::function; - using UnaryAssignment = - std::function&, const double&)>; - using Binary = std::function; public: /// @name Standard Constructors From 1152470800d2f7845343b20ce592e32837564b6a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 9 Dec 2024 17:37:11 -0500 Subject: [PATCH 048/112] fix wrapper --- gtsam/discrete/discrete.i | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/gtsam/discrete/discrete.i b/gtsam/discrete/discrete.i index 55c8f9e22..d82ba9794 100644 --- a/gtsam/discrete/discrete.i +++ b/gtsam/discrete/discrete.i @@ -41,7 +41,7 @@ virtual class DiscreteFactor : gtsam::Factor { const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::DiscreteFactor& other, double tol = 1e-9) const; - double operator()(const gtsam::DiscreteValues& values) const; + double operator()(const gtsam::Assignment& values) const; }; #include @@ -69,7 +69,7 @@ virtual class DecisionTreeFactor : gtsam::DiscreteFactor { size_t cardinality(gtsam::Key j) const; - double operator()(const gtsam::DiscreteValues& values) const; + double operator()(const gtsam::Assignment& values) const; gtsam::DecisionTreeFactor operator*(const gtsam::DecisionTreeFactor& f) const; size_t cardinality(gtsam::Key j) const; gtsam::DecisionTreeFactor operator/(const gtsam::DecisionTreeFactor& f) const; @@ -248,7 +248,6 @@ class DiscreteBayesTree { void saveGraph(string s, const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; - double operator()(const gtsam::DiscreteValues& values) const; string markdown(const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; From ab943b539ec3f7c186aa40be2579407fa320278d Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 9 Dec 2024 18:21:26 -0500 Subject: [PATCH 049/112] Revert "fix wrapper" This reverts commit 1152470800d2f7845343b20ce592e32837564b6a. --- gtsam/discrete/discrete.i | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/gtsam/discrete/discrete.i b/gtsam/discrete/discrete.i index d82ba9794..55c8f9e22 100644 --- a/gtsam/discrete/discrete.i +++ b/gtsam/discrete/discrete.i @@ -41,7 +41,7 @@ virtual class DiscreteFactor : gtsam::Factor { const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::DiscreteFactor& other, double tol = 1e-9) const; - double operator()(const gtsam::Assignment& values) const; + double operator()(const gtsam::DiscreteValues& values) const; }; #include @@ -69,7 +69,7 @@ virtual class DecisionTreeFactor : gtsam::DiscreteFactor { size_t cardinality(gtsam::Key j) const; - double operator()(const gtsam::Assignment& values) const; + double operator()(const gtsam::DiscreteValues& values) const; gtsam::DecisionTreeFactor operator*(const gtsam::DecisionTreeFactor& f) const; size_t cardinality(gtsam::Key j) const; gtsam::DecisionTreeFactor operator/(const gtsam::DecisionTreeFactor& f) const; @@ -248,6 +248,7 @@ class DiscreteBayesTree { void saveGraph(string s, const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; + double operator()(const gtsam::DiscreteValues& values) const; string markdown(const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; From a98ac0fdb232e8bf50b48df7008565010bfd0a4f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 9 Dec 2024 21:09:00 -0500 Subject: [PATCH 050/112] make evaluate the overriden method --- gtsam/discrete/DecisionTreeFactor.cpp | 2 +- gtsam/discrete/DecisionTreeFactor.h | 5 ++++- gtsam/discrete/DiscreteConditional.h | 4 ++-- gtsam/discrete/DiscreteFactor.h | 8 ++++---- gtsam/discrete/TableFactor.cpp | 2 +- gtsam/discrete/TableFactor.h | 2 +- gtsam_unstable/discrete/AllDiff.cpp | 2 +- gtsam_unstable/discrete/AllDiff.h | 2 +- gtsam_unstable/discrete/BinaryAllDiff.h | 2 +- gtsam_unstable/discrete/Domain.cpp | 2 +- gtsam_unstable/discrete/Domain.h | 2 +- gtsam_unstable/discrete/SingleValue.cpp | 2 +- gtsam_unstable/discrete/SingleValue.h | 2 +- 13 files changed, 20 insertions(+), 17 deletions(-) diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp index 9ec3b0ac5..a57915e45 100644 --- a/gtsam/discrete/DecisionTreeFactor.cpp +++ b/gtsam/discrete/DecisionTreeFactor.cpp @@ -195,7 +195,7 @@ namespace gtsam { // Construct unordered_map with values std::vector> result; for (const auto& assignment : assignments) { - result.emplace_back(assignment, operator()(assignment)); + result.emplace_back(assignment, evaluate(assignment)); } return result; } diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h index 8f3dd85a6..741715e43 100644 --- a/gtsam/discrete/DecisionTreeFactor.h +++ b/gtsam/discrete/DecisionTreeFactor.h @@ -132,10 +132,13 @@ namespace gtsam { /// Calculate probability for given values, /// is just look up in AlgebraicDecisionTree. - double operator()(const Assignment& values) const override { + virtual double evaluate(const Assignment& values) const override { return ADT::operator()(values); } + /// Disambiguate to use DiscreteFactor version. Mainly for wrapper + using DiscreteFactor::operator(); + /// Calculate error for DiscreteValues `x`, is -log(probability). double error(const DiscreteValues& values) const override; diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h index f59e29285..858623301 100644 --- a/gtsam/discrete/DiscreteConditional.h +++ b/gtsam/discrete/DiscreteConditional.h @@ -169,12 +169,12 @@ class GTSAM_EXPORT DiscreteConditional } /// Evaluate, just look up in AlgebraicDecisionTree - double evaluate(const DiscreteValues& values) const { + virtual double evaluate(const Assignment& values) const override { return ADT::operator()(values); } using DecisionTreeFactor::error; ///< DiscreteValues version - using DecisionTreeFactor::operator(); ///< DiscreteValues version + using DiscreteFactor::operator(); ///< DiscreteValues version /** * @brief restrict to given *parent* values. diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h index 5bc3c463f..2ba670004 100644 --- a/gtsam/discrete/DiscreteFactor.h +++ b/gtsam/discrete/DiscreteFactor.h @@ -101,12 +101,12 @@ class GTSAM_EXPORT DiscreteFactor : public Factor { * @param values Discrete assignment. * @return double */ - double evaluate(const Assignment& values) const { - return operator()(values); - } + virtual double evaluate(const Assignment& values) const = 0; /// Find value for given assignment of values to variables - virtual double operator()(const Assignment& values) const = 0; + double operator()(const DiscreteValues& values) const { + return evaluate(values); + } /// Error is just -log(value) virtual double error(const DiscreteValues& values) const; diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp index 32cba84ed..ea51a996c 100644 --- a/gtsam/discrete/TableFactor.cpp +++ b/gtsam/discrete/TableFactor.cpp @@ -133,7 +133,7 @@ bool TableFactor::equals(const DiscreteFactor& other, double tol) const { } /* ************************************************************************ */ -double TableFactor::operator()(const Assignment& values) const { +double TableFactor::evaluate(const Assignment& values) const { // a b c d => D * (C * (B * (a) + b) + c) + d uint64_t idx = 0, card = 1; for (auto it = sorted_dkeys_.rbegin(); it != sorted_dkeys_.rend(); ++it) { diff --git a/gtsam/discrete/TableFactor.h b/gtsam/discrete/TableFactor.h index 53495d078..1aecc1669 100644 --- a/gtsam/discrete/TableFactor.h +++ b/gtsam/discrete/TableFactor.h @@ -156,7 +156,7 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor { // /// @{ /// Evaluate probability distribution, is just look up in TableFactor. - double operator()(const Assignment& values) const override; + double evaluate(const Assignment& values) const override; /// Calculate error for DiscreteValues `x`, is -log(probability). double error(const DiscreteValues& values) const override; diff --git a/gtsam_unstable/discrete/AllDiff.cpp b/gtsam_unstable/discrete/AllDiff.cpp index a450605b3..585ca8103 100644 --- a/gtsam_unstable/discrete/AllDiff.cpp +++ b/gtsam_unstable/discrete/AllDiff.cpp @@ -26,7 +26,7 @@ void AllDiff::print(const std::string& s, const KeyFormatter& formatter) const { } /* ************************************************************************* */ -double AllDiff::operator()(const Assignment& values) const { +double AllDiff::evaluate(const Assignment& values) const { std::set taken; // record values taken by keys for (Key dkey : keys_) { size_t value = values.at(dkey); // get the value for that key diff --git a/gtsam_unstable/discrete/AllDiff.h b/gtsam_unstable/discrete/AllDiff.h index 6fe43568e..1180abad4 100644 --- a/gtsam_unstable/discrete/AllDiff.h +++ b/gtsam_unstable/discrete/AllDiff.h @@ -45,7 +45,7 @@ class GTSAM_UNSTABLE_EXPORT AllDiff : public Constraint { } /// Calculate value = expensive ! - double operator()(const Assignment& values) const override; + double evaluate(const Assignment& values) const override; /// Convert into a decisiontree, can be *very* expensive ! DecisionTreeFactor toDecisionTreeFactor() const override; diff --git a/gtsam_unstable/discrete/BinaryAllDiff.h b/gtsam_unstable/discrete/BinaryAllDiff.h index a55865c77..e96bfdfde 100644 --- a/gtsam_unstable/discrete/BinaryAllDiff.h +++ b/gtsam_unstable/discrete/BinaryAllDiff.h @@ -47,7 +47,7 @@ class BinaryAllDiff : public Constraint { } /// Calculate value - double operator()(const Assignment& values) const override { + double evaluate(const Assignment& values) const override { return (double)(values.at(keys_[0]) != values.at(keys_[1])); } diff --git a/gtsam_unstable/discrete/Domain.cpp b/gtsam_unstable/discrete/Domain.cpp index 752228c18..74f621dc7 100644 --- a/gtsam_unstable/discrete/Domain.cpp +++ b/gtsam_unstable/discrete/Domain.cpp @@ -30,7 +30,7 @@ string Domain::base1Str() const { } /* ************************************************************************* */ -double Domain::operator()(const Assignment& values) const { +double Domain::evaluate(const Assignment& values) const { return contains(values.at(key())); } diff --git a/gtsam_unstable/discrete/Domain.h b/gtsam_unstable/discrete/Domain.h index 13249d733..23a566d24 100644 --- a/gtsam_unstable/discrete/Domain.h +++ b/gtsam_unstable/discrete/Domain.h @@ -82,7 +82,7 @@ class GTSAM_UNSTABLE_EXPORT Domain : public Constraint { bool contains(size_t value) const { return values_.count(value) > 0; } /// Calculate value - double operator()(const Assignment& values) const override; + double evaluate(const Assignment& values) const override; /// Convert into a decisiontree DecisionTreeFactor toDecisionTreeFactor() const override; diff --git a/gtsam_unstable/discrete/SingleValue.cpp b/gtsam_unstable/discrete/SingleValue.cpp index 9762aec0f..220bc9c06 100644 --- a/gtsam_unstable/discrete/SingleValue.cpp +++ b/gtsam_unstable/discrete/SingleValue.cpp @@ -22,7 +22,7 @@ void SingleValue::print(const string& s, const KeyFormatter& formatter) const { } /* ************************************************************************* */ -double SingleValue::operator()(const Assignment& values) const { +double SingleValue::evaluate(const Assignment& values) const { return (double)(values.at(keys_[0]) == value_); } diff --git a/gtsam_unstable/discrete/SingleValue.h b/gtsam_unstable/discrete/SingleValue.h index 93fe38aaa..3df1209b8 100644 --- a/gtsam_unstable/discrete/SingleValue.h +++ b/gtsam_unstable/discrete/SingleValue.h @@ -55,7 +55,7 @@ class GTSAM_UNSTABLE_EXPORT SingleValue : public Constraint { } /// Calculate value - double operator()(const Assignment& values) const override; + double evaluate(const Assignment& values) const override; /// Convert into a decisiontree DecisionTreeFactor toDecisionTreeFactor() const override; From 8145086e5b57315bc94491def0bcdc67aa48148d Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 9 Dec 2024 21:09:37 -0500 Subject: [PATCH 051/112] formatting --- gtsam/discrete/discrete.i | 4 ++-- python/gtsam/tests/test_DecisionTreeFactor.py | 3 ++- python/gtsam/tests/test_DiscreteBayesTree.py | 4 ++-- python/gtsam/tests/test_DiscreteConditional.py | 3 ++- python/gtsam/tests/test_DiscreteFactorGraph.py | 5 ++++- 5 files changed, 12 insertions(+), 7 deletions(-) diff --git a/gtsam/discrete/discrete.i b/gtsam/discrete/discrete.i index 55c8f9e22..b2e2524f8 100644 --- a/gtsam/discrete/discrete.i +++ b/gtsam/discrete/discrete.i @@ -61,14 +61,14 @@ virtual class DecisionTreeFactor : gtsam::DiscreteFactor { DecisionTreeFactor(const std::vector& keys, string table); DecisionTreeFactor(const gtsam::DiscreteConditional& c); - + void print(string s = "DecisionTreeFactor\n", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::DecisionTreeFactor& other, double tol = 1e-9) const; size_t cardinality(gtsam::Key j) const; - + double operator()(const gtsam::DiscreteValues& values) const; gtsam::DecisionTreeFactor operator*(const gtsam::DecisionTreeFactor& f) const; size_t cardinality(gtsam::Key j) const; diff --git a/python/gtsam/tests/test_DecisionTreeFactor.py b/python/gtsam/tests/test_DecisionTreeFactor.py index 12308bb3c..a78d9c94a 100644 --- a/python/gtsam/tests/test_DecisionTreeFactor.py +++ b/python/gtsam/tests/test_DecisionTreeFactor.py @@ -13,9 +13,10 @@ Author: Frank Dellaert import unittest +from gtsam.utils.test_case import GtsamTestCase + from gtsam import (DecisionTreeFactor, DiscreteDistribution, DiscreteValues, Ordering) -from gtsam.utils.test_case import GtsamTestCase class TestDecisionTreeFactor(GtsamTestCase): diff --git a/python/gtsam/tests/test_DiscreteBayesTree.py b/python/gtsam/tests/test_DiscreteBayesTree.py index 2a9b6ea09..e08491fab 100644 --- a/python/gtsam/tests/test_DiscreteBayesTree.py +++ b/python/gtsam/tests/test_DiscreteBayesTree.py @@ -19,8 +19,8 @@ from gtsam.utils.test_case import GtsamTestCase import gtsam from gtsam import (DiscreteBayesNet, DiscreteBayesTreeClique, - DiscreteConditional, DiscreteFactorGraph, - DiscreteValues, Ordering) + DiscreteConditional, DiscreteFactorGraph, DiscreteValues, + Ordering) class TestDiscreteBayesNet(GtsamTestCase): diff --git a/python/gtsam/tests/test_DiscreteConditional.py b/python/gtsam/tests/test_DiscreteConditional.py index 241a5f0be..6c9eb9aec 100644 --- a/python/gtsam/tests/test_DiscreteConditional.py +++ b/python/gtsam/tests/test_DiscreteConditional.py @@ -13,9 +13,10 @@ Author: Varun Agrawal import unittest -from gtsam import DecisionTreeFactor, DiscreteConditional, DiscreteKeys from gtsam.utils.test_case import GtsamTestCase +from gtsam import DecisionTreeFactor, DiscreteConditional, DiscreteKeys + # Some DiscreteKeys for binary variables: A = 0, 2 B = 1, 2 diff --git a/python/gtsam/tests/test_DiscreteFactorGraph.py b/python/gtsam/tests/test_DiscreteFactorGraph.py index d725ceac8..3053087b4 100644 --- a/python/gtsam/tests/test_DiscreteFactorGraph.py +++ b/python/gtsam/tests/test_DiscreteFactorGraph.py @@ -14,9 +14,12 @@ Author: Frank Dellaert import unittest import numpy as np -from gtsam import DecisionTreeFactor, DiscreteConditional, DiscreteFactorGraph, DiscreteKeys, DiscreteValues, Ordering, Symbol from gtsam.utils.test_case import GtsamTestCase +from gtsam import (DecisionTreeFactor, DiscreteConditional, + DiscreteFactorGraph, DiscreteKeys, DiscreteValues, Ordering, + Symbol) + OrderingType = Ordering.OrderingType From 80b1fe569a39b77986d38ccd127c99b0fdc5f395 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 10 Dec 2024 10:41:36 -0500 Subject: [PATCH 052/112] use product method since it has a nullptr check --- gtsam/discrete/DiscreteFactorGraph.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp index 4ededbb8b..523b99201 100644 --- a/gtsam/discrete/DiscreteFactorGraph.cpp +++ b/gtsam/discrete/DiscreteFactorGraph.cpp @@ -66,8 +66,9 @@ namespace gtsam { /* ************************************************************************* */ DecisionTreeFactor DiscreteFactorGraph::product() const { DecisionTreeFactor result; - for(const sharedFactor& factor: *this) + for (const sharedFactor& factor : *this) { if (factor) result = (*factor) * result; + } return result; } @@ -75,8 +76,9 @@ namespace gtsam { double DiscreteFactorGraph::operator()( const DiscreteValues &values) const { double product = 1.0; - for( const sharedFactor& factor: factors_ ) + for (const sharedFactor& factor : factors_) { product *= (*factor)(values); + } return product; } @@ -117,8 +119,7 @@ namespace gtsam { const Ordering& frontalKeys) { // PRODUCT: multiply all factors gttic(product); - DecisionTreeFactor product; - for (auto&& factor : factors) product = (*factor) * product; + DecisionTreeFactor product = factors.product(); gttoc(product); // Max over all the potentials by pretending all keys are frontal: @@ -206,8 +207,7 @@ namespace gtsam { const Ordering& frontalKeys) { // PRODUCT: multiply all factors gttic(product); - DecisionTreeFactor product; - for (auto&& factor : factors) product = (*factor) * product; + DecisionTreeFactor product = factors.product(); gttoc(product); // Max over all the potentials by pretending all keys are frontal: From 92e0a55e7817d7507ebc098df85e1e8d7cbfca5f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 10 Dec 2024 10:41:57 -0500 Subject: [PATCH 053/112] generalize discreteKeys method --- gtsam/discrete/DiscreteFactorGraph.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp index 523b99201..96fdfc338 100644 --- a/gtsam/discrete/DiscreteFactorGraph.cpp +++ b/gtsam/discrete/DiscreteFactorGraph.cpp @@ -54,7 +54,7 @@ namespace gtsam { DiscreteKeys DiscreteFactorGraph::discreteKeys() const { DiscreteKeys result; for (auto&& factor : *this) { - if (auto p = std::dynamic_pointer_cast(factor)) { + if (auto p = std::dynamic_pointer_cast(factor)) { DiscreteKeys factor_keys = p->discreteKeys(); result.insert(result.end(), factor_keys.begin(), factor_keys.end()); } From dea9c7f7655576d7629903e38fc357e81898ec82 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 10 Dec 2024 10:49:08 -0500 Subject: [PATCH 054/112] common function for product and normalization --- gtsam/discrete/DiscreteFactorGraph.cpp | 32 ++++++++++++++------------ 1 file changed, 17 insertions(+), 15 deletions(-) diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp index 96fdfc338..436d784f7 100644 --- a/gtsam/discrete/DiscreteFactorGraph.cpp +++ b/gtsam/discrete/DiscreteFactorGraph.cpp @@ -112,11 +112,14 @@ namespace gtsam { // } // } - /* ************************************************************************ */ - // Alternate eliminate function for MPE - std::pair // - EliminateForMPE(const DiscreteFactorGraph& factors, - const Ordering& frontalKeys) { + /** + * @brief Multiply all the `factors` and normalize the + * product to prevent underflow. + * + * @param factors The factors to multiply as a DiscreteFactorGraph. + * @return DecisionTreeFactor + */ + static DecisionTreeFactor ProductAndNormalize(const DiscreteFactorGraph& factors) { // PRODUCT: multiply all factors gttic(product); DecisionTreeFactor product = factors.product(); @@ -127,6 +130,14 @@ namespace gtsam { // Normalize the product factor to prevent underflow. product = product / (*normalization); + } + + /* ************************************************************************ */ + // Alternate eliminate function for MPE + std::pair // + EliminateForMPE(const DiscreteFactorGraph& factors, + const Ordering& frontalKeys) { + DecisionTreeFactor product = ProductAndNormalize(factors); // max out frontals, this is the factor on the separator gttic(max); @@ -205,16 +216,7 @@ namespace gtsam { std::pair // EliminateDiscrete(const DiscreteFactorGraph& factors, const Ordering& frontalKeys) { - // PRODUCT: multiply all factors - gttic(product); - DecisionTreeFactor product = factors.product(); - gttoc(product); - - // Max over all the potentials by pretending all keys are frontal: - auto normalization = product.max(product.size()); - - // Normalize the product factor to prevent underflow. - product = product / (*normalization); + DecisionTreeFactor product = ProductAndNormalize(factors); // sum out frontals, this is the factor on the separator gttic(sum); From 590293bb92f53291a0a77837dfc51314e34c641a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 10 Dec 2024 10:52:15 -0500 Subject: [PATCH 055/112] return tau factor as DiscreteFactor for discrete elimination --- gtsam/discrete/DiscreteFactorGraph.cpp | 5 +++-- gtsam/discrete/DiscreteFactorGraph.h | 5 +++-- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp index 436d784f7..508a5f0f8 100644 --- a/gtsam/discrete/DiscreteFactorGraph.cpp +++ b/gtsam/discrete/DiscreteFactorGraph.cpp @@ -14,6 +14,7 @@ * @date Feb 14, 2011 * @author Duy-Nguyen Ta * @author Frank Dellaert + * @author Varun Agrawal */ #include @@ -134,7 +135,7 @@ namespace gtsam { /* ************************************************************************ */ // Alternate eliminate function for MPE - std::pair // + std::pair // EliminateForMPE(const DiscreteFactorGraph& factors, const Ordering& frontalKeys) { DecisionTreeFactor product = ProductAndNormalize(factors); @@ -213,7 +214,7 @@ namespace gtsam { } /* ************************************************************************ */ - std::pair // + std::pair // EliminateDiscrete(const DiscreteFactorGraph& factors, const Ordering& frontalKeys) { DecisionTreeFactor product = ProductAndNormalize(factors); diff --git a/gtsam/discrete/DiscreteFactorGraph.h b/gtsam/discrete/DiscreteFactorGraph.h index d0dc282b4..c57d2258c 100644 --- a/gtsam/discrete/DiscreteFactorGraph.h +++ b/gtsam/discrete/DiscreteFactorGraph.h @@ -14,6 +14,7 @@ * @date Feb 14, 2011 * @author Duy-Nguyen Ta * @author Frank Dellaert + * @author Varun Agrawal */ #pragma once @@ -48,7 +49,7 @@ class DiscreteJunctionTree; * @ingroup discrete */ GTSAM_EXPORT -std::pair +std::pair EliminateDiscrete(const DiscreteFactorGraph& factors, const Ordering& frontalKeys); @@ -61,7 +62,7 @@ EliminateDiscrete(const DiscreteFactorGraph& factors, * @ingroup discrete */ GTSAM_EXPORT -std::pair +std::pair EliminateForMPE(const DiscreteFactorGraph& factors, const Ordering& frontalKeys); From 162f61061cbbe02298250c759ff84387dd065039 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 10 Dec 2024 10:52:36 -0500 Subject: [PATCH 056/112] use BaseFactor methods to reduce code in DiscreteConditional --- gtsam/discrete/DiscreteConditional.h | 10 +++------- gtsam/discrete/DiscreteDistribution.h | 2 +- 2 files changed, 4 insertions(+), 8 deletions(-) diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h index 858623301..8cba6dbe7 100644 --- a/gtsam/discrete/DiscreteConditional.h +++ b/gtsam/discrete/DiscreteConditional.h @@ -168,13 +168,9 @@ class GTSAM_EXPORT DiscreteConditional static_cast(this)->print(s, formatter); } - /// Evaluate, just look up in AlgebraicDecisionTree - virtual double evaluate(const Assignment& values) const override { - return ADT::operator()(values); - } - - using DecisionTreeFactor::error; ///< DiscreteValues version - using DiscreteFactor::operator(); ///< DiscreteValues version + using BaseFactor::error; ///< DiscreteValues version + using BaseFactor::evaluate; ///< DiscreteValues version + using BaseFactor::operator(); ///< DiscreteValues version /** * @brief restrict to given *parent* values. diff --git a/gtsam/discrete/DiscreteDistribution.h b/gtsam/discrete/DiscreteDistribution.h index 4b690da15..09ea50332 100644 --- a/gtsam/discrete/DiscreteDistribution.h +++ b/gtsam/discrete/DiscreteDistribution.h @@ -40,7 +40,7 @@ class GTSAM_EXPORT DiscreteDistribution : public DiscreteConditional { /// Default constructor needed for serialization. DiscreteDistribution() {} - /// Constructor from factor. + /// Constructor from DecisionTreeFactor. explicit DiscreteDistribution(const DecisionTreeFactor& f) : Base(f.size(), f) {} From 372b133366626d03d5319843fad08e77909156db Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 10 Dec 2024 11:05:50 -0500 Subject: [PATCH 057/112] formatting --- gtsam/discrete/DiscreteFactorGraph.cpp | 27 +++++++++++++------------- 1 file changed, 13 insertions(+), 14 deletions(-) diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp index 508a5f0f8..ebba93382 100644 --- a/gtsam/discrete/DiscreteFactorGraph.cpp +++ b/gtsam/discrete/DiscreteFactorGraph.cpp @@ -36,13 +36,12 @@ namespace gtsam { template class FactorGraph; template class EliminateableFactorGraph; - /* ************************************************************************* */ - bool DiscreteFactorGraph::equals(const This& fg, double tol) const - { + /* ************************************************************************ */ + bool DiscreteFactorGraph::equals(const This& fg, double tol) const { return Base::equals(fg, tol); } - /* ************************************************************************* */ + /* ************************************************************************ */ KeySet DiscreteFactorGraph::keys() const { KeySet keys; for (const sharedFactor& factor : *this) { @@ -51,7 +50,7 @@ namespace gtsam { return keys; } - /* ************************************************************************* */ + /* ************************************************************************ */ DiscreteKeys DiscreteFactorGraph::discreteKeys() const { DiscreteKeys result; for (auto&& factor : *this) { @@ -64,7 +63,7 @@ namespace gtsam { return result; } - /* ************************************************************************* */ + /* ************************************************************************ */ DecisionTreeFactor DiscreteFactorGraph::product() const { DecisionTreeFactor result; for (const sharedFactor& factor : *this) { @@ -73,9 +72,8 @@ namespace gtsam { return result; } - /* ************************************************************************* */ - double DiscreteFactorGraph::operator()( - const DiscreteValues &values) const { + /* ************************************************************************ */ + double DiscreteFactorGraph::operator()(const DiscreteValues& values) const { double product = 1.0; for (const sharedFactor& factor : factors_) { product *= (*factor)(values); @@ -83,9 +81,9 @@ namespace gtsam { return product; } - /* ************************************************************************* */ + /* ************************************************************************ */ void DiscreteFactorGraph::print(const string& s, - const KeyFormatter& formatter) const { + const KeyFormatter& formatter) const { std::cout << s << std::endl; std::cout << "size: " << size() << std::endl; for (size_t i = 0; i < factors_.size(); i++) { @@ -120,7 +118,8 @@ namespace gtsam { * @param factors The factors to multiply as a DiscreteFactorGraph. * @return DecisionTreeFactor */ - static DecisionTreeFactor ProductAndNormalize(const DiscreteFactorGraph& factors) { + static DecisionTreeFactor ProductAndNormalize( + const DiscreteFactorGraph& factors) { // PRODUCT: multiply all factors gttic(product); DecisionTreeFactor product = factors.product(); @@ -155,8 +154,8 @@ namespace gtsam { // Make lookup with product gttic(lookup); size_t nrFrontals = frontalKeys.size(); - auto lookup = std::make_shared(nrFrontals, - orderedKeys, product); + auto lookup = + std::make_shared(nrFrontals, orderedKeys, product); gttoc(lookup); return {std::dynamic_pointer_cast(lookup), max}; From 62008fc99543c8c87e6a38bea11ff1592b1aac16 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 10 Dec 2024 11:10:25 -0500 Subject: [PATCH 058/112] add return --- gtsam/discrete/DiscreteFactorGraph.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp index ebba93382..ec6dac2fc 100644 --- a/gtsam/discrete/DiscreteFactorGraph.cpp +++ b/gtsam/discrete/DiscreteFactorGraph.cpp @@ -130,6 +130,8 @@ namespace gtsam { // Normalize the product factor to prevent underflow. product = product / (*normalization); + + return product; } /* ************************************************************************ */ From 2fd60a47a231b6cbebc6b916d213e64d379b39fc Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 10 Dec 2024 11:54:05 -0500 Subject: [PATCH 059/112] fix test --- gtsam/discrete/tests/testDiscreteFactorGraph.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/gtsam/discrete/tests/testDiscreteFactorGraph.cpp b/gtsam/discrete/tests/testDiscreteFactorGraph.cpp index 341eb63e3..0d71c12ba 100644 --- a/gtsam/discrete/tests/testDiscreteFactorGraph.cpp +++ b/gtsam/discrete/tests/testDiscreteFactorGraph.cpp @@ -113,7 +113,8 @@ TEST(DiscreteFactorGraph, test) { const Ordering frontalKeys{0}; const auto [conditional, newFactorPtr] = EliminateDiscrete(graph, frontalKeys); - DecisionTreeFactor newFactor = *newFactorPtr; + DecisionTreeFactor newFactor = + *std::dynamic_pointer_cast(newFactorPtr); // Normalize newFactor by max for comparison with expected auto normalization = newFactor.max(newFactor.size()); From 588533751b07d69a1940241ba6a144eb30ae4938 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 10 Dec 2024 14:10:33 -0500 Subject: [PATCH 060/112] add another pointer check --- gtsam/discrete/DiscreteFactorGraph.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp index ec6dac2fc..d0bf21047 100644 --- a/gtsam/discrete/DiscreteFactorGraph.cpp +++ b/gtsam/discrete/DiscreteFactorGraph.cpp @@ -76,7 +76,7 @@ namespace gtsam { double DiscreteFactorGraph::operator()(const DiscreteValues& values) const { double product = 1.0; for (const sharedFactor& factor : factors_) { - product *= (*factor)(values); + if (factor) product *= (*factor)(values); } return product; } From 8ce55a221001d931a41cccfd013b9adc95dad5d9 Mon Sep 17 00:00:00 2001 From: Jade Turner Date: Thu, 12 Dec 2024 00:16:03 +0800 Subject: [PATCH 061/112] document flags Signed-off-by: Jade Turner --- INSTALL.md | 20 +++++++++++++++----- 1 file changed, 15 insertions(+), 5 deletions(-) diff --git a/INSTALL.md b/INSTALL.md index 10bee196c..51f00fd3b 100644 --- a/INSTALL.md +++ b/INSTALL.md @@ -27,7 +27,7 @@ $ make install downloaded from https://www.threadingbuildingblocks.org/ - GTSAM may be configured to use MKL by toggling `GTSAM_WITH_EIGEN_MKL` and `GTSAM_WITH_EIGEN_MKL_OPENMP` to `ON`; however, best performance is usually - achieved with MKL disabled. We therefore advise you to benchmark your problem + achieved with MKL disabled. We therefore advise you to benchmark your problem before using MKL. Tested compilers: @@ -128,12 +128,12 @@ We support several build configurations for GTSAM (case insensitive) #### CMAKE_INSTALL_PREFIX -The install folder. The default is typically `/usr/local/`. +The install folder. The default is typically `/usr/local/`. To configure to install to your home directory, you could execute: ```cmake -DCMAKE_INSTALL_PREFIX:PATH=$HOME ..``` -#### GTSAM_TOOLBOX_INSTALL_PATH +#### GTSAM_TOOLBOX_INSTALL_PATH The Matlab toolbox will be installed in a subdirectory of this folder, called 'gtsam'. @@ -142,7 +142,7 @@ of this folder, called 'gtsam'. #### GTSAM_BUILD_CONVENIENCE_LIBRARIES -This is a build option to allow for tests in subfolders to be linked against convenience libraries rather than the full libgtsam. +This is a build option to allow for tests in subfolders to be linked against convenience libraries rather than the full libgtsam. Set with the command line as follows: ```cmake -DGTSAM_BUILD_CONVENIENCE_LIBRARIES:OPTION=ON ..``` @@ -159,6 +159,16 @@ Set with the command line as follows: ON: When enabled, libgtsam_unstable will be built and installed with the same options as libgtsam. In addition, if tests are enabled, the unit tests will be built as well. The Matlab toolbox will also be generated if the matlab toolbox is enabled, installing into a folder called `gtsam_unstable`. OFF (Default) If disabled, no `gtsam_unstable` code will be included in build or install. +## Convenience Options: + +#### GTSAM_BUILD_EXAMPLES_ALWAYS + +Whether or not to force building examples, can be true or false. + +#### GTSAM_BUILD_TESTS + +Whether or not to build tests, can be true or false. + ## Check `make check` will build and run all of the tests. Note that the tests will only be @@ -179,7 +189,7 @@ Here are some tips to get the best possible performance out of GTSAM. 1. Build in `Release` mode. GTSAM will run up to 10x faster compared to `Debug` mode. 2. Enable TBB. On modern processors with multiple cores, this can easily speed up - optimization by 30-50%. Please note that this may not be true for very small + optimization by 30-50%. Please note that this may not be true for very small problems where the overhead of dispatching work to multiple threads outweighs the benefit. We recommend that you benchmark your problem with/without TBB. 3. Use `GTSAM_BUILD_WITH_MARCH_NATIVE`. A performance gain of From af9db89ea9ce9e18dbcbfbb3791501ddaf8e6724 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 11 Dec 2024 13:10:31 -0500 Subject: [PATCH 062/112] Fixed math, use functor not macro --- gtsam/linear/KalmanFilter.cpp | 74 ++++++++++++++++++----------------- gtsam/linear/KalmanFilter.h | 2 +- 2 files changed, 40 insertions(+), 36 deletions(-) diff --git a/gtsam/linear/KalmanFilter.cpp b/gtsam/linear/KalmanFilter.cpp index 12a43093c..7c6e78527 100644 --- a/gtsam/linear/KalmanFilter.cpp +++ b/gtsam/linear/KalmanFilter.cpp @@ -25,22 +25,27 @@ #include #include -using namespace std; +#include "gtsam/base/Matrix.h" + +// In the code below we often get a covariance matrix Q, and we need to create a +// JacobianFactor with cost 0.5 * |Ax - b|^T Q^{-1} |Ax - b|. Factorizing Q as +// Q = L L^T +// and hence +// Q^{-1} = L^{-T} L^{-1} = M^T M, with M = L^{-1} +// We then have +// 0.5 * |Ax - b|^T Q^{-1} |Ax - b| +// = 0.5 * |Ax - b|^T M^T M |Ax - b| +// = 0.5 * |MAx - Mb|^T |MAx - Mb| +// The functor below efficiently multiplies with M by calling L.solve(). +namespace { +using Matrix = gtsam::Matrix; +struct InverseL : public Eigen::LLT { + InverseL(const Matrix& Q) : Eigen::LLT(Q) {} + Matrix operator*(const Matrix& A) const { return matrixL().solve(A); } +}; +} // namespace namespace gtsam { - -// In the code below we often need to multiply matrices with R, the Cholesky -// factor of the information matrix Q^{-1}, when given a covariance matrix Q. -// We can do this efficiently using Eigen, as we have: -// Q = L L^T -// => Q^{-1} = L^{-T}L^{-1} = R^T R -// => L^{-1} = R -// Rather than explicitly calculate R and do R*A, we should use L.solve(A) -// The following macro makes L available in the scope where it is used: -#define FACTORIZE_Q_INTO_L(Q) \ - Eigen::LLT llt(Q); \ - auto L = llt.matrixL(); - /* ************************************************************************* */ // Auxiliary function to solve factor graph and return pointer to root // conditional @@ -85,19 +90,19 @@ KalmanFilter::State KalmanFilter::init(const Vector& x0, // Create a factor graph f(x0), eliminate it into P(x0) GaussianFactorGraph factorGraph; - // Perform Cholesky decomposition of P0 - FACTORIZE_Q_INTO_L(P0) + // Perform Cholesky decomposition of P0 = LL^T + InverseL M(P0); - // Premultiply I and x0 with R - const Matrix R = L.solve(I_); // = R - const Vector b = L.solve(x0); // = R*x0 - factorGraph.emplace_shared(0, R, b); + // Premultiply I and x0 with M=L^{-1} + const Matrix A = M * I_; // = M + const Vector b = M * x0; // = M*x0 + factorGraph.emplace_shared(0, A, b); return solve(factorGraph); } /* ************************************************************************* */ -void KalmanFilter::print(const string& s) const { - cout << "KalmanFilter " << s << ", dim = " << n_ << endl; +void KalmanFilter::print(const std::string& s) const { + std::cout << "KalmanFilter " << s << ", dim = " << n_ << std::endl; } /* ************************************************************************* */ @@ -126,13 +131,13 @@ KalmanFilter::State KalmanFilter::predictQ(const State& p, const Matrix& F, assert(Q.cols() == n); #endif - // Perform Cholesky decomposition of Q - FACTORIZE_Q_INTO_L(Q) + // Perform Cholesky decomposition of Q = LL^T + InverseL M(Q); - // Premultiply A1 = -F and A2 = I, b = B * u with R - const Matrix A1 = -L.solve(F); // = -R*F - const Matrix A2 = L.solve(I_); // = R - const Vector b = L.solve(B * u); // = R * (B * u) + // Premultiply -F, I, and B * u with M=L^{-1} + const Matrix A1 = -(M * F); + const Matrix A2 = M * I_; + const Vector b = M * (B * u); return predict2(p, A1, A2, b); } @@ -163,14 +168,13 @@ KalmanFilter::State KalmanFilter::updateQ(const State& p, const Matrix& H, const Matrix& Q) const { Key k = step(p); - // Perform Cholesky decomposition of Q - FACTORIZE_Q_INTO_L(Q) + // Perform Cholesky decomposition of Q = LL^T + InverseL M(Q); - // Pre-multiply H and z with R, respectively - const Matrix RtH = L.solve(H); // = R * H - const Vector b = L.solve(z); // = R * z - - return fuse(p, std::make_shared(k, RtH, b)); + // Pre-multiply H and z with M=L^{-1}, respectively + const Matrix A = M * H; + const Vector b = M * z; + return fuse(p, std::make_shared(k, A, b)); } /* ************************************************************************* */ diff --git a/gtsam/linear/KalmanFilter.h b/gtsam/linear/KalmanFilter.h index 7fde9975d..2bf0549fa 100644 --- a/gtsam/linear/KalmanFilter.h +++ b/gtsam/linear/KalmanFilter.h @@ -214,4 +214,4 @@ class GTSAM_EXPORT KalmanFilter { size_t dim() const { return n_; } }; -} // namespace gtsam \ No newline at end of file +} // namespace gtsam From 2d0672059cc26ec3dd9ea58ba462f2b087eca25c Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 10 Dec 2024 13:41:19 -0500 Subject: [PATCH 063/112] Update some header includes --- gtsam/basis/Chebyshev2.cpp | 8 ++++++++ gtsam/basis/Chebyshev2.h | 10 +--------- gtsam/geometry/Pose2.h | 11 +++++++---- gtsam/geometry/Rot3.h | 7 +++++-- gtsam/geometry/triangulation.h | 7 +++++-- gtsam/inference/MetisIndex.h | 7 +++++-- gtsam/linear/HessianFactor.h | 18 ++++++++++++------ gtsam/nonlinear/ExpressionFactor.h | 4 ++-- gtsam/slam/EssentialMatrixFactor.h | 5 +++-- gtsam/slam/SmartFactorBase.h | 7 +++++-- 10 files changed, 53 insertions(+), 31 deletions(-) diff --git a/gtsam/basis/Chebyshev2.cpp b/gtsam/basis/Chebyshev2.cpp index 63fca64cc..71c3db7f0 100644 --- a/gtsam/basis/Chebyshev2.cpp +++ b/gtsam/basis/Chebyshev2.cpp @@ -20,6 +20,14 @@ namespace gtsam { +double Chebyshev2::Point(size_t N, int j, double a, double b) { + assert(j >= 0 && size_t(j) < N); + const double dtheta = M_PI / (N > 1 ? (N - 1) : 1); + // We add -PI so that we get values ordered from -1 to +1 + // sin(-M_PI_2 + dtheta*j); also works + return a + (b - a) * (1. + cos(-M_PI + dtheta * j)) / 2; +} + Weights Chebyshev2::CalculateWeights(size_t N, double x, double a, double b) { // Allocate space for weights Weights weights(N); diff --git a/gtsam/basis/Chebyshev2.h b/gtsam/basis/Chebyshev2.h index 1374faeef..207f4b617 100644 --- a/gtsam/basis/Chebyshev2.h +++ b/gtsam/basis/Chebyshev2.h @@ -36,8 +36,6 @@ #include #include -#include - namespace gtsam { /** @@ -63,13 +61,7 @@ class GTSAM_EXPORT Chebyshev2 : public Basis { * @param b Upper bound of interval (default: 1) * @return double */ - static double Point(size_t N, int j, double a = -1, double b = 1) { - assert(j >= 0 && size_t(j) < N); - const double dtheta = M_PI / (N > 1 ? (N - 1) : 1); - // We add -PI so that we get values ordered from -1 to +1 - // sin(-M_PI_2 + dtheta*j); also works - return a + (b - a) * (1. + cos(-M_PI + dtheta * j)) / 2; - } + static double Point(size_t N, int j, double a = -1, double b = 1); /// All Chebyshev points static Vector Points(size_t N) { diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index d9cad37f3..ff9a6399e 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -28,7 +28,6 @@ #include #include -#include namespace gtsam { @@ -82,9 +81,13 @@ public: Pose2(const Rot2& r, const Point2& t) : r_(r), t_(t) {} /** Constructor from 3*3 matrix */ - Pose2(const Matrix &T) : - r_(Rot2::atan2(T(1, 0), T(0, 0))), t_(T(0, 2), T(1, 2)) { - assert(T.rows() == 3 && T.cols() == 3); + Pose2(const Matrix &T) + : r_(Rot2::atan2(T(1, 0), T(0, 0))), t_(T(0, 2), T(1, 2)) { +#ifndef NDEBUG + if (T.rows() != 3 || T.cols() != 3) { + throw; + } +#endif } /// @} diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 398e8d473..d1e330438 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -30,7 +30,6 @@ #include // Get GTSAM_USE_QUATERNIONS macro #include -#include // You can override the default coordinate mode using this flag #ifndef ROT3_DEFAULT_COORDINATES_MODE @@ -160,7 +159,11 @@ class GTSAM_EXPORT Rot3 : public LieGroup { /// Rotations around Z, Y, then X axes as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis. inline static Rot3 RzRyRx(const Vector& xyz, OptionalJacobian<3, 3> H = {}) { - assert(xyz.size() == 3); +#ifndef NDEBUG + if (xyz.size() != 3) { + throw; + } +#endif Rot3 out; if (H) { Vector3 Hx, Hy, Hz; diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 0fa9d074e..9087f01c5 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -35,7 +35,6 @@ #include #include -#include namespace gtsam { @@ -318,7 +317,11 @@ typename CAMERA::MeasurementVector undistortMeasurements( const CameraSet& cameras, const typename CAMERA::MeasurementVector& measurements) { const size_t nrMeasurements = measurements.size(); - assert(nrMeasurements == cameras.size()); +#ifndef NDEBUG + if (nrMeasurements != cameras.size()) { + throw; + } +#endif typename CAMERA::MeasurementVector undistortedMeasurements(nrMeasurements); for (size_t ii = 0; ii < nrMeasurements; ++ii) { // Calibrate with cal and uncalibrate with pinhole version of cal so that diff --git a/gtsam/inference/MetisIndex.h b/gtsam/inference/MetisIndex.h index cc058dfbd..e051b9ae9 100644 --- a/gtsam/inference/MetisIndex.h +++ b/gtsam/inference/MetisIndex.h @@ -25,7 +25,6 @@ #include #include #include -#include namespace gtsam { /** @@ -93,7 +92,11 @@ public: return nKeys_; } Key intToKey(int32_t value) const { - assert(value >= 0); +#ifndef NDEBUG + if (value < 0) { + throw; + } +#endif return intKeyBMap_.right.find(value)->second; } diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index 53b6d0e8b..56b3d6537 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -23,8 +23,6 @@ #include #include -#include - namespace gtsam { // Forward declarations @@ -242,14 +240,18 @@ namespace gtsam { * use, for example, begin() + 2 to get the 3rd variable in this factor. * @return The linear term \f$ g \f$ */ SymmetricBlockMatrix::constBlock linearTerm(const_iterator j) const { - assert(!empty()); +#ifndef NDEBUG + if(empty()) throw; +#endif return info_.aboveDiagonalBlock(j - begin(), size()); } /** Return the complete linear term \f$ g \f$ as described above. * @return The linear term \f$ g \f$ */ SymmetricBlockMatrix::constBlock linearTerm() const { - assert(!empty()); +#ifndef NDEBUG + if(empty()) throw; +#endif // get the last column (except the bottom right block) return info_.aboveDiagonalRange(0, size(), size(), size() + 1); } @@ -257,7 +259,9 @@ namespace gtsam { /** Return the complete linear term \f$ g \f$ as described above. * @return The linear term \f$ g \f$ */ SymmetricBlockMatrix::Block linearTerm() { - assert(!empty()); +#ifndef NDEBUG + if(empty()) throw; +#endif return info_.aboveDiagonalRange(0, size(), size(), size() + 1); } @@ -326,7 +330,9 @@ namespace gtsam { * @param other the HessianFactor to be updated */ void updateHessian(HessianFactor* other) const { - assert(other); +#ifndef NDEBUG + if(!other) throw; +#endif updateHessian(other->keys_, &other->info_); } diff --git a/gtsam/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h index b3e34d079..63b2469a9 100644 --- a/gtsam/nonlinear/ExpressionFactor.h +++ b/gtsam/nonlinear/ExpressionFactor.h @@ -43,7 +43,7 @@ namespace gtsam { * */ template -class ExpressionFactor : public NoiseModelFactor { +class GTSAM_EXPORT ExpressionFactor : public NoiseModelFactor { GTSAM_CONCEPT_ASSERT(IsTestable); protected: @@ -246,7 +246,7 @@ struct traits > : public Testable > {}; * */ template -class ExpressionFactorN : public ExpressionFactor { +class GTSAM_EXPORT ExpressionFactorN : public ExpressionFactor { public: static const std::size_t NARY_EXPRESSION_SIZE = sizeof...(Args); using ArrayNKeys = std::array; diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index 3c2f105d4..601f8e94e 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -25,7 +25,6 @@ #include #include -#include namespace gtsam { @@ -71,7 +70,9 @@ class EssentialMatrixFactor : public NoiseModelFactorN { const SharedNoiseModel& model, std::shared_ptr K) : Base(model, key) { - assert(K); +#ifndef NDEBUG + if (K->empty()) throw; +#endif vA_ = EssentialMatrix::Homogeneous(K->calibrate(pA)); vB_ = EssentialMatrix::Homogeneous(K->calibrate(pB)); } diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index b8e789aaa..d3e879c33 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -34,7 +34,6 @@ #include #endif #include -#include namespace gtsam { @@ -136,7 +135,11 @@ protected: /// Add a bunch of measurements, together with the camera keys. void add(const ZVector& measurements, const KeyVector& cameraKeys) { - assert(measurements.size() == cameraKeys.size()); +#ifndef NDEBUG + if (measurements.size() != cameraKeys.size()) { + throw std::runtime_error("Number of measurements and camera keys do not match"); + } +#endif for (size_t i = 0; i < measurements.size(); i++) { this->add(measurements[i], cameraKeys[i]); } From a1ba3d77c8dcc896223bcbe4f08b7c3f207e3c3c Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 10 Dec 2024 17:25:12 -0500 Subject: [PATCH 064/112] Fix essential matrix factor --- gtsam/slam/EssentialMatrixFactor.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index 601f8e94e..5d38cb234 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -71,7 +71,7 @@ class EssentialMatrixFactor : public NoiseModelFactorN { std::shared_ptr K) : Base(model, key) { #ifndef NDEBUG - if (K->empty()) throw; + if (!K) throw; #endif vA_ = EssentialMatrix::Homogeneous(K->calibrate(pA)); vB_ = EssentialMatrix::Homogeneous(K->calibrate(pB)); From b76c6d8250270f7359884f0db98ce937f02ae5a3 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Wed, 11 Dec 2024 12:59:13 -0500 Subject: [PATCH 065/112] Fix ExpressionFactor wrong GTSAM_EXPORT --- gtsam/nonlinear/ExpressionFactor.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h index 63b2469a9..b3e34d079 100644 --- a/gtsam/nonlinear/ExpressionFactor.h +++ b/gtsam/nonlinear/ExpressionFactor.h @@ -43,7 +43,7 @@ namespace gtsam { * */ template -class GTSAM_EXPORT ExpressionFactor : public NoiseModelFactor { +class ExpressionFactor : public NoiseModelFactor { GTSAM_CONCEPT_ASSERT(IsTestable); protected: @@ -246,7 +246,7 @@ struct traits > : public Testable > {}; * */ template -class GTSAM_EXPORT ExpressionFactorN : public ExpressionFactor { +class ExpressionFactorN : public ExpressionFactor { public: static const std::size_t NARY_EXPRESSION_SIZE = sizeof...(Args); using ArrayNKeys = std::array; From ed098eaec64d185fbf91923b89e9442383b18979 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Wed, 4 Dec 2024 11:43:51 -0500 Subject: [PATCH 066/112] Cleanup some includes and fix cassert --- examples/CombinedImuFactorsExample.cpp | 1 + examples/ImuFactorsExample.cpp | 1 + examples/elaboratePoint2KalmanFilter.cpp | 2 ++ gtsam/base/Matrix.cpp | 1 + gtsam/base/Vector.cpp | 1 + gtsam/base/VerticalBlockMatrix.h | 2 ++ gtsam/base/cholesky.cpp | 1 + gtsam/base/tests/testNumericalDerivative.cpp | 2 ++ gtsam/basis/Chebyshev2.h | 2 ++ gtsam/discrete/DiscreteConditional.cpp | 1 + gtsam/discrete/DiscreteLookupDAG.cpp | 1 + gtsam/geometry/Cal3f.cpp | 1 + gtsam/geometry/CameraSet.h | 1 + gtsam/geometry/Pose2.cpp | 1 + gtsam/geometry/Pose2.h | 1 + gtsam/geometry/Rot3.cpp | 1 + gtsam/geometry/Rot3.h | 1 + gtsam/geometry/SOn.h | 1 + gtsam/geometry/triangulation.h | 3 ++- gtsam/hybrid/HybridJunctionTree.cpp | 1 + gtsam/inference/BayesTree-inst.h | 2 +- gtsam/inference/ClusterTree-inst.h | 1 + gtsam/inference/EliminationTree-inst.h | 1 + gtsam/inference/JunctionTree-inst.h | 2 ++ gtsam/inference/MetisIndex.h | 1 + gtsam/inference/Ordering.cpp | 1 + gtsam/linear/HessianFactor.cpp | 3 ++- gtsam/linear/HessianFactor.h | 1 + gtsam/linear/JacobianFactor.cpp | 1 + gtsam/linear/KalmanFilter.cpp | 3 +++ gtsam/linear/NoiseModel.cpp | 1 + gtsam/linear/Sampler.cpp | 3 +++ gtsam/linear/SubgraphPreconditioner.cpp | 1 + gtsam/navigation/ImuFactor.cpp | 1 + gtsam/navigation/PreintegrationBase.cpp | 4 +++- gtsam/nonlinear/DoglegOptimizerImpl.cpp | 1 + gtsam/nonlinear/DoglegOptimizerImpl.h | 1 + gtsam/nonlinear/Expression-inl.h | 1 + gtsam/nonlinear/ExtendedKalmanFilter-inl.h | 2 ++ gtsam/nonlinear/ISAM2-impl.cpp | 1 + gtsam/nonlinear/ISAM2-impl.h | 1 + gtsam/nonlinear/ISAM2.cpp | 1 + gtsam/nonlinear/ISAM2Clique.cpp | 1 + gtsam/nonlinear/NonlinearFactor.cpp | 2 ++ gtsam/nonlinear/Values.cpp | 1 + gtsam/nonlinear/internal/ExpressionNode.h | 1 + gtsam/sfm/ShonanAveraging.cpp | 1 + gtsam/slam/EssentialMatrixFactor.h | 1 + gtsam/slam/SmartFactorBase.h | 1 + gtsam_unstable/discrete/Scheduler.cpp | 1 + gtsam_unstable/dynamics/DynamicsPriors.h | 2 ++ gtsam_unstable/dynamics/FullIMUFactor.h | 2 ++ gtsam_unstable/dynamics/IMUFactor.h | 2 ++ gtsam_unstable/dynamics/PoseRTV.cpp | 2 ++ gtsam_unstable/dynamics/VelocityConstraint.h | 2 ++ gtsam_unstable/geometry/BearingS2.cpp | 2 ++ gtsam_unstable/geometry/Pose3Upright.cpp | 5 +++-- gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp | 2 ++ gtsam_unstable/nonlinear/LinearizedFactor.cpp | 1 + gtsam_unstable/partition/FindSeparator-inl.h | 1 + gtsam_unstable/slam/PartialPriorFactor.h | 2 ++ gtsam_unstable/slam/SmartStereoProjectionFactorPP.cpp | 2 ++ gtsam_unstable/slam/SmartStereoProjectionPoseFactor.cpp | 2 ++ tests/testGaussianISAM2.cpp | 1 + 64 files changed, 91 insertions(+), 6 deletions(-) diff --git a/examples/CombinedImuFactorsExample.cpp b/examples/CombinedImuFactorsExample.cpp index 6ce4f76fa..436b6e283 100644 --- a/examples/CombinedImuFactorsExample.cpp +++ b/examples/CombinedImuFactorsExample.cpp @@ -48,6 +48,7 @@ #include #include #include +#include using namespace gtsam; using namespace std; diff --git a/examples/ImuFactorsExample.cpp b/examples/ImuFactorsExample.cpp index 342f1f220..7cbc8989f 100644 --- a/examples/ImuFactorsExample.cpp +++ b/examples/ImuFactorsExample.cpp @@ -49,6 +49,7 @@ #include #include +#include #include #include diff --git a/examples/elaboratePoint2KalmanFilter.cpp b/examples/elaboratePoint2KalmanFilter.cpp index a2ad8cf0b..bc4e6e340 100644 --- a/examples/elaboratePoint2KalmanFilter.cpp +++ b/examples/elaboratePoint2KalmanFilter.cpp @@ -29,6 +29,8 @@ #include #include +#include + using namespace std; using namespace gtsam; diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index 247c53cce..1e04bc58a 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -24,6 +24,7 @@ #include #include +#include #include #include #include diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index ce7aeba7b..dbd48dfc8 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -24,6 +24,7 @@ #include #include #include +#include #include #include #include diff --git a/gtsam/base/VerticalBlockMatrix.h b/gtsam/base/VerticalBlockMatrix.h index 777441300..c8fef76e9 100644 --- a/gtsam/base/VerticalBlockMatrix.h +++ b/gtsam/base/VerticalBlockMatrix.h @@ -21,6 +21,8 @@ #include #include +#include + namespace gtsam { // Forward declarations diff --git a/gtsam/base/cholesky.cpp b/gtsam/base/cholesky.cpp index 71cef2524..9fee627fe 100644 --- a/gtsam/base/cholesky.cpp +++ b/gtsam/base/cholesky.cpp @@ -21,6 +21,7 @@ #include #include +#include using namespace std; diff --git a/gtsam/base/tests/testNumericalDerivative.cpp b/gtsam/base/tests/testNumericalDerivative.cpp index e8838a476..b14f699a3 100644 --- a/gtsam/base/tests/testNumericalDerivative.cpp +++ b/gtsam/base/tests/testNumericalDerivative.cpp @@ -18,6 +18,8 @@ #include #include +#include + using namespace std; using namespace gtsam; diff --git a/gtsam/basis/Chebyshev2.h b/gtsam/basis/Chebyshev2.h index 849a51104..1374faeef 100644 --- a/gtsam/basis/Chebyshev2.h +++ b/gtsam/basis/Chebyshev2.h @@ -36,6 +36,8 @@ #include #include +#include + namespace gtsam { /** diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp index c44905d79..eeb5dca3f 100644 --- a/gtsam/discrete/DiscreteConditional.cpp +++ b/gtsam/discrete/DiscreteConditional.cpp @@ -30,6 +30,7 @@ #include #include #include +#include using namespace std; using std::pair; diff --git a/gtsam/discrete/DiscreteLookupDAG.cpp b/gtsam/discrete/DiscreteLookupDAG.cpp index 4d02e007b..d1108e7b7 100644 --- a/gtsam/discrete/DiscreteLookupDAG.cpp +++ b/gtsam/discrete/DiscreteLookupDAG.cpp @@ -23,6 +23,7 @@ #include #include #include +#include using std::pair; using std::vector; diff --git a/gtsam/geometry/Cal3f.cpp b/gtsam/geometry/Cal3f.cpp index 319155dc9..b8e365eed 100644 --- a/gtsam/geometry/Cal3f.cpp +++ b/gtsam/geometry/Cal3f.cpp @@ -28,6 +28,7 @@ #include #include +#include namespace gtsam { diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index 26d4952c8..e3c16f066 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -26,6 +26,7 @@ #include #include +#include namespace gtsam { diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 05678dc27..640481406 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -20,6 +20,7 @@ #include #include +#include #include #include diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index aad81493f..d9cad37f3 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -28,6 +28,7 @@ #include #include +#include namespace gtsam { diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index a936bd02a..10ba5894e 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -23,6 +23,7 @@ #include #include +#include #include using namespace std; diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 7e05ee4da..398e8d473 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -30,6 +30,7 @@ #include // Get GTSAM_USE_QUATERNIONS macro #include +#include // You can override the default coordinate mode using this flag #ifndef ROT3_DEFAULT_COORDINATES_MODE diff --git a/gtsam/geometry/SOn.h b/gtsam/geometry/SOn.h index 7e1762b3f..75ff872a8 100644 --- a/gtsam/geometry/SOn.h +++ b/gtsam/geometry/SOn.h @@ -33,6 +33,7 @@ #include #include #include +#include namespace gtsam { diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index d82da3459..0fa9d074e 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -20,7 +20,7 @@ #pragma once -#include "gtsam/geometry/Point3.h" +#include #include #include #include @@ -35,6 +35,7 @@ #include #include +#include namespace gtsam { diff --git a/gtsam/hybrid/HybridJunctionTree.cpp b/gtsam/hybrid/HybridJunctionTree.cpp index 22d3c7dd2..b9bdcbed2 100644 --- a/gtsam/hybrid/HybridJunctionTree.cpp +++ b/gtsam/hybrid/HybridJunctionTree.cpp @@ -22,6 +22,7 @@ #include #include +#include namespace gtsam { diff --git a/gtsam/inference/BayesTree-inst.h b/gtsam/inference/BayesTree-inst.h index 95d475a02..fdfa94d2b 100644 --- a/gtsam/inference/BayesTree-inst.h +++ b/gtsam/inference/BayesTree-inst.h @@ -27,7 +27,7 @@ #include #include - +#include namespace gtsam { /* ************************************************************************* */ diff --git a/gtsam/inference/ClusterTree-inst.h b/gtsam/inference/ClusterTree-inst.h index 318624608..2a5f0b455 100644 --- a/gtsam/inference/ClusterTree-inst.h +++ b/gtsam/inference/ClusterTree-inst.h @@ -19,6 +19,7 @@ #include #endif #include +#include namespace gtsam { diff --git a/gtsam/inference/EliminationTree-inst.h b/gtsam/inference/EliminationTree-inst.h index e9acc12e1..16449d0f4 100644 --- a/gtsam/inference/EliminationTree-inst.h +++ b/gtsam/inference/EliminationTree-inst.h @@ -19,6 +19,7 @@ #include #include +#include #include #include diff --git a/gtsam/inference/JunctionTree-inst.h b/gtsam/inference/JunctionTree-inst.h index 0f79c2a9a..6a8d6918a 100644 --- a/gtsam/inference/JunctionTree-inst.h +++ b/gtsam/inference/JunctionTree-inst.h @@ -25,6 +25,8 @@ #include #include +#include + namespace gtsam { template diff --git a/gtsam/inference/MetisIndex.h b/gtsam/inference/MetisIndex.h index abdf11c5f..cc058dfbd 100644 --- a/gtsam/inference/MetisIndex.h +++ b/gtsam/inference/MetisIndex.h @@ -25,6 +25,7 @@ #include #include #include +#include namespace gtsam { /** diff --git a/gtsam/inference/Ordering.cpp b/gtsam/inference/Ordering.cpp index cb2ca752d..95eb17a1c 100644 --- a/gtsam/inference/Ordering.cpp +++ b/gtsam/inference/Ordering.cpp @@ -18,6 +18,7 @@ #include #include +#include #include #include diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 3c3050f90..4e8ef804c 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -25,13 +25,14 @@ #include #include #include +#include #include #include #include #include +#include #include -#include "gtsam/base/Vector.h" using namespace std; diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index 8cbabcd5d..53b6d0e8b 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -23,6 +23,7 @@ #include #include +#include namespace gtsam { diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 3cfb5ce7b..51d513e33 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -32,6 +32,7 @@ #include #include +#include #include #include diff --git a/gtsam/linear/KalmanFilter.cpp b/gtsam/linear/KalmanFilter.cpp index ded6bc5e3..6291c90b6 100644 --- a/gtsam/linear/KalmanFilter.cpp +++ b/gtsam/linear/KalmanFilter.cpp @@ -26,6 +26,9 @@ #include #include +#ifndef NDEBUG +#include +#endif using namespace std; diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 884c87270..39666b46b 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -20,6 +20,7 @@ #include #include +#include #include #include #include diff --git a/gtsam/linear/Sampler.cpp b/gtsam/linear/Sampler.cpp index 20d4c955b..68f21d723 100644 --- a/gtsam/linear/Sampler.cpp +++ b/gtsam/linear/Sampler.cpp @@ -17,6 +17,9 @@ */ #include + +#include + namespace gtsam { /* ************************************************************************* */ diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp index 96f4847b5..53ea94d6e 100644 --- a/gtsam/linear/SubgraphPreconditioner.cpp +++ b/gtsam/linear/SubgraphPreconditioner.cpp @@ -25,6 +25,7 @@ #include #include +#include using std::cout; using std::endl; diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 88e45d318..5697d54d4 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -23,6 +23,7 @@ /* External or standard includes */ #include +#include namespace gtsam { diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index 7f998987b..dd86b829c 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -20,9 +20,11 @@ * @author Varun Agrawal **/ -#include "PreintegrationBase.h" +#include #include +#include + using namespace std; namespace gtsam { diff --git a/gtsam/nonlinear/DoglegOptimizerImpl.cpp b/gtsam/nonlinear/DoglegOptimizerImpl.cpp index 4f1c6fb54..e82a513ca 100644 --- a/gtsam/nonlinear/DoglegOptimizerImpl.cpp +++ b/gtsam/nonlinear/DoglegOptimizerImpl.cpp @@ -16,6 +16,7 @@ */ #include +#include #include using namespace std; diff --git a/gtsam/nonlinear/DoglegOptimizerImpl.h b/gtsam/nonlinear/DoglegOptimizerImpl.h index 8ce9b361e..1038dd522 100644 --- a/gtsam/nonlinear/DoglegOptimizerImpl.h +++ b/gtsam/nonlinear/DoglegOptimizerImpl.h @@ -17,6 +17,7 @@ #pragma once #include +#include #include #include diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h index 71ef1d840..b09bcb3a5 100644 --- a/gtsam/nonlinear/Expression-inl.h +++ b/gtsam/nonlinear/Expression-inl.h @@ -26,6 +26,7 @@ #include #include #include +#include namespace gtsam { diff --git a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h index 5bf643c12..072f40b58 100644 --- a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h +++ b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h @@ -23,6 +23,8 @@ #include #include +#include + namespace gtsam { /* ************************************************************************* */ diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index 20874e2dc..d0b27f3f3 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -25,6 +25,7 @@ #include #include #include +#include using namespace std; diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index e9a9696eb..bb0210237 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -33,6 +33,7 @@ #include #include #include +#include namespace gtsam { diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 47857a2a2..0eb2deef5 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -29,6 +29,7 @@ #include #include #include +#include using namespace std; diff --git a/gtsam/nonlinear/ISAM2Clique.cpp b/gtsam/nonlinear/ISAM2Clique.cpp index d4b050f84..64ab7c66e 100644 --- a/gtsam/nonlinear/ISAM2Clique.cpp +++ b/gtsam/nonlinear/ISAM2Clique.cpp @@ -22,6 +22,7 @@ #include #include +#include using namespace std; diff --git a/gtsam/nonlinear/NonlinearFactor.cpp b/gtsam/nonlinear/NonlinearFactor.cpp index 7312bf6d9..5eabfac91 100644 --- a/gtsam/nonlinear/NonlinearFactor.cpp +++ b/gtsam/nonlinear/NonlinearFactor.cpp @@ -19,6 +19,8 @@ #include #include +#include + namespace gtsam { /* ************************************************************************* */ diff --git a/gtsam/nonlinear/Values.cpp b/gtsam/nonlinear/Values.cpp index 6af9f2b6a..1caaa9db1 100644 --- a/gtsam/nonlinear/Values.cpp +++ b/gtsam/nonlinear/Values.cpp @@ -28,6 +28,7 @@ #include #include #include +#include using namespace std; diff --git a/gtsam/nonlinear/internal/ExpressionNode.h b/gtsam/nonlinear/internal/ExpressionNode.h index de0c9721e..25587f511 100644 --- a/gtsam/nonlinear/internal/ExpressionNode.h +++ b/gtsam/nonlinear/internal/ExpressionNode.h @@ -26,6 +26,7 @@ #include // operator typeid #include #include +#include class ExpressionFactorBinaryTest; // Forward declare for testing diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index 830f1c719..6c9f91e90 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -37,6 +37,7 @@ #include #include #include +#include namespace gtsam { diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index 5fdf138cc..3c2f105d4 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -25,6 +25,7 @@ #include #include +#include namespace gtsam { diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 5d6ec4445..b8e789aaa 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -34,6 +34,7 @@ #include #endif #include +#include namespace gtsam { diff --git a/gtsam_unstable/discrete/Scheduler.cpp b/gtsam_unstable/discrete/Scheduler.cpp index 0de4d32c4..53be25d55 100644 --- a/gtsam_unstable/discrete/Scheduler.cpp +++ b/gtsam_unstable/discrete/Scheduler.cpp @@ -14,6 +14,7 @@ #include #include #include +#include namespace gtsam { diff --git a/gtsam_unstable/dynamics/DynamicsPriors.h b/gtsam_unstable/dynamics/DynamicsPriors.h index d4ebcba19..2dc0d552b 100644 --- a/gtsam_unstable/dynamics/DynamicsPriors.h +++ b/gtsam_unstable/dynamics/DynamicsPriors.h @@ -12,6 +12,8 @@ #include #include +#include + namespace gtsam { // Indices of relevant variables in the PoseRTV tangent vector: diff --git a/gtsam_unstable/dynamics/FullIMUFactor.h b/gtsam_unstable/dynamics/FullIMUFactor.h index 02da899b7..eea019cd2 100644 --- a/gtsam_unstable/dynamics/FullIMUFactor.h +++ b/gtsam_unstable/dynamics/FullIMUFactor.h @@ -10,6 +10,8 @@ #include #include +#include + namespace gtsam { /** diff --git a/gtsam_unstable/dynamics/IMUFactor.h b/gtsam_unstable/dynamics/IMUFactor.h index ee09600e5..241b6b497 100644 --- a/gtsam_unstable/dynamics/IMUFactor.h +++ b/gtsam_unstable/dynamics/IMUFactor.h @@ -10,6 +10,8 @@ #include #include +#include + namespace gtsam { /** diff --git a/gtsam_unstable/dynamics/PoseRTV.cpp b/gtsam_unstable/dynamics/PoseRTV.cpp index cf21c315b..9457c99e8 100644 --- a/gtsam_unstable/dynamics/PoseRTV.cpp +++ b/gtsam_unstable/dynamics/PoseRTV.cpp @@ -7,6 +7,8 @@ #include #include +#include + namespace gtsam { using namespace std; diff --git a/gtsam_unstable/dynamics/VelocityConstraint.h b/gtsam_unstable/dynamics/VelocityConstraint.h index 2bb70e1b5..dbdd27259 100644 --- a/gtsam_unstable/dynamics/VelocityConstraint.h +++ b/gtsam_unstable/dynamics/VelocityConstraint.h @@ -10,6 +10,8 @@ #include #include +#include + namespace gtsam { namespace dynamics { diff --git a/gtsam_unstable/geometry/BearingS2.cpp b/gtsam_unstable/geometry/BearingS2.cpp index 9dfed612c..23afd158b 100644 --- a/gtsam_unstable/geometry/BearingS2.cpp +++ b/gtsam_unstable/geometry/BearingS2.cpp @@ -9,6 +9,8 @@ #include +#include + namespace gtsam { using namespace std; diff --git a/gtsam_unstable/geometry/Pose3Upright.cpp b/gtsam_unstable/geometry/Pose3Upright.cpp index 9cd9f78f5..c5ef0d70f 100644 --- a/gtsam_unstable/geometry/Pose3Upright.cpp +++ b/gtsam_unstable/geometry/Pose3Upright.cpp @@ -6,8 +6,9 @@ */ #include -#include "gtsam/base/OptionalJacobian.h" -#include "gtsam/base/Vector.h" +#include +#include +#include #include diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp index 36cb6165f..8dfb15aa2 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp @@ -21,6 +21,8 @@ #include #include +#include + namespace gtsam { /* ************************************************************************* */ diff --git a/gtsam_unstable/nonlinear/LinearizedFactor.cpp b/gtsam_unstable/nonlinear/LinearizedFactor.cpp index 2d404005d..375c49341 100644 --- a/gtsam_unstable/nonlinear/LinearizedFactor.cpp +++ b/gtsam_unstable/nonlinear/LinearizedFactor.cpp @@ -17,6 +17,7 @@ #include #include +#include namespace gtsam { diff --git a/gtsam_unstable/partition/FindSeparator-inl.h b/gtsam_unstable/partition/FindSeparator-inl.h index f4718f7a9..21ed91604 100644 --- a/gtsam_unstable/partition/FindSeparator-inl.h +++ b/gtsam_unstable/partition/FindSeparator-inl.h @@ -14,6 +14,7 @@ #include #include #include +#include #include #include diff --git a/gtsam_unstable/slam/PartialPriorFactor.h b/gtsam_unstable/slam/PartialPriorFactor.h index 7722e5d82..0a735a2e1 100644 --- a/gtsam_unstable/slam/PartialPriorFactor.h +++ b/gtsam_unstable/slam/PartialPriorFactor.h @@ -20,6 +20,8 @@ #include #include +#include + namespace gtsam { /** diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.cpp b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.cpp index c4a473154..6d78bdb7a 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.cpp +++ b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.cpp @@ -18,6 +18,8 @@ #include +#include + namespace gtsam { SmartStereoProjectionFactorPP::SmartStereoProjectionFactorPP( diff --git a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.cpp index 57913385a..52129daa8 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.cpp @@ -21,6 +21,8 @@ #include +#include + namespace gtsam { SmartStereoProjectionPoseFactor::SmartStereoProjectionPoseFactor( diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 721ed51c0..3b507d806 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -25,6 +25,7 @@ #include +#include using namespace std; using namespace gtsam; From 425c3ac42c10cef214839416f1427362a2f5e32c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 11 Dec 2024 15:00:53 -0500 Subject: [PATCH 067/112] check if input size matches the total cardinality of the keys --- gtsam/discrete/TableFactor.cpp | 20 ++++++++++++++++---- gtsam/discrete/TableFactor.h | 16 ++++++++++------ 2 files changed, 26 insertions(+), 10 deletions(-) diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp index ea51a996c..ea51fa550 100644 --- a/gtsam/discrete/TableFactor.cpp +++ b/gtsam/discrete/TableFactor.cpp @@ -98,7 +98,17 @@ TableFactor::TableFactor(const DiscreteConditional& c) /* ************************************************************************ */ Eigen::SparseVector TableFactor::Convert( - const std::vector& table) { + const DiscreteKeys& keys, const std::vector& table) { + size_t max_size = 1; + for (auto&& [_, cardinality] : keys.cardinalities()) { + max_size *= cardinality; + } + if (table.size() != max_size) { + throw std::runtime_error( + "The cardinalities of the keys don't match the number of values in the " + "input."); + } + Eigen::SparseVector sparse_table(table.size()); // Count number of nonzero elements in table and reserve the space. const uint64_t nnz = std::count_if(table.begin(), table.end(), @@ -113,13 +123,14 @@ Eigen::SparseVector TableFactor::Convert( } /* ************************************************************************ */ -Eigen::SparseVector TableFactor::Convert(const std::string& table) { +Eigen::SparseVector TableFactor::Convert(const DiscreteKeys& keys, + const std::string& table) { // Convert string to doubles. std::vector ys; std::istringstream iss(table); std::copy(std::istream_iterator(iss), std::istream_iterator(), std::back_inserter(ys)); - return Convert(ys); + return Convert(keys, ys); } /* ************************************************************************ */ @@ -250,7 +261,8 @@ void TableFactor::print(const string& s, const KeyFormatter& formatter) const { for (auto&& kv : assignment) { cout << "(" << formatter(kv.first) << ", " << kv.second << ")"; } - cout << " | " << it.value() << " | " << it.index() << endl; + cout << " | " << std::setw(10) << std::left << it.value() << " | " + << it.index() << endl; } cout << "number of nnzs: " << sparse_table_.nonZeros() << endl; } diff --git a/gtsam/discrete/TableFactor.h b/gtsam/discrete/TableFactor.h index 07564892f..ee6f456db 100644 --- a/gtsam/discrete/TableFactor.h +++ b/gtsam/discrete/TableFactor.h @@ -80,12 +80,16 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor { return DiscreteKey(keys_[i], cardinalities_.at(keys_[i])); } - /// Convert probability table given as doubles to SparseVector. - /// Example) {0, 1, 1, 0, 0, 1, 0} -> values: {1, 1, 1}, indices: {1, 2, 5} - static Eigen::SparseVector Convert(const std::vector& table); + /** + * Convert probability table given as doubles to SparseVector. + * Example: {0, 1, 1, 0, 0, 1, 0} -> values: {1, 1, 1}, indices: {1, 2, 5} + */ + static Eigen::SparseVector Convert(const DiscreteKeys& keys, + const std::vector& table); /// Convert probability table given as string to SparseVector. - static Eigen::SparseVector Convert(const std::string& table); + static Eigen::SparseVector Convert(const DiscreteKeys& keys, + const std::string& table); public: // typedefs needed to play nice with gtsam @@ -111,11 +115,11 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor { /** Constructor from doubles */ TableFactor(const DiscreteKeys& keys, const std::vector& table) - : TableFactor(keys, Convert(table)) {} + : TableFactor(keys, Convert(keys, table)) {} /** Constructor from string */ TableFactor(const DiscreteKeys& keys, const std::string& table) - : TableFactor(keys, Convert(table)) {} + : TableFactor(keys, Convert(keys, table)) {} /// Single-key specialization template From b5cd82d0b4d81ec33586f55a944be3428cdc3da3 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 11 Dec 2024 15:01:30 -0500 Subject: [PATCH 068/112] helper constructor that doesn't take separate keys --- gtsam/discrete/TableFactor.cpp | 5 +++++ gtsam/discrete/TableFactor.h | 1 + 2 files changed, 6 insertions(+) diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp index ea51fa550..486641e3f 100644 --- a/gtsam/discrete/TableFactor.cpp +++ b/gtsam/discrete/TableFactor.cpp @@ -92,6 +92,11 @@ TableFactor::TableFactor(const DiscreteKeys& dkeys, const DecisionTreeFactor& dtf) : TableFactor(dkeys, ComputeLeafOrdering(dkeys, dtf)) {} +/* ************************************************************************ */ +TableFactor::TableFactor(const DecisionTreeFactor& dtf) + : TableFactor(dtf.discreteKeys(), + ComputeLeafOrdering(dtf.discreteKeys(), dtf)) {} + /* ************************************************************************ */ TableFactor::TableFactor(const DiscreteConditional& c) : TableFactor(c.discreteKeys(), c) {} diff --git a/gtsam/discrete/TableFactor.h b/gtsam/discrete/TableFactor.h index ee6f456db..d27c4740c 100644 --- a/gtsam/discrete/TableFactor.h +++ b/gtsam/discrete/TableFactor.h @@ -132,6 +132,7 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor { /// Constructor from DecisionTreeFactor TableFactor(const DiscreteKeys& keys, const DecisionTreeFactor& dtf); + TableFactor(const DecisionTreeFactor& dtf); /// Constructor from DecisionTree/AlgebraicDecisionTree TableFactor(const DiscreteKeys& keys, const DecisionTree& dtree); From ab3f48bbe9b2da13ebc048350bc8273219d4ac88 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 11 Dec 2024 15:36:25 -0500 Subject: [PATCH 069/112] fix equality to check for matching keys --- gtsam/discrete/DecisionTreeFactor.cpp | 2 +- gtsam/discrete/DiscreteFactor.cpp | 5 +++++ gtsam/discrete/DiscreteFactor.h | 2 +- gtsam/discrete/TableFactor.cpp | 3 ++- gtsam/discrete/tests/testTableFactor.cpp | 13 ++++++++----- 5 files changed, 17 insertions(+), 8 deletions(-) diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp index f68d2ae00..1ac782b88 100644 --- a/gtsam/discrete/DecisionTreeFactor.cpp +++ b/gtsam/discrete/DecisionTreeFactor.cpp @@ -48,7 +48,7 @@ namespace gtsam { return false; } else { const auto& f(static_cast(other)); - return ADT::equals(f, tol); + return Base::equals(other, tol) && ADT::equals(f, tol); } } diff --git a/gtsam/discrete/DiscreteFactor.cpp b/gtsam/discrete/DiscreteFactor.cpp index 2b11046f4..68cc1df7d 100644 --- a/gtsam/discrete/DiscreteFactor.cpp +++ b/gtsam/discrete/DiscreteFactor.cpp @@ -28,6 +28,11 @@ using namespace std; namespace gtsam { +/* ************************************************************************* */ +bool DiscreteFactor::equals(const DiscreteFactor& lf, double tol) const { + return Base::equals(lf, tol) && cardinalities_ == lf.cardinalities_; +} + /* ************************************************************************ */ DiscreteKeys DiscreteFactor::discreteKeys() const { DiscreteKeys result; diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h index fa1179c39..29981e94b 100644 --- a/gtsam/discrete/DiscreteFactor.h +++ b/gtsam/discrete/DiscreteFactor.h @@ -77,7 +77,7 @@ class GTSAM_EXPORT DiscreteFactor : public Factor { /// @{ /// equals - virtual bool equals(const DiscreteFactor& lf, double tol = 1e-9) const = 0; + virtual bool equals(const DiscreteFactor& lf, double tol = 1e-9) const; /// print void print( diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp index 486641e3f..8e185eb3b 100644 --- a/gtsam/discrete/TableFactor.cpp +++ b/gtsam/discrete/TableFactor.cpp @@ -144,7 +144,8 @@ bool TableFactor::equals(const DiscreteFactor& other, double tol) const { return false; } else { const auto& f(static_cast(other)); - return sparse_table_.isApprox(f.sparse_table_, tol); + return Base::equals(other, tol) && + sparse_table_.isApprox(f.sparse_table_, tol); } } diff --git a/gtsam/discrete/tests/testTableFactor.cpp b/gtsam/discrete/tests/testTableFactor.cpp index 0f7d7a615..35dcaf093 100644 --- a/gtsam/discrete/tests/testTableFactor.cpp +++ b/gtsam/discrete/tests/testTableFactor.cpp @@ -134,14 +134,17 @@ TEST(TableFactor, constructors) { EXPECT(assert_equal(expected, f4)); // Test for 9=3x3 values. - DiscreteKey V(0, 3), W(1, 3); + DiscreteKey V(0, 3), W(1, 3), O(100, 3); DiscreteConditional conditional5(V | W = "1/2/3 5/6/7 9/10/11"); TableFactor f5(conditional5); - // GTSAM_PRINT(f5); - TableFactor expected_f5( - X & Y, - "0.166667 0.277778 0.3 0.333333 0.333333 0.333333 0.5 0.388889 0.366667"); + + std::string expected_values = + "0.166667 0.277778 0.3 0.333333 0.333333 0.333333 0.5 0.388889 0.366667"; + TableFactor expected_f5(V & W, expected_values); EXPECT(assert_equal(expected_f5, f5, 1e-6)); + + TableFactor f5_with_wrong_keys(V & O, expected_values); + EXPECT(!assert_equal(f5_with_wrong_keys, f5, 1e-9)); } /* ************************************************************************* */ From ed742d3cb76dd10d8b2c3e700f86dde165f37221 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 11 Dec 2024 16:26:31 -0500 Subject: [PATCH 070/112] fix assertion --- gtsam/discrete/tests/testTableFactor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/discrete/tests/testTableFactor.cpp b/gtsam/discrete/tests/testTableFactor.cpp index 35dcaf093..212067cb3 100644 --- a/gtsam/discrete/tests/testTableFactor.cpp +++ b/gtsam/discrete/tests/testTableFactor.cpp @@ -144,7 +144,7 @@ TEST(TableFactor, constructors) { EXPECT(assert_equal(expected_f5, f5, 1e-6)); TableFactor f5_with_wrong_keys(V & O, expected_values); - EXPECT(!assert_equal(f5_with_wrong_keys, f5, 1e-9)); + EXPECT(assert_inequal(f5_with_wrong_keys, f5, 1e-9)); } /* ************************************************************************* */ From d8e412555885925b32d6f3f3f189b53c5f3a1547 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sun, 1 Dec 2024 12:47:56 -0500 Subject: [PATCH 071/112] Change the deprecated C++ enum math to constexprs --- gtsam/base/ProductLieGroup.h | 4 ++-- gtsam/geometry/BearingRange.h | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/gtsam/base/ProductLieGroup.h b/gtsam/base/ProductLieGroup.h index beb376c19..085da712a 100644 --- a/gtsam/base/ProductLieGroup.h +++ b/gtsam/base/ProductLieGroup.h @@ -32,8 +32,8 @@ class ProductLieGroup: public std::pair { typedef std::pair Base; protected: - enum {dimension1 = traits::dimension}; - enum {dimension2 = traits::dimension}; + constexpr static const size_t dimension1 = traits::dimension; + constexpr static const size_t dimension2 = traits::dimension; public: /// Default constructor yields identity diff --git a/gtsam/geometry/BearingRange.h b/gtsam/geometry/BearingRange.h index 19194bb7a..9b8fe1f87 100644 --- a/gtsam/geometry/BearingRange.h +++ b/gtsam/geometry/BearingRange.h @@ -55,9 +55,9 @@ private: R range_; public: - enum { dimB = traits::dimension }; - enum { dimR = traits::dimension }; - enum { dimension = dimB + dimR }; + constexpr static const size_t dimB = traits::dimension; + constexpr static const size_t dimR = traits::dimension; + constexpr static const size_t dimension = dimB + dimR; /// @name Standard Constructors /// @{ From 456df093f110af74b6ee6e85ba6e44ced009a680 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Mon, 2 Dec 2024 14:11:51 -0500 Subject: [PATCH 072/112] Change all occurances of unnamed enum (deprecated in C++20) to `constexpr` --- GTSAM-Concepts.md | 2 +- gtsam/base/GenericValue.h | 3 +-- gtsam/base/Lie.h | 4 ++-- gtsam/base/Manifold.h | 4 ++-- gtsam/base/Matrix.h | 2 +- gtsam/base/ProductLieGroup.h | 6 +++--- gtsam/base/VectorSpace.h | 10 +++++----- gtsam/basis/Basis.h | 2 +- gtsam/geometry/BearingRange.h | 4 +--- gtsam/geometry/Cal3.h | 2 +- gtsam/geometry/Cal3Bundler.h | 2 +- gtsam/geometry/Cal3DS2.h | 2 +- gtsam/geometry/Cal3DS2_Base.h | 2 +- gtsam/geometry/Cal3Fisheye.h | 2 +- gtsam/geometry/Cal3Unified.h | 2 +- gtsam/geometry/Cal3_S2.h | 2 +- gtsam/geometry/Cal3_S2Stereo.h | 2 +- gtsam/geometry/Cal3f.h | 2 +- gtsam/geometry/CalibratedCamera.h | 4 +--- gtsam/geometry/EssentialMatrix.h | 2 +- gtsam/geometry/FundamentalMatrix.h | 4 ++-- gtsam/geometry/Line3.h | 2 +- gtsam/geometry/OrientedPlane3.h | 4 +--- gtsam/geometry/PinholeCamera.h | 4 +--- gtsam/geometry/PinholePose.h | 4 +--- gtsam/geometry/Quaternion.h | 4 +--- gtsam/geometry/SOn.h | 2 +- gtsam/geometry/SphericalCamera.h | 4 ++-- gtsam/geometry/StereoCamera.h | 4 +--- gtsam/geometry/StereoPoint2.h | 2 +- gtsam/geometry/Unit3.h | 4 +--- gtsam/navigation/NavState.h | 4 +--- gtsam/nonlinear/ExpressionFactor.h | 6 +++--- gtsam/nonlinear/NonlinearFactor.h | 2 +- gtsam/nonlinear/PriorFactor.h | 2 +- gtsam/nonlinear/tests/testExpression.cpp | 2 +- gtsam/nonlinear/tests/testValues.cpp | 2 +- gtsam/slam/BetweenFactor.h | 8 ++++---- gtsam/slam/FrobeniusFactor.h | 6 +++--- gtsam/slam/KarcherMeanFactor.h | 2 +- gtsam_unstable/geometry/Event.h | 2 +- 41 files changed, 58 insertions(+), 77 deletions(-) diff --git a/GTSAM-Concepts.md b/GTSAM-Concepts.md index 9911b3764..733672fbe 100644 --- a/GTSAM-Concepts.md +++ b/GTSAM-Concepts.md @@ -22,7 +22,7 @@ In GTSAM, all properties and operations needed to use a type must be defined thr In detail, we ask that the following items are defined in the traits object (although, not all are needed for optimization): * values: - * `enum { dimension = D};`, an enum that indicates the dimensionality *n* of the manifold. In Eigen-fashion, we also support manifolds whose dimensionality is only defined at runtime, by specifying the value -1. + * `inline constexpr static auto dimension = D;`, a constexpr that indicates the dimensionality *n* of the manifold. In Eigen-fashion, we also support manifolds whose dimensionality is only defined at runtime, by specifying the value -1. * types: * `TangentVector`, type that lives in tangent space. This will almost always be an `Eigen::Matrix`. * `ChartJacobian`, a typedef for `OptionalJacobian`. diff --git a/gtsam/base/GenericValue.h b/gtsam/base/GenericValue.h index bb92b6b2e..87ce7a73d 100644 --- a/gtsam/base/GenericValue.h +++ b/gtsam/base/GenericValue.h @@ -184,9 +184,8 @@ public: } #endif - // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html - enum { NeedsToAlign = (sizeof(T) % 16) == 0 }; + constexpr static const bool NeedsToAlign = (sizeof(T) % 16) == 0; public: GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) }; diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index ce021e10e..862ae6f4d 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -36,7 +36,7 @@ namespace gtsam { template struct LieGroup { - enum { dimension = N }; + inline constexpr static auto dimension = N; typedef OptionalJacobian ChartJacobian; typedef Eigen::Matrix Jacobian; typedef Eigen::Matrix TangentVector; @@ -183,7 +183,7 @@ struct LieGroupTraits: GetDimensionImpl { /// @name Manifold /// @{ typedef Class ManifoldType; - enum { dimension = Class::dimension }; + inline constexpr static auto dimension = Class::dimension; typedef Eigen::Matrix TangentVector; typedef OptionalJacobian ChartJacobian; diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index cb30fa9c0..922b8dd2f 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -55,7 +55,7 @@ namespace internal { template struct HasManifoldPrereqs { - enum { dim = Class::dimension }; + inline constexpr static auto dim = Class::dimension; Class p, q; Eigen::Matrix v; @@ -95,7 +95,7 @@ struct ManifoldTraits: GetDimensionImpl { GTSAM_CONCEPT_ASSERT(HasManifoldPrereqs); // Dimension of the manifold - enum { dimension = Class::dimension }; + inline constexpr static auto dimension = Class::dimension; // Typedefs required by all manifold types. typedef Class ManifoldType; diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 6dba9cb05..c8dc46ed5 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -479,7 +479,7 @@ struct MultiplyWithInverse { */ template struct MultiplyWithInverseFunction { - enum { M = traits::dimension }; + inline constexpr static auto M = traits::dimension; typedef Eigen::Matrix VectorN; typedef Eigen::Matrix MatrixN; diff --git a/gtsam/base/ProductLieGroup.h b/gtsam/base/ProductLieGroup.h index 085da712a..bb7ba72b8 100644 --- a/gtsam/base/ProductLieGroup.h +++ b/gtsam/base/ProductLieGroup.h @@ -67,9 +67,9 @@ public: /// @name Manifold /// @{ - enum {dimension = dimension1 + dimension2}; - inline static size_t Dim() {return dimension;} - inline size_t dim() const {return dimension;} + inline constexpr static auto dimension = dimension1 + dimension2; + inline static size_t Dim() { return dimension; } + inline size_t dim() const { return dimension; } typedef Eigen::Matrix TangentVector; typedef OptionalJacobian ChartJacobian; diff --git a/gtsam/base/VectorSpace.h b/gtsam/base/VectorSpace.h index a9e9ca106..c5c4ad622 100644 --- a/gtsam/base/VectorSpace.h +++ b/gtsam/base/VectorSpace.h @@ -163,7 +163,7 @@ struct VectorSpaceImpl { template struct HasVectorSpacePrereqs { - enum { dim = Class::dimension }; + inline constexpr static auto dim = Class::dimension; Class p, q; Vector v; @@ -197,7 +197,7 @@ GTSAM_CONCEPT_ASSERT(HasVectorSpacePrereqs); /// @name Manifold /// @{ - enum { dimension = Class::dimension}; + inline constexpr static auto dimension = Class::dimension; typedef Class ManifoldType; /// @} }; @@ -232,7 +232,7 @@ struct ScalarTraits : VectorSpaceImpl { /// @name Manifold /// @{ typedef Scalar ManifoldType; - enum { dimension = 1 }; + inline constexpr static auto dimension = 1; typedef Eigen::Matrix TangentVector; typedef OptionalJacobian<1, 1> ChartJacobian; @@ -305,7 +305,7 @@ struct traits > : /// @name Manifold /// @{ - enum { dimension = M*N}; + inline constexpr static auto dimension = M * N; typedef Fixed ManifoldType; typedef Eigen::Matrix TangentVector; typedef Eigen::Matrix Jacobian; @@ -377,7 +377,7 @@ struct DynamicTraits { /// @name Manifold /// @{ - enum { dimension = Eigen::Dynamic }; + inline constexpr static auto dimension = Eigen::Dynamic; typedef Eigen::VectorXd TangentVector; typedef Eigen::MatrixXd Jacobian; typedef OptionalJacobian ChartJacobian; diff --git a/gtsam/basis/Basis.h b/gtsam/basis/Basis.h index 1b1c44acc..960440daf 100644 --- a/gtsam/basis/Basis.h +++ b/gtsam/basis/Basis.h @@ -291,7 +291,7 @@ class Basis { */ template class ManifoldEvaluationFunctor : public VectorEvaluationFunctor { - enum { M = traits::dimension }; + inline constexpr static auto M = traits::dimension; using Base = VectorEvaluationFunctor; public: diff --git a/gtsam/geometry/BearingRange.h b/gtsam/geometry/BearingRange.h index 9b8fe1f87..07229dfca 100644 --- a/gtsam/geometry/BearingRange.h +++ b/gtsam/geometry/BearingRange.h @@ -162,9 +162,7 @@ private: /// @} // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html - enum { - NeedsToAlign = (sizeof(B) % 16) == 0 || (sizeof(R) % 16) == 0 - }; + inline constexpr static auto NeedsToAlign = (sizeof(B) % 16) == 0 || (sizeof(R) % 16) == 0; public: GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) }; diff --git a/gtsam/geometry/Cal3.h b/gtsam/geometry/Cal3.h index 1b66eb336..5eecee845 100644 --- a/gtsam/geometry/Cal3.h +++ b/gtsam/geometry/Cal3.h @@ -73,7 +73,7 @@ class GTSAM_EXPORT Cal3 { double u0_ = 0.0f, v0_ = 0.0f; ///< principal point public: - enum { dimension = 5 }; + inline constexpr static auto dimension = 5; ///< shared pointer to calibration object using shared_ptr = std::shared_ptr; diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index 081688d48..01eb9437a 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -37,7 +37,7 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3f { // Note: u0 and v0 are constants and not optimized. public: - enum { dimension = 3 }; + inline constexpr static auto dimension = 3; using shared_ptr = std::shared_ptr; /// @name Constructors diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h index a7ca08afc..0ecfc24ae 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -36,7 +36,7 @@ class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base { using Base = Cal3DS2_Base; public: - enum { dimension = 9 }; + inline constexpr static auto dimension = 9; ///< shared pointer to stereo calibration object using shared_ptr = std::shared_ptr; diff --git a/gtsam/geometry/Cal3DS2_Base.h b/gtsam/geometry/Cal3DS2_Base.h index b0ef0368b..6eff04c10 100644 --- a/gtsam/geometry/Cal3DS2_Base.h +++ b/gtsam/geometry/Cal3DS2_Base.h @@ -46,7 +46,7 @@ class GTSAM_EXPORT Cal3DS2_Base : public Cal3 { double tol_ = 1e-5; ///< tolerance value when calibrating public: - enum { dimension = 9 }; + inline constexpr static auto dimension = 9; ///< shared pointer to stereo calibration object using shared_ptr = std::shared_ptr; diff --git a/gtsam/geometry/Cal3Fisheye.h b/gtsam/geometry/Cal3Fisheye.h index 3f1cac148..4c542c5be 100644 --- a/gtsam/geometry/Cal3Fisheye.h +++ b/gtsam/geometry/Cal3Fisheye.h @@ -55,7 +55,7 @@ class GTSAM_EXPORT Cal3Fisheye : public Cal3 { double tol_ = 1e-5; ///< tolerance value when calibrating public: - enum { dimension = 9 }; + inline constexpr static auto dimension = 9; ///< shared pointer to fisheye calibration object using shared_ptr = std::shared_ptr; diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h index 309b6dca1..7eb4d2edb 100644 --- a/gtsam/geometry/Cal3Unified.h +++ b/gtsam/geometry/Cal3Unified.h @@ -50,7 +50,7 @@ class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base { double xi_ = 0.0f; ///< mirror parameter public: - enum { dimension = 10 }; + inline constexpr static auto dimension = 10; ///< shared pointer to stereo calibration object using shared_ptr = std::shared_ptr; diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index 885be614f..568cde41e 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -33,7 +33,7 @@ namespace gtsam { */ class GTSAM_EXPORT Cal3_S2 : public Cal3 { public: - enum { dimension = 5 }; + inline constexpr static auto dimension = 5; ///< shared pointer to calibration object using shared_ptr = std::shared_ptr; diff --git a/gtsam/geometry/Cal3_S2Stereo.h b/gtsam/geometry/Cal3_S2Stereo.h index 4fee1a022..292519cfd 100644 --- a/gtsam/geometry/Cal3_S2Stereo.h +++ b/gtsam/geometry/Cal3_S2Stereo.h @@ -32,7 +32,7 @@ class GTSAM_EXPORT Cal3_S2Stereo : public Cal3_S2 { double b_ = 1.0f; ///< Stereo baseline. public: - enum { dimension = 6 }; + inline constexpr static auto dimension = 6; ///< shared pointer to stereo calibration object using shared_ptr = std::shared_ptr; diff --git a/gtsam/geometry/Cal3f.h b/gtsam/geometry/Cal3f.h index f053f3d11..6c3c214b8 100644 --- a/gtsam/geometry/Cal3f.h +++ b/gtsam/geometry/Cal3f.h @@ -34,7 +34,7 @@ namespace gtsam { */ class GTSAM_EXPORT Cal3f : public Cal3 { public: - enum { dimension = 1 }; + inline constexpr static auto dimension = 1; using shared_ptr = std::shared_ptr; /// @name Constructors diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index dca15feb2..d3d1e1b9a 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -252,9 +252,7 @@ class GTSAM_EXPORT CalibratedCamera: public PinholeBase { public: - enum { - dimension = 6 - }; + inline constexpr static auto dimension = 6; /// @name Standard Constructors /// @{ diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index d514c4f19..3986f3063 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -82,7 +82,7 @@ class EssentialMatrix { /// @name Manifold /// @{ - enum { dimension = 5 }; + inline constexpr static auto dimension = 5; inline static size_t Dim() { return dimension;} inline size_t dim() const { return dimension;} diff --git a/gtsam/geometry/FundamentalMatrix.h b/gtsam/geometry/FundamentalMatrix.h index df2c2881a..0752ced7d 100644 --- a/gtsam/geometry/FundamentalMatrix.h +++ b/gtsam/geometry/FundamentalMatrix.h @@ -102,7 +102,7 @@ class GTSAM_EXPORT FundamentalMatrix { /// @name Manifold /// @{ - enum { dimension = 7 }; // 3 for U, 1 for s, 3 for V + inline constexpr static auto dimension = 7; // 3 for U, 1 for s, 3 for V inline static size_t Dim() { return dimension; } inline size_t dim() const { return dimension; } @@ -179,7 +179,7 @@ class GTSAM_EXPORT SimpleFundamentalMatrix { /// @name Manifold /// @{ - enum { dimension = 7 }; // 5 for E, 1 for fa, 1 for fb + inline constexpr static auto dimension = 7; // 5 for E, 1 for fa, 1 for fb inline static size_t Dim() { return dimension; } inline size_t dim() const { return dimension; } diff --git a/gtsam/geometry/Line3.h b/gtsam/geometry/Line3.h index def49d268..5ae50e138 100644 --- a/gtsam/geometry/Line3.h +++ b/gtsam/geometry/Line3.h @@ -47,7 +47,7 @@ class GTSAM_EXPORT Line3 { double a_, b_; // Intersection of line with the world x-y plane rotated by R_ // Also the closest point on line to origin public: - enum { dimension = 4 }; + inline constexpr static auto dimension = 4; /** Default constructor is the Z axis **/ Line3() : diff --git a/gtsam/geometry/OrientedPlane3.h b/gtsam/geometry/OrientedPlane3.h index 07c8445fe..3d84086ac 100644 --- a/gtsam/geometry/OrientedPlane3.h +++ b/gtsam/geometry/OrientedPlane3.h @@ -39,9 +39,7 @@ private: double d_; ///< The perpendicular distance to this plane public: - enum { - dimension = 3 - }; + inline constexpr static auto dimension = 3; /// @name Constructors /// @{ diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index 8329664fd..0439b2fde 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -51,9 +51,7 @@ private: public: - enum { - dimension = 6 + DimK - }; ///< Dimension depends on calibration + inline constexpr static auto dimension = 6 + DimK; ///< Dimension depends on calibration /// @name Standard Constructors /// @{ diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h index df6ec5c08..f1191dbcc 100644 --- a/gtsam/geometry/PinholePose.h +++ b/gtsam/geometry/PinholePose.h @@ -245,9 +245,7 @@ private: public: - enum { - dimension = 6 - }; ///< There are 6 DOF to optimize for + inline constexpr static auto dimension = 6; ///< There are 6 DOF to optimize for /// @name Standard Constructors /// @{ diff --git a/gtsam/geometry/Quaternion.h b/gtsam/geometry/Quaternion.h index f9ed2a383..36f891505 100644 --- a/gtsam/geometry/Quaternion.h +++ b/gtsam/geometry/Quaternion.h @@ -45,9 +45,7 @@ struct traits { /// @} /// @name Basic manifold traits /// @{ - enum { - dimension = 3 - }; + inline constexpr static auto dimension = 3; typedef OptionalJacobian<3, 3> ChartJacobian; typedef Eigen::Matrix<_Scalar, 3, 1, _Options, 3, 1> TangentVector; diff --git a/gtsam/geometry/SOn.h b/gtsam/geometry/SOn.h index 75ff872a8..b0da3b213 100644 --- a/gtsam/geometry/SOn.h +++ b/gtsam/geometry/SOn.h @@ -54,7 +54,7 @@ constexpr int NSquaredSO(int N) { return (N < 0) ? Eigen::Dynamic : N * N; } template class SO : public LieGroup, internal::DimensionSO(N)> { public: - enum { dimension = internal::DimensionSO(N) }; + inline constexpr static auto dimension = internal::DimensionSO(N); using MatrixNN = Eigen::Matrix; using VectorN2 = Eigen::Matrix; using MatrixDD = Eigen::Matrix; diff --git a/gtsam/geometry/SphericalCamera.h b/gtsam/geometry/SphericalCamera.h index ef20aa7fe..1ff87cec3 100644 --- a/gtsam/geometry/SphericalCamera.h +++ b/gtsam/geometry/SphericalCamera.h @@ -41,7 +41,7 @@ namespace gtsam { */ class GTSAM_EXPORT EmptyCal { public: - enum { dimension = 0 }; + inline constexpr static auto dimension = 0; EmptyCal() {} virtual ~EmptyCal() = default; using shared_ptr = std::shared_ptr; @@ -73,7 +73,7 @@ class GTSAM_EXPORT EmptyCal { */ class GTSAM_EXPORT SphericalCamera { public: - enum { dimension = 6 }; + inline constexpr static auto dimension = 6; using Measurement = Unit3; using MeasurementVector = std::vector; diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index da71dd070..026125572 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/gtsam/geometry/StereoCamera.h @@ -61,9 +61,7 @@ private: public: - enum { - dimension = 6 - }; + inline constexpr static auto dimension = 6; /// @name Standard Constructors /// @{ diff --git a/gtsam/geometry/StereoPoint2.h b/gtsam/geometry/StereoPoint2.h index 4383e212e..7a1910e42 100644 --- a/gtsam/geometry/StereoPoint2.h +++ b/gtsam/geometry/StereoPoint2.h @@ -37,7 +37,7 @@ private: double uL_, uR_, v_; public: - enum { dimension = 3 }; + inline constexpr static auto dimension = 3; /// @name Standard Constructors /// @{ diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index 18bc5d9f0..a7304280f 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -53,9 +53,7 @@ private: public: - enum { - dimension = 2 - }; + inline constexpr static auto dimension = 2; /// @name Constructors /// @{ diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h index f778a7123..35ce435a3 100644 --- a/gtsam/navigation/NavState.h +++ b/gtsam/navigation/NavState.h @@ -42,9 +42,7 @@ private: public: - enum { - dimension = 9 - }; + inline constexpr static auto dimension = 9; typedef std::pair PositionAndVelocity; diff --git a/gtsam/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h index b3e34d079..d9af1933c 100644 --- a/gtsam/nonlinear/ExpressionFactor.h +++ b/gtsam/nonlinear/ExpressionFactor.h @@ -224,9 +224,9 @@ private: #endif // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html - enum { NeedsToAlign = (sizeof(T) % 16) == 0 }; - public: - GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) + inline constexpr static auto NeedsToAlign = (sizeof(T) % 16) == 0; + public: + GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) }; // ExpressionFactor diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 2d3a13766..101743b78 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -434,7 +434,7 @@ class NoiseModelFactorN public detail::NoiseModelFactorAliases { public: /// N is the number of variables (N-way factor) - enum { N = sizeof...(ValueTypes) }; + inline constexpr static auto N = sizeof...(ValueTypes); using NoiseModelFactor::unwhitenedError; diff --git a/gtsam/nonlinear/PriorFactor.h b/gtsam/nonlinear/PriorFactor.h index ab2a66a77..159373459 100644 --- a/gtsam/nonlinear/PriorFactor.h +++ b/gtsam/nonlinear/PriorFactor.h @@ -118,7 +118,7 @@ namespace gtsam { #endif // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html - enum { NeedsToAlign = (sizeof(T) % 16) == 0 }; + inline constexpr static auto NeedsToAlign = (sizeof(T) % 16) == 0; public: GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) }; diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp index 6f5fc53f5..67815e262 100644 --- a/gtsam/nonlinear/tests/testExpression.cpp +++ b/gtsam/nonlinear/tests/testExpression.cpp @@ -115,7 +115,7 @@ TEST(Expression, Unary3) { // Simple test class that implements the `VectorSpace` protocol. class Class : public Point3 { public: - enum {dimension = 3}; + inline constexpr static auto dimension = 3; using Point3::Point3; const Vector3& vector() const { return *this; } inline static Class Identity() { return Class(0,0,0); } diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index a2b265a56..06eb46d4c 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -54,7 +54,7 @@ int TestValueData::DestructorCount = 0; class TestValue { TestValueData data_; public: - enum {dimension = 0}; + inline constexpr static auto dimension = 0; void print(const std::string& str = "") const {} bool equals(const TestValue& other, double tol = 1e-9) const { return true; } size_t dim() const { return 0; } diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index 6eaa5c01b..3e2478e73 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -147,10 +147,10 @@ namespace gtsam { } #endif - // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html - enum { NeedsToAlign = (sizeof(VALUE) % 16) == 0 }; - public: - GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) + // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html + inline constexpr static auto NeedsToAlign = (sizeof(VALUE) % 16) == 0; + public: + GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) }; // \class BetweenFactor /// traits diff --git a/gtsam/slam/FrobeniusFactor.h b/gtsam/slam/FrobeniusFactor.h index a000a1514..37efa5f24 100644 --- a/gtsam/slam/FrobeniusFactor.h +++ b/gtsam/slam/FrobeniusFactor.h @@ -49,7 +49,7 @@ ConvertNoiseModel(const SharedNoiseModel &model, size_t n, */ template class FrobeniusPrior : public NoiseModelFactorN { - enum { Dim = Rot::VectorN2::RowsAtCompileTime }; + inline constexpr static auto Dim = Rot::VectorN2::RowsAtCompileTime; using MatrixNN = typename Rot::MatrixNN; Eigen::Matrix vecM_; ///< vectorized matrix to approximate @@ -79,7 +79,7 @@ class FrobeniusPrior : public NoiseModelFactorN { */ template class FrobeniusFactor : public NoiseModelFactorN { - enum { Dim = Rot::VectorN2::RowsAtCompileTime }; + inline constexpr static auto Dim = Rot::VectorN2::RowsAtCompileTime; public: @@ -111,7 +111,7 @@ class FrobeniusBetweenFactor : public NoiseModelFactorN { Rot R12_; ///< measured rotation between R1 and R2 Eigen::Matrix R2hat_H_R1_; ///< fixed derivative of R2hat wrpt R1 - enum { Dim = Rot::VectorN2::RowsAtCompileTime }; + inline constexpr static auto Dim = Rot::VectorN2::RowsAtCompileTime; public: diff --git a/gtsam/slam/KarcherMeanFactor.h b/gtsam/slam/KarcherMeanFactor.h index 4e82ca1d9..6b25dd261 100644 --- a/gtsam/slam/KarcherMeanFactor.h +++ b/gtsam/slam/KarcherMeanFactor.h @@ -45,7 +45,7 @@ template T FindKarcherMean(std::initializer_list &&rotations); * */ template class KarcherMeanFactor : public NonlinearFactor { // Compile time dimension: can be -1 - enum { D = traits::dimension }; + inline constexpr static auto D = traits::dimension; // Runtime dimension: always >=0 size_t d_; diff --git a/gtsam_unstable/geometry/Event.h b/gtsam_unstable/geometry/Event.h index a3c907646..471ada35b 100644 --- a/gtsam_unstable/geometry/Event.h +++ b/gtsam_unstable/geometry/Event.h @@ -39,7 +39,7 @@ class GTSAM_UNSTABLE_EXPORT Event { Point3 location_; ///< Location at time event was generated public: - enum { dimension = 4 }; + inline constexpr static auto dimension = 4; /// Default Constructor Event() : time_(0), location_(0, 0, 0) {} From b91c470b6929bffca39bfbd36c39c3795183c79e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 13 Dec 2024 08:07:10 -0500 Subject: [PATCH 073/112] test exposing incorrect conversion --- gtsam/discrete/tests/testTableFactor.cpp | 30 ++++++++++++++++++++++++ 1 file changed, 30 insertions(+) diff --git a/gtsam/discrete/tests/testTableFactor.cpp b/gtsam/discrete/tests/testTableFactor.cpp index 212067cb3..7df6da83e 100644 --- a/gtsam/discrete/tests/testTableFactor.cpp +++ b/gtsam/discrete/tests/testTableFactor.cpp @@ -147,6 +147,36 @@ TEST(TableFactor, constructors) { EXPECT(assert_inequal(f5_with_wrong_keys, f5, 1e-9)); } +/* ************************************************************************* */ +// Check conversion from DecisionTreeFactor. +TEST(TableFactor, Conversion) { + /* This is the DecisionTree we are using + Choice(m2) + 0 Choice(m1) + 0 0 Leaf 0 + 0 1 Choice(m0) + 0 1 0 Leaf 0 + 0 1 1 Leaf 0.14649446 // 3 + 1 Choice(m1) + 1 0 Choice(m0) + 1 0 0 Leaf 0 + 1 0 1 Leaf 0.14648756 // 5 + 1 1 Choice(m0) + 1 1 0 Leaf 0.14649446 // 6 + 1 1 1 Leaf 0.23918345 // 7 + */ + DiscreteKeys dkeys = {{0, 2}, {1, 2}, {2, 2}}; + DecisionTreeFactor dtf( + dkeys, std::vector{0, 0, 0, 0.14649446, 0, 0.14648756, 0.14649446, + 0.23918345}); + + // dtf.print(); + TableFactor tf(dtf.discreteKeys(), dtf); + // tf.print(); + // tf.toDecisionTreeFactor().print(); + EXPECT(assert_equal(dtf, tf.toDecisionTreeFactor())); +} + /* ************************************************************************* */ // Check multiplication between two TableFactors. TEST(TableFactor, multiplication) { From a8e24efdeca680f391098979ad2402dd3918153f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 13 Dec 2024 09:34:01 -0500 Subject: [PATCH 074/112] update ComputeLeafOrdering to give a correct vector of values --- gtsam/discrete/TableFactor.cpp | 72 +++++++++++++++++++++++++++------- 1 file changed, 57 insertions(+), 15 deletions(-) diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp index 8e185eb3b..a2d68853e 100644 --- a/gtsam/discrete/TableFactor.cpp +++ b/gtsam/discrete/TableFactor.cpp @@ -64,27 +64,69 @@ TableFactor::TableFactor(const DiscreteKeys& dkeys, /** * @brief Compute the correct ordering of the leaves in the decision tree. * - * This is done by first taking all the values which have modulo 0 value with - * the cardinality of the innermost key `n`, and we go up to modulo n. - * * @param dt The DecisionTree - * @return std::vector + * @return Eigen::SparseVector */ -std::vector ComputeLeafOrdering(const DiscreteKeys& dkeys, - const DecisionTreeFactor& dt) { - std::vector probs = dt.probabilities(); - std::vector ordered; +static Eigen::SparseVector ComputeLeafOrdering( + const DiscreteKeys& dkeys, const DecisionTreeFactor& dt) { + // SparseVector needs to know the maximum possible index, + // so we compute the product of cardinalities. + size_t prod_cardinality = 1; + for (auto&& [_, c] : dt.cardinalities()) { + prod_cardinality *= c; + } + Eigen::SparseVector sparse_table(prod_cardinality); + size_t nrValues = 0; + dt.visit([&nrValues](double x) { + if (x > 0) nrValues += 1; + }); + sparse_table.reserve(nrValues); - size_t n = dkeys[0].second; + std::set allKeys(dt.keys().begin(), dt.keys().end()); - for (size_t k = 0; k < n; ++k) { - for (size_t idx = 0; idx < probs.size(); ++idx) { - if (idx % n == k) { - ordered.push_back(probs[idx]); + auto op = [&](const Assignment& assignment, double p) { + if (p > 0) { + // Get all the keys involved in this assignment + std::set assignment_keys; + for (auto&& [k, _] : assignment) { + assignment_keys.insert(k); + } + + // Find the keys missing in the assignment + std::vector diff; + std::set_difference(allKeys.begin(), allKeys.end(), + assignment_keys.begin(), assignment_keys.end(), + std::back_inserter(diff)); + + // Generate all assignments using the missing keys + DiscreteKeys extras; + for (auto&& key : diff) { + extras.push_back({key, dt.cardinality(key)}); + } + auto&& extra_assignments = DiscreteValues::CartesianProduct(extras); + + for (auto&& extra : extra_assignments) { + // Create new assignment using the extra assignment + DiscreteValues updated_assignment(assignment); + updated_assignment.insert(extra); + + // Generate index and add to the sparse vector. + Eigen::Index idx = 0; + size_t prev_cardinality = 1; + // We go in reverse since a DecisionTree has the highest label first + for (auto&& it = updated_assignment.rbegin(); + it != updated_assignment.rend(); it++) { + idx += prev_cardinality * it->second; + prev_cardinality *= dt.cardinality(it->first); + } + sparse_table.coeffRef(idx) = p; } } - } - return ordered; + }; + + dt.visitWith(op); + + return sparse_table; } /* ************************************************************************ */ From 7d389a53007af3811a0c8195d368e8fcd3825c3f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 13 Dec 2024 09:46:55 -0500 Subject: [PATCH 075/112] clean up test --- gtsam/discrete/tests/testTableFactor.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/gtsam/discrete/tests/testTableFactor.cpp b/gtsam/discrete/tests/testTableFactor.cpp index 7df6da83e..a455faaaa 100644 --- a/gtsam/discrete/tests/testTableFactor.cpp +++ b/gtsam/discrete/tests/testTableFactor.cpp @@ -170,10 +170,8 @@ TEST(TableFactor, Conversion) { dkeys, std::vector{0, 0, 0, 0.14649446, 0, 0.14648756, 0.14649446, 0.23918345}); - // dtf.print(); TableFactor tf(dtf.discreteKeys(), dtf); - // tf.print(); - // tf.toDecisionTreeFactor().print(); + EXPECT(assert_equal(dtf, tf.toDecisionTreeFactor())); } From 9830981351fd5e5b714c6a5dc58b6e67ba8ec7cc Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 13 Dec 2024 13:58:19 -0500 Subject: [PATCH 076/112] address review comments --- gtsam/discrete/TableFactor.cpp | 61 ++++++++++++++++++++++------------ 1 file changed, 39 insertions(+), 22 deletions(-) diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp index a2d68853e..de1e1f867 100644 --- a/gtsam/discrete/TableFactor.cpp +++ b/gtsam/discrete/TableFactor.cpp @@ -62,40 +62,55 @@ TableFactor::TableFactor(const DiscreteKeys& dkeys, : TableFactor(dkeys, DecisionTreeFactor(dkeys, dtree)) {} /** - * @brief Compute the correct ordering of the leaves in the decision tree. + * @brief Compute the indexing of the leaves in the decision tree based on the + * assignment and add the (index, leaf) pair to a SparseVector. + * + * We visit each leaf in the tree, and using the cardinalities of the keys, + * compute the correct index to add the leaf to a SparseVector which + * is then used to create the TableFactor. * * @param dt The DecisionTree * @return Eigen::SparseVector */ -static Eigen::SparseVector ComputeLeafOrdering( +static Eigen::SparseVector ComputeSparseTable( const DiscreteKeys& dkeys, const DecisionTreeFactor& dt) { // SparseVector needs to know the maximum possible index, // so we compute the product of cardinalities. - size_t prod_cardinality = 1; + size_t cardinalityProduct = 1; for (auto&& [_, c] : dt.cardinalities()) { - prod_cardinality *= c; + cardinalityProduct *= c; } - Eigen::SparseVector sparse_table(prod_cardinality); + Eigen::SparseVector sparseTable(cardinalityProduct); size_t nrValues = 0; dt.visit([&nrValues](double x) { if (x > 0) nrValues += 1; }); - sparse_table.reserve(nrValues); + sparseTable.reserve(nrValues); std::set allKeys(dt.keys().begin(), dt.keys().end()); + /** + * @brief Functor which is called by the DecisionTree for each leaf. + * For each leaf value, we use the corresponding assignment to compute a + * corresponding index into a SparseVector. We then populate sparseTable with + * the value at the computed index. + * + * Takes advantage of the sparsity of the DecisionTree to be efficient. When + * merged branches are encountered, we enumerate over the missing keys. + * + */ auto op = [&](const Assignment& assignment, double p) { if (p > 0) { // Get all the keys involved in this assignment - std::set assignment_keys; + std::set assignmentKeys; for (auto&& [k, _] : assignment) { - assignment_keys.insert(k); + assignmentKeys.insert(k); } // Find the keys missing in the assignment std::vector diff; std::set_difference(allKeys.begin(), allKeys.end(), - assignment_keys.begin(), assignment_keys.end(), + assignmentKeys.begin(), assignmentKeys.end(), std::back_inserter(diff)); // Generate all assignments using the missing keys @@ -103,41 +118,43 @@ static Eigen::SparseVector ComputeLeafOrdering( for (auto&& key : diff) { extras.push_back({key, dt.cardinality(key)}); } - auto&& extra_assignments = DiscreteValues::CartesianProduct(extras); + auto&& extraAssignments = DiscreteValues::CartesianProduct(extras); - for (auto&& extra : extra_assignments) { + for (auto&& extra : extraAssignments) { // Create new assignment using the extra assignment - DiscreteValues updated_assignment(assignment); - updated_assignment.insert(extra); + DiscreteValues updatedAssignment(assignment); + updatedAssignment.insert(extra); // Generate index and add to the sparse vector. Eigen::Index idx = 0; - size_t prev_cardinality = 1; + size_t previousCardinality = 1; // We go in reverse since a DecisionTree has the highest label first - for (auto&& it = updated_assignment.rbegin(); - it != updated_assignment.rend(); it++) { - idx += prev_cardinality * it->second; - prev_cardinality *= dt.cardinality(it->first); + for (auto&& it = updatedAssignment.rbegin(); + it != updatedAssignment.rend(); it++) { + idx += previousCardinality * it->second; + previousCardinality *= dt.cardinality(it->first); } - sparse_table.coeffRef(idx) = p; + sparseTable.coeffRef(idx) = p; } } }; + // Visit each leaf in `dt` to get the Assignment and leaf value + // to populate the sparseTable. dt.visitWith(op); - return sparse_table; + return sparseTable; } /* ************************************************************************ */ TableFactor::TableFactor(const DiscreteKeys& dkeys, const DecisionTreeFactor& dtf) - : TableFactor(dkeys, ComputeLeafOrdering(dkeys, dtf)) {} + : TableFactor(dkeys, ComputeSparseTable(dkeys, dtf)) {} /* ************************************************************************ */ TableFactor::TableFactor(const DecisionTreeFactor& dtf) : TableFactor(dtf.discreteKeys(), - ComputeLeafOrdering(dtf.discreteKeys(), dtf)) {} + ComputeSparseTable(dtf.discreteKeys(), dtf)) {} /* ************************************************************************ */ TableFactor::TableFactor(const DiscreteConditional& c) From 923afba4d81c083a750be9b9f355310012590b1a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 13 Dec 2024 23:36:27 -0500 Subject: [PATCH 077/112] Endow NavState with group operations --- gtsam/navigation/NavState.cpp | 255 +++++++++++++- gtsam/navigation/NavState.h | 126 ++++++- gtsam/navigation/tests/testNavState.cpp | 443 +++++++++++++++++++++++- 3 files changed, 779 insertions(+), 45 deletions(-) diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index 3e2817752..9f91ae415 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -77,10 +77,13 @@ Vector3 NavState::bodyVelocity(OptionalJacobian<3, 9> H) const { } //------------------------------------------------------------------------------ -Matrix7 NavState::matrix() const { +Matrix5 NavState::matrix() const { Matrix3 R = this->R(); - Matrix7 T; - T << R, Z_3x3, t(), Z_3x3, R, v(), Vector6::Zero().transpose(), 1.0; + + Matrix5 T = Matrix5::Identity(); + T.block<3, 3>(0, 0) = R; + T.block<3, 1>(0, 3) = t_; + T.block<3, 1>(0, 4) = v_; return T; } @@ -103,9 +106,215 @@ bool NavState::equals(const NavState& other, double tol) const { && equal_with_abs_tol(v_, other.v_, tol); } +//------------------------------------------------------------------------------ +NavState NavState::inverse() const { + Rot3 Rt = R_.inverse(); + return NavState(Rt, Rt * (-t_), Rt * -(v_)); +} + +//------------------------------------------------------------------------------ +NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi) { + if (Hxi) *Hxi = ExpmapDerivative(xi); + + // Order is rotation, position, velocity, represented by ω,ρ,ν + Vector3 omega(xi(0), xi(1), xi(2)), rho(xi(3), xi(4), xi(5)), + nu(xi(6), xi(7), xi(8)); + + Rot3 R = Rot3::Expmap(omega); + + double omega_norm = omega.norm(); + + if (omega_norm < 1e-8) { + return NavState(Rot3(), Point3(rho), Point3(nu)); + + } else { + Matrix W = skewSymmetric(omega); + double omega_norm2 = omega_norm * omega_norm; + double omega_norm3 = omega_norm2 * omega_norm; + Matrix A = I_3x3 + ((1 - cos(omega_norm)) / omega_norm2) * W + + ((omega_norm - sin(omega_norm)) / omega_norm3) * (W * W); + + return NavState(Rot3::Expmap(omega), Point3(A * rho), Point3(A * nu)); + } +} + +//------------------------------------------------------------------------------ +Vector9 NavState::Logmap(const NavState& state, OptionalJacobian<9, 9> Hstate) { + if (Hstate) *Hstate = LogmapDerivative(state); + + const Vector3 phi = Rot3::Logmap(state.rotation()); + const Vector3& p = state.position(); + const Vector3& v = state.velocity(); + const double t = phi.norm(); + if (t < 1e-8) { + Vector9 log; + log << phi, p, v; + return log; + + } else { + const Matrix3 W = skewSymmetric(phi / t); + + const double Tan = tan(0.5 * t); + const Vector3 Wp = W * p; + const Vector3 Wv = W * v; + const Vector3 rho = p - (0.5 * t) * Wp + (1 - t / (2. * Tan)) * (W * Wp); + const Vector3 nu = v - (0.5 * t) * Wv + (1 - t / (2. * Tan)) * (W * Wv); + Vector9 log; + // Order is ω, p, v + log << phi, rho, nu; + return log; + } +} + +//------------------------------------------------------------------------------ +Matrix9 NavState::AdjointMap() const { + const Matrix3 R = R_.matrix(); + Matrix3 A = skewSymmetric(t_.x(), t_.y(), t_.z()) * R; + Matrix3 B = skewSymmetric(v_.x(), v_.y(), v_.z()) * R; + // Eqn 2 in Barrau20icra + Matrix9 adj; + adj << R, Z_3x3, Z_3x3, A, R, Z_3x3, B, Z_3x3, R; + return adj; +} + +//------------------------------------------------------------------------------ +Vector9 NavState::Adjoint(const Vector9& xi_b, OptionalJacobian<9, 9> H_state, + OptionalJacobian<9, 9> H_xib) const { + const Matrix9 Ad = AdjointMap(); + + // Jacobians + if (H_state) *H_state = -Ad * adjointMap(xi_b); + if (H_xib) *H_xib = Ad; + + return Ad * xi_b; +} + +//------------------------------------------------------------------------------ +Matrix9 NavState::adjointMap(const Vector9& xi) { + Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2)); + Matrix3 v_hat = skewSymmetric(xi(3), xi(4), xi(5)); + Matrix3 a_hat = skewSymmetric(xi(6), xi(7), xi(8)); + Matrix9 adj; + adj << w_hat, Z_3x3, Z_3x3, v_hat, w_hat, Z_3x3, a_hat, Z_3x3, w_hat; + return adj; +} + +//------------------------------------------------------------------------------ +Vector9 NavState::adjoint(const Vector9& xi, const Vector9& y, + OptionalJacobian<9, 9> Hxi, + OptionalJacobian<9, 9> H_y) { + if (Hxi) { + Hxi->setZero(); + for (int i = 0; i < 9; ++i) { + Vector9 dxi; + dxi.setZero(); + dxi(i) = 1.0; + Matrix9 Gi = adjointMap(dxi); + Hxi->col(i) = Gi * y; + } + } + + const Matrix9& ad_xi = adjointMap(xi); + if (H_y) *H_y = ad_xi; + + return ad_xi * y; +} + +/* ************************************************************************* */ +Matrix63 NavState::ComputeQforExpmapDerivative(const Vector9& xi, + double nearZeroThreshold) { + const auto omega = xi.head<3>(); + const auto nu = xi.segment<3>(3); + const auto rho = xi.tail<3>(); + const Matrix3 V = skewSymmetric(nu); + const Matrix3 P = skewSymmetric(rho); + const Matrix3 W = skewSymmetric(omega); + + Matrix3 Qv, Qp; + Matrix63 Q; + + // The closed-form formula in Barfoot14tro eq. (102) + double phi = omega.norm(); + if (std::abs(phi) > 1e-5) { + const double sinPhi = sin(phi), cosPhi = cos(phi); + const double phi2 = phi * phi, phi3 = phi2 * phi, phi4 = phi3 * phi, + phi5 = phi4 * phi; + // Invert the sign of odd-order terms to have the right Jacobian + Qv = -0.5 * V + (phi - sinPhi) / phi3 * (W * V + V * W - W * V * W) + + (1 - phi2 / 2 - cosPhi) / phi4 * + (W * W * V + V * W * W - 3 * W * V * W) - + 0.5 * + ((1 - phi2 / 2 - cosPhi) / phi4 - + 3 * (phi - sinPhi - phi3 / 6.) / phi5) * + (W * V * W * W + W * W * V * W); + Qp = -0.5 * P + (phi - sinPhi) / phi3 * (W * P + P * W - W * P * W) + + (1 - phi2 / 2 - cosPhi) / phi4 * + (W * W * P + P * W * W - 3 * W * P * W) - + 0.5 * + ((1 - phi2 / 2 - cosPhi) / phi4 - + 3 * (phi - sinPhi - phi3 / 6.) / phi5) * + (W * P * W * W + W * W * P * W); + } else { + Qv = -0.5 * V + 1. / 6. * (W * V + V * W - W * V * W) + + 1. / 24. * (W * W * V + V * W * W - 3 * W * V * W) - + 0.5 * (1. / 24. + 3. / 120.) * (W * V * W * W + W * W * V * W); + Qp = -0.5 * P + 1. / 6. * (W * P + P * W - W * P * W) + + 1. / 24. * (W * W * P + P * W * W - 3 * W * P * W) - + 0.5 * (1. / 24. + 3. / 120.) * (W * P * W * W + W * W * P * W); + } + + Q << Qv, Qp; + return Q; +} + +//------------------------------------------------------------------------------ +Matrix9 NavState::ExpmapDerivative(const Vector9& xi) { + const Vector3 w = xi.head<3>(); + const Matrix3 Jw = Rot3::ExpmapDerivative(w); + const Matrix63 Q = ComputeQforExpmapDerivative(xi); + const Matrix3 Qv = Q.topRows<3>(); + const Matrix3 Qp = Q.bottomRows<3>(); + + Matrix9 J; + J << Jw, Z_3x3, Z_3x3, Qv, Jw, Z_3x3, Qp, Z_3x3, Jw; + + return J; +} + +//------------------------------------------------------------------------------ +Matrix9 NavState::LogmapDerivative(const NavState& state) { + const Vector9 xi = Logmap(state); + const Vector3 w = xi.head<3>(); + const Matrix3 Jw = Rot3::LogmapDerivative(w); + const Matrix63 Q = ComputeQforExpmapDerivative(xi); + const Matrix3 Qv = Q.topRows<3>(); + const Matrix3 Qp = Q.bottomRows<3>(); + const Matrix3 Qv2 = -Jw * Qv * Jw; + const Matrix3 Qp2 = -Jw * Qp * Jw; + + Matrix9 J; + J << Jw, Z_3x3, Z_3x3, Qv2, Jw, Z_3x3, Qp2, Z_3x3, Jw; + return J; +} + + +//------------------------------------------------------------------------------ +NavState NavState::ChartAtOrigin::Retract(const Vector9& xi, + ChartJacobian Hxi) { + return Expmap(xi, Hxi); +} + +//------------------------------------------------------------------------------ +Vector9 NavState::ChartAtOrigin::Local(const NavState& state, + ChartJacobian Hstate) { + return Logmap(state, Hstate); +} + //------------------------------------------------------------------------------ NavState NavState::retract(const Vector9& xi, // OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const { + // return LieGroup::retract(xi, H1, H2); + Rot3 nRb = R_; Point3 n_t = t_, n_v = v_; Matrix3 D_bRc_xi, D_R_nRb, D_t_nRb, D_v_nRb; @@ -133,6 +342,30 @@ NavState NavState::retract(const Vector9& xi, // //------------------------------------------------------------------------------ Vector9 NavState::localCoordinates(const NavState& g, // OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const { + // return LieGroup::localCoordinates(g, H1, H2); + + //TODO(Varun) Fix so that test on L680 passes + + // Matrix3 D_dR_R, D_dt_R, D_dv_R; + // const Rot3 dR = R_.between(g.R_, H1 ? &D_dR_R : 0); + // const Point3 dP = R_.unrotate(g.t_ - t_, H1 ? &D_dt_R : 0); + // const Vector dV = R_.unrotate(g.v_ - v_, H1 ? &D_dv_R : 0); + + // Vector9 xi; + // Matrix3 D_xi_R; + // xi << Rot3::Logmap(dR, (H1 || H2) ? &D_xi_R : 0), dP, dV; + // if (H1) { + // *H1 << D_xi_R * D_dR_R, Z_3x3, Z_3x3, // + // D_dt_R, -I_3x3, Z_3x3, // + // D_dv_R, Z_3x3, -I_3x3; + // } + // if (H2) { + // *H2 << D_xi_R, Z_3x3, Z_3x3, // + // Z_3x3, dR.matrix(), Z_3x3, // + // Z_3x3, Z_3x3, dR.matrix(); + // } + // return xi; + Matrix3 D_dR_R, D_dt_R, D_dv_R; const Rot3 dR = R_.between(g.R_, H1 ? &D_dR_R : 0); const Point3 dP = R_.unrotate(g.t_ - t_, H1 ? &D_dt_R : 0); @@ -142,15 +375,16 @@ Vector9 NavState::localCoordinates(const NavState& g, // Matrix3 D_xi_R; xi << Rot3::Logmap(dR, (H1 || H2) ? &D_xi_R : 0), dP, dV; if (H1) { - *H1 << D_xi_R * D_dR_R, Z_3x3, Z_3x3, // - D_dt_R, -I_3x3, Z_3x3, // - D_dv_R, Z_3x3, -I_3x3; + *H1 << D_xi_R * D_dR_R, Z_3x3, Z_3x3, // + D_dt_R, -I_3x3, Z_3x3, // + D_dv_R, Z_3x3, -I_3x3; } if (H2) { - *H2 << D_xi_R, Z_3x3, Z_3x3, // - Z_3x3, dR.matrix(), Z_3x3, // - Z_3x3, Z_3x3, dR.matrix(); + *H2 << D_xi_R, Z_3x3, Z_3x3, // + Z_3x3, dR.matrix(), Z_3x3, // + Z_3x3, Z_3x3, dR.matrix(); } + return xi; } @@ -213,7 +447,8 @@ NavState NavState::update(const Vector3& b_acceleration, const Vector3& b_omega, //------------------------------------------------------------------------------ Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder, OptionalJacobian<9, 9> H) const { - auto [nRb, n_t, n_v] = (*this); + Rot3 nRb = R_; + Point3 n_t = t_, n_v = v_; const double dt2 = dt * dt; const Vector3 omega_cross_vel = omega.cross(n_v); diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h index 35ce435a3..2f037a795 100644 --- a/gtsam/navigation/NavState.h +++ b/gtsam/navigation/NavState.h @@ -12,7 +12,7 @@ /** * @file NavState.h * @brief Navigation state composing of attitude, position, and velocity - * @author Frank Dellaert + * @authors Frank Dellaert, Varun Agrawal, Fan Jiang * @date July 2015 **/ @@ -25,14 +25,18 @@ namespace gtsam { /// Velocity is currently typedef'd to Vector3 -typedef Vector3 Velocity3; +using Velocity3 = Vector3; /** * Navigation state: Pose (rotation, translation) + velocity - * NOTE(frank): it does not make sense to make this a Lie group, but it is a 9D manifold + * Following Barrau20icra, this class belongs to the Lie group SE_2(3). + * This group is also called "double direct isometries”. + * + * NOTE: While Barrau20icra follow a R,v,t order, + * we use a R,t,v order to maintain backwards compatibility. */ -class GTSAM_EXPORT NavState { -private: +class GTSAM_EXPORT NavState : public LieGroup { + private: // TODO(frank): // - should we rename t_ to p_? if not, we should rename dP do dT @@ -44,7 +48,6 @@ public: inline constexpr static auto dimension = 9; - typedef std::pair PositionAndVelocity; /// @name Constructors /// @{ @@ -67,11 +70,14 @@ public: } /// Named constructor with derivatives static NavState Create(const Rot3& R, const Point3& t, const Velocity3& v, - OptionalJacobian<9, 3> H1, OptionalJacobian<9, 3> H2, - OptionalJacobian<9, 3> H3); + OptionalJacobian<9, 3> H1 = {}, + OptionalJacobian<9, 3> H2 = {}, + OptionalJacobian<9, 3> H3 = {}); + /// Named constructor with derivatives static NavState FromPoseVelocity(const Pose3& pose, const Vector3& vel, - OptionalJacobian<9, 6> H1, OptionalJacobian<9, 3> H2); + OptionalJacobian<9, 6> H1 = {}, + OptionalJacobian<9, 3> H2 = {}); /// @} /// @name Component Access @@ -109,9 +115,8 @@ public: Velocity3 bodyVelocity(OptionalJacobian<3, 9> H = {}) const; /// Return matrix group representation, in MATLAB notation: - /// nTb = [nRb 0 n_t; 0 nRb n_v; 0 0 1] - /// With this embedding in GL(3), matrix product agrees with compose - Matrix7 matrix() const; + /// nTb = [nRb n_t n_v; 0_1x3 1 0; 0_1x3 0 1] + Matrix5 matrix() const; /// @} /// @name Testable @@ -128,7 +133,29 @@ public: bool equals(const NavState& other, double tol = 1e-8) const; /// @} - /// @name Manifold + /// @name Group + /// @{ + + /// identity for group operation + static NavState Identity() { + return NavState(); + } + + /// inverse transformation with derivatives + NavState inverse() const; + + using LieGroup::inverse; // version with derivative + + /// compose syntactic sugar + NavState operator*(const NavState& T) const { + return NavState(R_ * T.R_, t_ + R_ * T.t_, v_ + R_ * T.v_); + } + + /// Syntactic sugar + const Rot3& rotation() const { return attitude(); }; + + /// @} + /// @name Lie Group /// @{ // Tangent space sugar. @@ -162,6 +189,66 @@ public: OptionalJacobian<9, 9> H1 = {}, OptionalJacobian<9, 9> H2 = {}) const; + /** + * Exponential map at identity - create a NavState from canonical coordinates + * \f$ [R_x,R_y,R_z,T_x,T_y,T_z,V_x,V_y,V_z] \f$ + */ + static NavState Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi = {}); + + /** + * Log map at identity - return the canonical coordinates \f$ + * [R_x,R_y,R_z,T_x,T_y,T_z,V_x,V_y,V_z] \f$ of this NavState + */ + static Vector9 Logmap(const NavState& pose, OptionalJacobian<9, 9> Hpose = {}); + + /** + * Calculate Adjoint map, transforming a twist in this pose's (i.e, body) + * frame to the world spatial frame. + */ + Matrix9 AdjointMap() const; + + /** + * Apply this NavState's AdjointMap Ad_g to a twist \f$ \xi_b \f$, i.e. a + * body-fixed velocity, transforming it to the spatial frame + * \f$ \xi^s = g*\xi^b*g^{-1} = Ad_g * \xi^b \f$ + * Note that H_xib = AdjointMap() + */ + Vector9 Adjoint(const Vector9& xi_b, + OptionalJacobian<9, 9> H_this = {}, + OptionalJacobian<9, 9> H_xib = {}) const; + + /** + * Compute the [ad(w,v)] operator as defined in [Kobilarov09siggraph], pg 11 + * but for the NavState [ad(w,v)] = [w^, zero3; v^, w^] + */ + static Matrix9 adjointMap(const Vector9& xi); + + /** + * Action of the adjointMap on a Lie-algebra vector y, with optional derivatives + */ + static Vector9 adjoint(const Vector9& xi, const Vector9& y, + OptionalJacobian<9, 9> Hxi = {}, + OptionalJacobian<9, 9> H_y = {}); + + /// Derivative of Expmap + static Matrix9 ExpmapDerivative(const Vector9& xi); + + /// Derivative of Logmap + static Matrix9 LogmapDerivative(const NavState& xi); + + // Chart at origin, depends on compile-time flag GTSAM_POSE3_EXPMAP + struct GTSAM_EXPORT ChartAtOrigin { + static NavState Retract(const Vector9& xi, ChartJacobian Hxi = {}); + static Vector9 Local(const NavState& state, ChartJacobian Hstate = {}); + }; + + /** + * Compute the 6x3 bottom-left block Qs of the SE_2(3) Expmap derivative + * matrix + */ + static Matrix63 ComputeQforExpmapDerivative(const Vector9& xi, + double nearZeroThreshold = 1e-5); + /// @} /// @name Dynamics /// @{ @@ -169,8 +256,9 @@ public: /// Integrate forward in time given angular velocity and acceleration in body frame /// Uses second order integration for position, returns derivatives except dt. NavState update(const Vector3& b_acceleration, const Vector3& b_omega, - const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1, - OptionalJacobian<9, 3> G2) const; + const double dt, OptionalJacobian<9, 9> F = {}, + OptionalJacobian<9, 3> G1 = {}, + OptionalJacobian<9, 3> G2 = {}) const; /// Compute tangent space contribution due to Coriolis forces Vector9 coriolis(double dt, const Vector3& omega, bool secondOrder = false, @@ -201,8 +289,10 @@ private: }; // Specialize NavState traits to use a Retract/Local that agrees with IMUFactors -template<> -struct traits : internal::Manifold { -}; +template <> +struct traits : public internal::LieGroup {}; + +template <> +struct traits : public internal::LieGroup {}; } // namespace gtsam diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp index b8f081518..8934129ee 100644 --- a/gtsam/navigation/tests/testNavState.cpp +++ b/gtsam/navigation/tests/testNavState.cpp @@ -12,13 +12,16 @@ /** * @file testNavState.cpp * @brief Unit tests for NavState - * @author Frank Dellaert + * @authors Frank Dellaert, Varun Agrawal, Fan Jiang * @date July 2015 */ #include -#include + +#include #include +#include +#include #include @@ -26,6 +29,9 @@ using namespace std::placeholders; using namespace std; using namespace gtsam; +GTSAM_CONCEPT_TESTABLE_INST(NavState) +GTSAM_CONCEPT_LIE_INST(NavState) + static const Rot3 kAttitude = Rot3::RzRyRx(0.1, 0.2, 0.3); static const Point3 kPosition(1.0, 2.0, 3.0); static const Pose3 kPose(kAttitude, kPosition); @@ -36,6 +42,16 @@ static const Vector3 kOmegaCoriolis(0.02, 0.03, 0.04); static const Vector3 kGravity(0, 0, 9.81); static const Vector9 kZeroXi = Vector9::Zero(); +static const Point3 V(3, 0.4, -2.2); +static const Point3 P(0.2, 0.7, -2); +static const Rot3 R = Rot3::Rodrigues(0.3, 0, 0); +static const Point3 V2(-6.5, 3.5, 6.2); +static const Point3 P2(3.5, -8.2, 4.2); +static const NavState T(R, P2, V2); +static const NavState T2(Rot3::Rodrigues(0.3, 0.2, 0.1), P2, V2); +static const NavState T3(Rot3::Rodrigues(-90, 0, 0), Point3(5, 6, 7), + Point3(1, 2, 3)); + /* ************************************************************************* */ TEST(NavState, Constructor) { std::function create = @@ -46,14 +62,14 @@ TEST(NavState, Constructor) { assert_equal(kState1, NavState::Create(kAttitude, kPosition, kVelocity, aH1, aH2, aH3))); EXPECT( - assert_equal( - numericalDerivative31(create, kAttitude, kPosition, kVelocity), aH1)); +assert_equal( + numericalDerivative31(create, kAttitude, kPosition, kVelocity), aH1)); EXPECT( - assert_equal( - numericalDerivative32(create, kAttitude, kPosition, kVelocity), aH2)); +assert_equal( + numericalDerivative32(create, kAttitude, kPosition, kVelocity), aH2)); EXPECT( - assert_equal( - numericalDerivative32(create, kAttitude, kPosition, kVelocity), aH2)); +assert_equal( + numericalDerivative32(create, kAttitude, kPosition, kVelocity), aH2)); } /* ************************************************************************* */ @@ -64,7 +80,7 @@ TEST(NavState, Constructor2) { Matrix aH1, aH2; EXPECT( assert_equal(kState1, - NavState::FromPoseVelocity(kPose, kVelocity, aH1, aH2))); + NavState::FromPoseVelocity(kPose, kVelocity, aH1, aH2))); EXPECT(assert_equal(numericalDerivative21(construct, kPose, kVelocity), aH1)); EXPECT(assert_equal(numericalDerivative22(construct, kPose, kVelocity), aH2)); } @@ -127,8 +143,8 @@ TEST( NavState, Manifold ) { Point3 dt = Point3(xi.segment<3>(3)); Velocity3 dvel = Velocity3(-0.1, -0.2, -0.3); NavState state2 = NavState(kState1.attitude() * drot, - kState1.position() + kState1.attitude() * dt, - kState1.velocity() + kState1.attitude() * dvel); + kState1.position() + kState1.attitude() * dt, + kState1.velocity() + kState1.attitude() * dvel); EXPECT(assert_equal(state2, kState1.retract(xi))); EXPECT(assert_equal(xi, kState1.localCoordinates(state2))); @@ -169,6 +185,143 @@ TEST( NavState, Manifold ) { EXPECT(assert_equal(numericalDerivative22(local, state2, kIdentity), aH2)); } +/* ************************************************************************* */ +TEST(NavState, Equals) { + NavState T3(Rot3::Rodrigues(-90, 0, 0), Point3(5, 6, 7), Point3(1, 2, 3)); + NavState pose2 = T3; + EXPECT(T3.equals(pose2)); + NavState origin; + EXPECT(!T3.equals(origin)); +} + +/* ************************************************************************* */ +TEST(NavState, Compose) { + NavState nav_state_a(Rot3::Identity(), {0.0, 1.0, 2.0}, {1.0, -1.0, 1.0}); + NavState nav_state_b(Rot3::Rx(M_PI_4), {0.0, 1.0, 3.0}, {1.0, -1.0, 2.0}); + NavState nav_state_c(Rot3::Ry(M_PI / 180.0), {1.0, 1.0, 2.0}, + {3.0, -1.0, 1.0}); + + auto ab_c = (nav_state_a * nav_state_b) * nav_state_c; + auto a_bc = nav_state_a * (nav_state_b * nav_state_c); + CHECK(assert_equal(ab_c, a_bc)); + + Matrix actual = (T2 * T2).matrix(); + + Matrix expected = T2.matrix() * T2.matrix(); + EXPECT(assert_equal(actual, expected, 1e-8)); + + Matrix actualDcompose1, actualDcompose2; + T2.compose(T2, actualDcompose1, actualDcompose2); + + Matrix numericalH1 = + numericalDerivative21(testing::compose, T2, T2); + + EXPECT(assert_equal(numericalH1, actualDcompose1, 5e-3)); + EXPECT(assert_equal(T2.inverse().AdjointMap(), actualDcompose1, 5e-3)); + + Matrix numericalH2 = + numericalDerivative22(testing::compose, T2, T2); + EXPECT(assert_equal(numericalH2, actualDcompose2, 1e-4)); +} + +/* ************************************************************************* */ +// Check compose and its push-forward, another case +TEST(NavState, Compose2) { + const NavState& T1 = T; + Matrix actual = (T1 * T2).matrix(); + Matrix expected = T1.matrix() * T2.matrix(); + EXPECT(assert_equal(actual, expected, 1e-8)); + + Matrix actualDcompose1, actualDcompose2; + T1.compose(T2, actualDcompose1, actualDcompose2); + + Matrix numericalH1 = + numericalDerivative21(testing::compose, T1, T2); + EXPECT(assert_equal(numericalH1, actualDcompose1, 5e-3)); + EXPECT(assert_equal(T2.inverse().AdjointMap(), actualDcompose1, 5e-3)); + + Matrix numericalH2 = + numericalDerivative22(testing::compose, T1, T2); + EXPECT(assert_equal(numericalH2, actualDcompose2, 1e-5)); +} + +/* ************************************************************************* */ +TEST(NavState, Inverse) { + NavState nav_state_a(Rot3::Identity(), {0.0, 1.0, 2.0}, {1.0, -1.0, 1.0}); + NavState nav_state_b(Rot3::Rx(M_PI_4), {0.0, 1.0, 3.0}, {1.0, -1.0, 2.0}); + NavState nav_state_c(Rot3::Ry(M_PI / 180.0), {1.0, 1.0, 2.0}, + {3.0, -1.0, 1.0}); + + auto a_inv = nav_state_a.inverse(); + auto a_a_inv = nav_state_a * a_inv; + CHECK(assert_equal(a_a_inv, NavState())); + + auto b_inv = nav_state_b.inverse(); + auto b_b_inv = nav_state_b * b_inv; + CHECK(assert_equal(b_b_inv, NavState())); + + Matrix actualDinverse; + Matrix actual = T.inverse(actualDinverse).matrix(); + Matrix expected = T.matrix().inverse(); + EXPECT(assert_equal(actual, expected, 1e-8)); + + Matrix numericalH = numericalDerivative11(testing::inverse, T); + EXPECT(assert_equal(numericalH, actualDinverse, 5e-3)); + EXPECT(assert_equal(-T.AdjointMap(), actualDinverse, 5e-3)); +} + +/* ************************************************************************* */ +TEST(NavState, InverseDerivatives) { + Rot3 R = Rot3::Rodrigues(0.3, 0.4, -0.5); + Vector3 v(3.5, -8.2, 4.2); + Point3 p(3.5, -8.2, 4.2); + NavState T(R, p, v); + + Matrix numericalH = numericalDerivative11(testing::inverse, T); + Matrix actualDinverse; + T.inverse(actualDinverse); + EXPECT(assert_equal(numericalH, actualDinverse, 5e-3)); + EXPECT(assert_equal(-T.AdjointMap(), actualDinverse, 5e-3)); +} + +/* ************************************************************************* */ +TEST(NavState, Compose_Inverse) { + NavState actual = T * T.inverse(); + NavState expected; + EXPECT(assert_equal(actual, expected, 1e-8)); +} + +/* ************************************************************************* */ +TEST(NavState, Between) { + NavState s1, s2(Rot3(), Point3(1, 2, 3), Velocity3(0, 0, 0)); + + NavState actual = s1.compose(s2); + EXPECT(assert_equal(s2, actual)); + + NavState between = s2.between(s1); + NavState expected_between(Rot3(), Point3(-1, -2, -3), Velocity3(0, 0, 0)); + EXPECT(assert_equal(expected_between, between)); + + NavState expected = T2.inverse() * T3; + Matrix actualDBetween1, actualDBetween2; + actual = T2.between(T3, actualDBetween1, actualDBetween2); + EXPECT(assert_equal(expected, actual)); + + Matrix numericalH1 = + numericalDerivative21(testing::between, T2, T3); + EXPECT(assert_equal(numericalH1, actualDBetween1, 5e-3)); + + Matrix numericalH2 = + numericalDerivative22(testing::between, T2, T3); + EXPECT(assert_equal(numericalH2, actualDBetween2, 1e-5)); +} + +/* ************************************************************************* */ +TEST(NavState, interpolate) { + EXPECT(assert_equal(T2, interpolate(T2, T3, 0.0))); + EXPECT(assert_equal(T3, interpolate(T2, T3, 1.0))); +} + /* ************************************************************************* */ static const double dt = 2.0; std::function coriolis = @@ -189,7 +342,7 @@ TEST(NavState, Coriolis) { TEST(NavState, Coriolis2) { Matrix9 aH; NavState state2(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0), - Point3(5.0, 1.0, -50.0), Vector3(0.5, 0.0, 0.0)); + Point3(5.0, 1.0, -50.0), Vector3(0.5, 0.0, 0.0)); // first-order state2.coriolis(dt, kOmegaCoriolis, false, aH); @@ -200,10 +353,10 @@ TEST(NavState, Coriolis2) { } TEST(NavState, Coriolis3) { - /** Consider a massless planet with an attached nav frame at - * n_omega = [0 0 1]', and a body at position n_t = [1 0 0]', travelling with + /** Consider a massless planet with an attached nav frame at + * n_omega = [0 0 1]', and a body at position n_t = [1 0 0]', travelling with * velocity n_v = [0 1 0]'. Orient the body so that it is not instantaneously - * aligned with the nav frame (i.e., nRb != I_3x3). Test that first and + * aligned with the nav frame (i.e., nRb != I_3x3). Test that first and * second order Coriolis corrections are as expected. */ @@ -216,9 +369,9 @@ TEST(NavState, Coriolis3) { bRn = nRb.inverse(); // Get expected first and second order corrections in the nav frame - Vector3 n_dP1e = 0.5 * dt2 * n_aCorr1, + Vector3 n_dP1e = 0.5 * dt2 * n_aCorr1, n_dP2e = 0.5 * dt2 * (n_aCorr1 + n_aCorr2), - n_dV1e = dt * n_aCorr1, + n_dV1e = dt * n_aCorr1, n_dV2e = dt * (n_aCorr1 + n_aCorr2); // Get expected first and second order corrections in the body frame @@ -271,6 +424,262 @@ TEST(NavState, Stream) EXPECT(os.str() == expected); } +/* ************************************************************************* */ +TEST(NavState, Print) { + NavState state(Rot3::Identity(), Point3(1, 2, 3), Vector3(1, 2, 3)); + + // Generate the expected output + std::string R = "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\n"; + std::string p = "p: 1 2 3\n"; + std::string v = "v: 1 2 3\n"; + std::string expected = R + p + v; + + EXPECT(assert_print_equal(expected, state)); +} + +/* ************************************************************************* */ +#ifndef GTSAM_POSE3_EXPMAP +TEST(NavState, Retract_first_order) { + NavState id; + Vector v = Z_9x1; + v(0) = 0.3; + EXPECT(assert_equal(NavState(R, Point3(0, 0, 0), Vector3(0, 0, 0)), + id.retract(v), 1e-2)); + v(3) = 0.2; + v(4) = 0.7; + v(5) = -2; + v(6) = 3; + v(7) = 0.4; + v(8) = -2.2; + EXPECT(assert_equal(NavState(R, P, V), id.retract(v), 1e-2)); +} +#endif + +/* ************************************************************************* */ +TEST(NavState, RetractExpmap) { + Vector xi = Z_9x1; + xi(0) = 0.3; + NavState pose = NavState::Expmap(xi), + expected(R, Point3(0, 0, 0), Point3(0, 0, 0)); + EXPECT(assert_equal(expected, pose, 1e-2)); + EXPECT(assert_equal(xi, NavState::Logmap(pose), 1e-2)); +} + +/* ************************************************************************* */ +TEST(NavState, Expmap_A_Full) { + NavState id; + Vector xi = Z_9x1; + xi(0) = 0.3; + EXPECT(assert_equal(expmap_default(id, xi), + NavState(R, Point3(0, 0, 0), Point3(0, 0, 0)))); + xi(3) = -0.2; + xi(4) = -0.394742; + xi(5) = 2.08998; + xi(6) = 0.2; + xi(7) = 0.394742; + xi(8) = -2.08998; + + NavState expected(R, -P, P); + EXPECT(assert_equal(expected, expmap_default(id, xi), 1e-5)); +} + +/* ************************************************************************* */ +TEST(NavState, Expmap_b) { + NavState p1(Rot3(), Point3(-100, 0, 0), Point3(100, 0, 0)); + NavState p2 = p1.retract( + (Vector(9) << 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0).finished()); + NavState expected(Rot3::Rodrigues(0.0, 0.0, 0.1), Point3(-100.0, 0.0, 0.0), + Point3(100.0, 0.0, 0.0)); + EXPECT(assert_equal(expected, p2, 1e-2)); +} + +/* ************************************************************************* */ +// test case for screw motion in the plane +namespace screwNavState { +double a = 0.3, c = cos(a), s = sin(a), w = 0.3; +Vector xi = (Vector(9) << 0.0, 0.0, w, w, 0.0, 1.0, w, 0.0, 1.0).finished(); +Rot3 expectedR(c, -s, 0, s, c, 0, 0, 0, 1); +Point3 expectedV(0.29552, 0.0446635, 1); +Point3 expectedP(0.29552, 0.0446635, 1); +NavState expected(expectedR, expectedV, expectedP); +} // namespace screwNavState + +/* ************************************************************************* */ +// assert that T*exp(xi)*T^-1 is equal to exp(Ad_T(xi)) +TEST(NavState, Adjoint_full) { + NavState expected = T * NavState::Expmap(screwNavState::xi) * T.inverse(); + Vector xiPrime = T.Adjoint(screwNavState::xi); + EXPECT(assert_equal(expected, NavState::Expmap(xiPrime), 1e-6)); + + NavState expected2 = T2 * NavState::Expmap(screwNavState::xi) * T2.inverse(); + Vector xiPrime2 = T2.Adjoint(screwNavState::xi); + EXPECT(assert_equal(expected2, NavState::Expmap(xiPrime2), 1e-6)); + + NavState expected3 = T3 * NavState::Expmap(screwNavState::xi) * T3.inverse(); + Vector xiPrime3 = T3.Adjoint(screwNavState::xi); + EXPECT(assert_equal(expected3, NavState::Expmap(xiPrime3), 1e-6)); +} + +/* ************************************************************************* */ +TEST(NavState, Adjoint_compose_full) { + // To debug derivatives of compose, assert that + // T1*T2*exp(Adjoint(inv(T2),x) = T1*exp(x)*T2 + const NavState& T1 = T; + Vector x = + (Vector(9) << 0.1, 0.1, 0.1, 0.4, 0.2, 0.8, 0.4, 0.2, 0.8).finished(); + NavState expected = T1 * NavState::Expmap(x) * T2; + Vector y = T2.inverse().Adjoint(x); + NavState actual = T1 * T2 * NavState::Expmap(y); + EXPECT(assert_equal(expected, actual, 1e-6)); +} + +/* ************************************************************************* */ +TEST(NavState, Retract_LocalCoordinates) { + Vector9 d; + d << 1, 2, 3, 4, 5, 6, 7, 8, 9; + d /= 10; + const Rot3 R = Rot3::Retract(d.head<3>()); + NavState t = NavState::Retract(d); + EXPECT(assert_equal(d, NavState::LocalCoordinates(t))); +} +/* ************************************************************************* */ +TEST(NavState, retract_localCoordinates) { + Vector9 d12; + d12 << 1, 2, 3, 4, 5, 6, 7, 8, 9; + d12 /= 10; + NavState t1 = T, t2 = t1.retract(d12); + EXPECT(assert_equal(d12, t1.localCoordinates(t2))); +} +/* ************************************************************************* */ +TEST(NavState, expmap_logmap) { + Vector d12 = Vector9::Constant(0.1); + NavState t1 = T, t2 = t1.expmap(d12); + EXPECT(assert_equal(d12, t1.logmap(t2))); +} + +/* ************************************************************************* */ +TEST(NavState, retract_localCoordinates2) { + NavState t1 = T; + NavState t2 = T3; + NavState origin; + Vector d12 = t1.localCoordinates(t2); + EXPECT(assert_equal(t2, t1.retract(d12))); + Vector d21 = t2.localCoordinates(t1); + EXPECT(assert_equal(t1, t2.retract(d21))); + // EXPECT(assert_equal(d12, -d21)); +} +/* ************************************************************************* */ +TEST(NavState, manifold_expmap) { + NavState t1 = T; + NavState t2 = T3; + NavState origin; + Vector d12 = t1.logmap(t2); + EXPECT(assert_equal(t2, t1.expmap(d12))); + Vector d21 = t2.logmap(t1); + EXPECT(assert_equal(t1, t2.expmap(d21))); + + // Check that log(t1,t2)=-log(t2,t1) + EXPECT(assert_equal(d12, -d21)); +} + +/* ************************************************************************* */ +TEST(NavState, subgroups) { + // Frank - Below only works for correct "Agrawal06iros style expmap + // lines in canonical coordinates correspond to Abelian subgroups in SE(3) + Vector d = + (Vector(9) << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9).finished(); + // exp(-d)=inverse(exp(d)) + EXPECT(assert_equal(NavState::Expmap(-d), NavState::Expmap(d).inverse())); + // exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d) + NavState T2 = NavState::Expmap(2 * d); + NavState T3 = NavState::Expmap(3 * d); + NavState T5 = NavState::Expmap(5 * d); + EXPECT(assert_equal(T5, T2 * T3)); + EXPECT(assert_equal(T5, T3 * T2)); +} + +/* ************************************************************************* */ +TEST(NavState, adjointMap) { + Matrix res = NavState::adjointMap(screwNavState::xi); + Matrix wh = skewSymmetric(screwNavState::xi(0), screwNavState::xi(1), + screwNavState::xi(2)); + Matrix vh = skewSymmetric(screwNavState::xi(3), screwNavState::xi(4), + screwNavState::xi(5)); + Matrix rh = skewSymmetric(screwNavState::xi(6), screwNavState::xi(7), + screwNavState::xi(8)); + Matrix9 expected; + expected << wh, Z_3x3, Z_3x3, vh, wh, Z_3x3, rh, Z_3x3, wh; + EXPECT(assert_equal(expected, res, 1e-5)); +} + +/* ************************************************************************* */ +TEST(NavState, ExpmapDerivative1) { + Matrix9 actualH; + Vector9 w; + w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0; + NavState::Expmap(w, actualH); + + std::function f = [](const Vector9& w) { + return NavState::Expmap(w); + }; + Matrix expectedH = + numericalDerivative21 >( + &NavState::Expmap, w, {}); + EXPECT(assert_equal(expectedH, actualH)); +} + +/* ************************************************************************* */ +TEST(NavState, LogmapDerivative) { + Matrix9 actualH; + Vector9 w; + w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0; + NavState p = NavState::Expmap(w); + EXPECT(assert_equal(w, NavState::Logmap(p, actualH), 1e-5)); + + std::function f = [](const NavState& p) { + return NavState::Logmap(p); + }; + Matrix expectedH = + numericalDerivative21 >( + &NavState::Logmap, p, {}); + EXPECT(assert_equal(expectedH, actualH)); +} + +//****************************************************************************** +TEST(NavState, Invariants) { + NavState id; + + EXPECT(check_group_invariants(id, id)); + EXPECT(check_group_invariants(id, T3)); + EXPECT(check_group_invariants(T2, id)); + EXPECT(check_group_invariants(T2, T3)); + + EXPECT(check_manifold_invariants(id, id)); + EXPECT(check_manifold_invariants(id, T3)); + EXPECT(check_manifold_invariants(T2, id)); + EXPECT(check_manifold_invariants(T2, T3)); +} + +//****************************************************************************** +TEST(NavState, LieGroupDerivatives) { + NavState id; + + CHECK_LIE_GROUP_DERIVATIVES(id, id); + CHECK_LIE_GROUP_DERIVATIVES(id, T2); + CHECK_LIE_GROUP_DERIVATIVES(T2, id); + CHECK_LIE_GROUP_DERIVATIVES(T2, T3); +} + +//****************************************************************************** +TEST(NavState, ChartDerivatives) { + NavState id; + if (ROT3_DEFAULT_COORDINATES_MODE == Rot3::EXPMAP) { + CHECK_CHART_DERIVATIVES(id, id); + // CHECK_CHART_DERIVATIVES(id,T2); + // CHECK_CHART_DERIVATIVES(T2,id); + // CHECK_CHART_DERIVATIVES(T2,T3); + } +} /* ************************************************************************* */ int main() { From d0c1854e63999f0b8e394e8ae20ad4af271f22a5 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 14 Dec 2024 01:04:05 -0500 Subject: [PATCH 078/112] ExpmapTranslation --- gtsam/geometry/Pose3.cpp | 120 +++++++++++++++++++++------------------ gtsam/geometry/Pose3.h | 15 +++++ 2 files changed, 80 insertions(+), 55 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 894314491..15eddf7a7 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -26,8 +26,6 @@ namespace gtsam { -using std::vector; - /** instantiate concept checks */ GTSAM_CONCEPT_POSE_INST(Pose3) @@ -167,21 +165,23 @@ Pose3 Pose3::interpolateRt(const Pose3& T, double t) const { /* ************************************************************************* */ /** Modified from Murray94book version (which assumes w and v normalized?) */ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi) { - if (Hxi) *Hxi = ExpmapDerivative(xi); + // Get angular velocity omega and translational velocity v from twist xi + const Vector3 w = xi.head<3>(), v = xi.tail<3>(); - // get angular velocity omega and translational velocity v from twist xi - Vector3 omega(xi(0), xi(1), xi(2)), v(xi(3), xi(4), xi(5)); + // Compute rotation using Expmap + Rot3 R = Rot3::Expmap(w); - Rot3 R = Rot3::Expmap(omega); - double theta2 = omega.dot(omega); - if (theta2 > std::numeric_limits::epsilon()) { - Vector3 t_parallel = omega * omega.dot(v); // translation parallel to axis - Vector3 omega_cross_v = omega.cross(v); // points towards axis - Vector3 t = (omega_cross_v - R * omega_cross_v + t_parallel) / theta2; - return Pose3(R, t); - } else { - return Pose3(R, v); + // Compute translation and optionally its Jacobian in w + Matrix3 Q; + const Vector3 t = ExpmapTranslation(w, v, Hxi ? &Q : nullptr, R); + + if (Hxi) { + const Matrix3 Jw = Rot3::ExpmapDerivative(w); + *Hxi << Jw, Z_3x3, + Q, Jw; } + + return Pose3(R, t); } /* ************************************************************************* */ @@ -240,55 +240,65 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) { } /* ************************************************************************* */ -Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi, double nearZeroThreshold) { +Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi, + double nearZeroThreshold) { + Matrix3 Q; const auto w = xi.head<3>(); const auto v = xi.tail<3>(); - const Matrix3 V = skewSymmetric(v); - const Matrix3 W = skewSymmetric(w); - - Matrix3 Q; - -#ifdef NUMERICAL_EXPMAP_DERIV - Matrix3 Qj = Z_3x3; - double invFac = 1.0; - Q = Z_3x3; - Matrix3 Wj = I_3x3; - for (size_t j=1; j<10; ++j) { - Qj = Qj*W + Wj*V; - invFac = -invFac/(j+1); - Q = Q + invFac*Qj; - Wj = Wj*W; - } -#else - // The closed-form formula in Barfoot14tro eq. (102) - double phi = w.norm(); - const Matrix3 WVW = W * V * W; - if (std::abs(phi) > nearZeroThreshold) { - const double s = sin(phi), c = cos(phi); - const double phi2 = phi * phi, phi3 = phi2 * phi, phi4 = phi3 * phi, - phi5 = phi4 * phi; - // Invert the sign of odd-order terms to have the right Jacobian - Q = -0.5 * V + (phi - s) / phi3 * (W * V + V * W - WVW) + - (1 - phi2 / 2 - c) / phi4 * (W * W * V + V * W * W - 3 * WVW) - - 0.5 * ((1 - phi2 / 2 - c) / phi4 - 3 * (phi - s - phi3 / 6.) / phi5) * - (WVW * W + W * WVW); - } else { - Q = -0.5 * V + 1. / 6. * (W * V + V * W - WVW) - - 1. / 24. * (W * W * V + V * W * W - 3 * WVW) + - 1. / 120. * (WVW * W + W * WVW); - } -#endif - + ExpmapTranslation(w, v, Q, {}, nearZeroThreshold); return Q; } +/* ************************************************************************* */ +Vector3 Pose3::ExpmapTranslation(const Vector3& w, const Vector3& v, + OptionalJacobian<3, 3> Q, + const std::optional& R, + double nearZeroThreshold) { + double phi2 = w.dot(w); + + if (Q) { + const Matrix3 V = skewSymmetric(v); + const Matrix3 W = skewSymmetric(w); + const Matrix3 WVW = W * V * W; + const double phi = w.norm(); + + if (std::abs(phi) > nearZeroThreshold) { + const double s = sin(phi), c = cos(phi); + const double phi3 = phi2 * phi, phi4 = phi3 * phi, phi5 = phi4 * phi; + + // Invert the sign of odd-order terms to have the right Jacobian + *Q = -0.5 * V + (phi - s) / phi3 * (W * V + V * W - WVW) + + (1 - phi2 / 2 - c) / phi4 * (W * W * V + V * W * W - 3 * WVW) - + 0.5 * + ((1 - phi2 / 2 - c) / phi4 - 3 * (phi - s - phi3 / 6.) / phi5) * + (WVW * W + W * WVW); + } else { + constexpr double one_sixth = 1. / 6.; + constexpr double one_twenty_fourth = 1. / 24.; + constexpr double one_one_hundred_twentieth = 1. / 120.; + + *Q = -0.5 * V + one_sixth * (W * V + V * W - WVW) - + one_twenty_fourth * (W * W * V + V * W * W - 3 * WVW) + + one_one_hundred_twentieth * (WVW * W + W * WVW); + } + } + + // TODO(Frank): this threshold is *different*. Why? + if (phi2 > std::numeric_limits::epsilon()) { + Vector3 t_parallel = w * w.dot(v); // translation parallel to axis + Vector3 w_cross_v = w.cross(v); // translation orthogonal to axis + Rot3 rotation = R.value_or(Rot3::Expmap(w)); + Vector3 t = (w_cross_v - rotation * w_cross_v + t_parallel) / phi2; + return t; + } else { + return v; + } +} + /* ************************************************************************* */ Matrix6 Pose3::ExpmapDerivative(const Vector6& xi) { - const Vector3 w = xi.head<3>(); - const Matrix3 Jw = Rot3::ExpmapDerivative(w); - const Matrix3 Q = ComputeQforExpmapDerivative(xi); Matrix6 J; - J << Jw, Z_3x3, Q, Jw; + Expmap(xi, J); return J; } diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 8a807cc23..d7c280c80 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -217,10 +217,25 @@ public: * (see Chirikjian11book2, pg 44, eq 10.95. * The closed-form formula is identical to formula 102 in Barfoot14tro where * Q_l of the SE3 Expmap left derivative matrix is given. + * This is the Jacobian of ExpmapTranslation and computed there. */ static Matrix3 ComputeQforExpmapDerivative( const Vector6& xi, double nearZeroThreshold = 1e-5); + /** + * Compute the translation part of the exponential map, with derivative. + * @param w 3D angular velocity + * @param v 3D velocity + * @param Q Optionally, compute 3x3 Jacobian wrpt w + * @param R Optionally, precomputed as Rot3::Expmap(w) + * @param nearZeroThreshold threshold for small values + * Note Q is 3x3 bottom-left block of SE3 Expmap right derivative matrix + */ + static Vector3 ExpmapTranslation(const Vector3& w, const Vector3& v, + OptionalJacobian<3, 3> Q = {}, + const std::optional& R = {}, + double nearZeroThreshold = 1e-5); + using LieGroup::inverse; // version with derivative /** From 693eafb0747b6fbadeaf9be4c3a5b48ccfbc1dae Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 14 Dec 2024 01:42:35 -0500 Subject: [PATCH 079/112] Use ExpmapTranslation --- gtsam/navigation/NavState.cpp | 108 ++++++++++------------------------ gtsam/navigation/NavState.h | 7 --- 2 files changed, 30 insertions(+), 85 deletions(-) diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index 9f91ae415..f7e0d48be 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -19,6 +19,7 @@ #include #include +#include "gtsam/geometry/Rot3.h" namespace gtsam { @@ -114,28 +115,27 @@ NavState NavState::inverse() const { //------------------------------------------------------------------------------ NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi) { - if (Hxi) *Hxi = ExpmapDerivative(xi); + // Get angular velocity w, translational velocity v, and acceleration a + Vector3 w = xi.head<3>(); + Vector3 rho = xi.segment<3>(3); + Vector3 nu = xi.tail<3>(); - // Order is rotation, position, velocity, represented by ω,ρ,ν - Vector3 omega(xi(0), xi(1), xi(2)), rho(xi(3), xi(4), xi(5)), - nu(xi(6), xi(7), xi(8)); + // Compute rotation using Expmap + Rot3 R = Rot3::Expmap(w); - Rot3 R = Rot3::Expmap(omega); + // Compute translations and optionally their Jacobians + Matrix3 Qt, Qv; + Vector3 t = Pose3::ExpmapTranslation(w, rho, Hxi ? &Qt : nullptr, R); + Vector3 v = Pose3::ExpmapTranslation(w, nu, Hxi ? &Qv : nullptr, R); - double omega_norm = omega.norm(); - - if (omega_norm < 1e-8) { - return NavState(Rot3(), Point3(rho), Point3(nu)); - - } else { - Matrix W = skewSymmetric(omega); - double omega_norm2 = omega_norm * omega_norm; - double omega_norm3 = omega_norm2 * omega_norm; - Matrix A = I_3x3 + ((1 - cos(omega_norm)) / omega_norm2) * W + - ((omega_norm - sin(omega_norm)) / omega_norm3) * (W * W); - - return NavState(Rot3::Expmap(omega), Point3(A * rho), Point3(A * nu)); + if (Hxi) { + const Matrix3 Jw = Rot3::ExpmapDerivative(w); + *Hxi << Jw, Z_3x3, Z_3x3, + Qt, Jw, Z_3x3, + Qv, Z_3x3, Jw; } + + return NavState(R, t, v); } //------------------------------------------------------------------------------ @@ -220,64 +220,10 @@ Vector9 NavState::adjoint(const Vector9& xi, const Vector9& y, return ad_xi * y; } -/* ************************************************************************* */ -Matrix63 NavState::ComputeQforExpmapDerivative(const Vector9& xi, - double nearZeroThreshold) { - const auto omega = xi.head<3>(); - const auto nu = xi.segment<3>(3); - const auto rho = xi.tail<3>(); - const Matrix3 V = skewSymmetric(nu); - const Matrix3 P = skewSymmetric(rho); - const Matrix3 W = skewSymmetric(omega); - - Matrix3 Qv, Qp; - Matrix63 Q; - - // The closed-form formula in Barfoot14tro eq. (102) - double phi = omega.norm(); - if (std::abs(phi) > 1e-5) { - const double sinPhi = sin(phi), cosPhi = cos(phi); - const double phi2 = phi * phi, phi3 = phi2 * phi, phi4 = phi3 * phi, - phi5 = phi4 * phi; - // Invert the sign of odd-order terms to have the right Jacobian - Qv = -0.5 * V + (phi - sinPhi) / phi3 * (W * V + V * W - W * V * W) + - (1 - phi2 / 2 - cosPhi) / phi4 * - (W * W * V + V * W * W - 3 * W * V * W) - - 0.5 * - ((1 - phi2 / 2 - cosPhi) / phi4 - - 3 * (phi - sinPhi - phi3 / 6.) / phi5) * - (W * V * W * W + W * W * V * W); - Qp = -0.5 * P + (phi - sinPhi) / phi3 * (W * P + P * W - W * P * W) + - (1 - phi2 / 2 - cosPhi) / phi4 * - (W * W * P + P * W * W - 3 * W * P * W) - - 0.5 * - ((1 - phi2 / 2 - cosPhi) / phi4 - - 3 * (phi - sinPhi - phi3 / 6.) / phi5) * - (W * P * W * W + W * W * P * W); - } else { - Qv = -0.5 * V + 1. / 6. * (W * V + V * W - W * V * W) + - 1. / 24. * (W * W * V + V * W * W - 3 * W * V * W) - - 0.5 * (1. / 24. + 3. / 120.) * (W * V * W * W + W * W * V * W); - Qp = -0.5 * P + 1. / 6. * (W * P + P * W - W * P * W) + - 1. / 24. * (W * W * P + P * W * W - 3 * W * P * W) - - 0.5 * (1. / 24. + 3. / 120.) * (W * P * W * W + W * W * P * W); - } - - Q << Qv, Qp; - return Q; -} - //------------------------------------------------------------------------------ Matrix9 NavState::ExpmapDerivative(const Vector9& xi) { - const Vector3 w = xi.head<3>(); - const Matrix3 Jw = Rot3::ExpmapDerivative(w); - const Matrix63 Q = ComputeQforExpmapDerivative(xi); - const Matrix3 Qv = Q.topRows<3>(); - const Matrix3 Qp = Q.bottomRows<3>(); - Matrix9 J; - J << Jw, Z_3x3, Z_3x3, Qv, Jw, Z_3x3, Qp, Z_3x3, Jw; - + Expmap(xi, J); return J; } @@ -285,15 +231,21 @@ Matrix9 NavState::ExpmapDerivative(const Vector9& xi) { Matrix9 NavState::LogmapDerivative(const NavState& state) { const Vector9 xi = Logmap(state); const Vector3 w = xi.head<3>(); + Vector3 rho = xi.segment<3>(3); + Vector3 nu = xi.tail<3>(); + + Matrix3 Qt, Qv; + const Rot3 R = Rot3::Expmap(w); + Pose3::ExpmapTranslation(w, rho, Qt, R); + Pose3::ExpmapTranslation(w, nu, Qv, R); const Matrix3 Jw = Rot3::LogmapDerivative(w); - const Matrix63 Q = ComputeQforExpmapDerivative(xi); - const Matrix3 Qv = Q.topRows<3>(); - const Matrix3 Qp = Q.bottomRows<3>(); + const Matrix3 Qt2 = -Jw * Qt * Jw; const Matrix3 Qv2 = -Jw * Qv * Jw; - const Matrix3 Qp2 = -Jw * Qp * Jw; Matrix9 J; - J << Jw, Z_3x3, Z_3x3, Qv2, Jw, Z_3x3, Qp2, Z_3x3, Jw; + J << Jw, Z_3x3, Z_3x3, + Qt2, Jw, Z_3x3, + Qv2, Z_3x3, Jw; return J; } diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h index 2f037a795..95ee71fe9 100644 --- a/gtsam/navigation/NavState.h +++ b/gtsam/navigation/NavState.h @@ -242,13 +242,6 @@ public: static Vector9 Local(const NavState& state, ChartJacobian Hstate = {}); }; - /** - * Compute the 6x3 bottom-left block Qs of the SE_2(3) Expmap derivative - * matrix - */ - static Matrix63 ComputeQforExpmapDerivative(const Vector9& xi, - double nearZeroThreshold = 1e-5); - /// @} /// @name Dynamics /// @{ From 3eba54c05cf703f3a105d11214345afe4f269dbb Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 14 Dec 2024 01:54:29 -0500 Subject: [PATCH 080/112] Remove TODO --- gtsam/navigation/NavState.cpp | 24 ------------------------ 1 file changed, 24 deletions(-) diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index f7e0d48be..6234b9c27 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -294,30 +294,6 @@ NavState NavState::retract(const Vector9& xi, // //------------------------------------------------------------------------------ Vector9 NavState::localCoordinates(const NavState& g, // OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const { - // return LieGroup::localCoordinates(g, H1, H2); - - //TODO(Varun) Fix so that test on L680 passes - - // Matrix3 D_dR_R, D_dt_R, D_dv_R; - // const Rot3 dR = R_.between(g.R_, H1 ? &D_dR_R : 0); - // const Point3 dP = R_.unrotate(g.t_ - t_, H1 ? &D_dt_R : 0); - // const Vector dV = R_.unrotate(g.v_ - v_, H1 ? &D_dv_R : 0); - - // Vector9 xi; - // Matrix3 D_xi_R; - // xi << Rot3::Logmap(dR, (H1 || H2) ? &D_xi_R : 0), dP, dV; - // if (H1) { - // *H1 << D_xi_R * D_dR_R, Z_3x3, Z_3x3, // - // D_dt_R, -I_3x3, Z_3x3, // - // D_dv_R, Z_3x3, -I_3x3; - // } - // if (H2) { - // *H2 << D_xi_R, Z_3x3, Z_3x3, // - // Z_3x3, dR.matrix(), Z_3x3, // - // Z_3x3, Z_3x3, dR.matrix(); - // } - // return xi; - Matrix3 D_dR_R, D_dt_R, D_dv_R; const Rot3 dR = R_.between(g.R_, H1 ? &D_dR_R : 0); const Point3 dP = R_.unrotate(g.t_ - t_, H1 ? &D_dt_R : 0); From dddb6acde08a31d8df02577110c67625f88df384 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 14 Dec 2024 01:55:16 -0500 Subject: [PATCH 081/112] Remove extra include --- gtsam/navigation/NavState.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index 6234b9c27..7c7aa373b 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -19,7 +19,6 @@ #include #include -#include "gtsam/geometry/Rot3.h" namespace gtsam { From 8193bdbec31158c4c18566aa64e96ac5c5d7b5c7 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 15 Dec 2024 12:11:48 -0500 Subject: [PATCH 082/112] Fix nearZero of SE(3) and SE_2(3) --- gtsam/geometry/Pose3.cpp | 44 +++++++++++++++++++++------------------- 1 file changed, 23 insertions(+), 21 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 15eddf7a7..bb1483432 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -21,7 +21,6 @@ #include #include -#include #include namespace gtsam { @@ -254,44 +253,47 @@ Vector3 Pose3::ExpmapTranslation(const Vector3& w, const Vector3& v, OptionalJacobian<3, 3> Q, const std::optional& R, double nearZeroThreshold) { - double phi2 = w.dot(w); + const double theta2 = w.dot(w); + bool nearZero = (theta2 <= nearZeroThreshold); if (Q) { const Matrix3 V = skewSymmetric(v); const Matrix3 W = skewSymmetric(w); const Matrix3 WVW = W * V * W; - const double phi = w.norm(); + const double theta = w.norm(); - if (std::abs(phi) > nearZeroThreshold) { - const double s = sin(phi), c = cos(phi); - const double phi3 = phi2 * phi, phi4 = phi3 * phi, phi5 = phi4 * phi; - - // Invert the sign of odd-order terms to have the right Jacobian - *Q = -0.5 * V + (phi - s) / phi3 * (W * V + V * W - WVW) + - (1 - phi2 / 2 - c) / phi4 * (W * W * V + V * W * W - 3 * WVW) - - 0.5 * - ((1 - phi2 / 2 - c) / phi4 - 3 * (phi - s - phi3 / 6.) / phi5) * - (WVW * W + W * WVW); - } else { - constexpr double one_sixth = 1. / 6.; - constexpr double one_twenty_fourth = 1. / 24.; - constexpr double one_one_hundred_twentieth = 1. / 120.; + if (nearZero) { + static constexpr double one_sixth = 1. / 6.; + static constexpr double one_twenty_fourth = 1. / 24.; + static constexpr double one_one_hundred_twentieth = 1. / 120.; *Q = -0.5 * V + one_sixth * (W * V + V * W - WVW) - one_twenty_fourth * (W * W * V + V * W * W - 3 * WVW) + one_one_hundred_twentieth * (WVW * W + W * WVW); + } else { + const double s = sin(theta), c = cos(theta); + const double theta3 = theta2 * theta, theta4 = theta3 * theta, + theta5 = theta4 * theta; + + // Invert the sign of odd-order terms to have the right Jacobian + *Q = -0.5 * V + (theta - s) / theta3 * (W * V + V * W - WVW) + + (1 - theta2 / 2 - c) / theta4 * (W * W * V + V * W * W - 3 * WVW) - + 0.5 * + ((1 - theta2 / 2 - c) / theta4 - + 3 * (theta - s - theta3 / 6.) / theta5) * + (WVW * W + W * WVW); } } // TODO(Frank): this threshold is *different*. Why? - if (phi2 > std::numeric_limits::epsilon()) { + if (nearZero) { + return v + 0.5 * w.cross(v); + } else { Vector3 t_parallel = w * w.dot(v); // translation parallel to axis Vector3 w_cross_v = w.cross(v); // translation orthogonal to axis Rot3 rotation = R.value_or(Rot3::Expmap(w)); - Vector3 t = (w_cross_v - rotation * w_cross_v + t_parallel) / phi2; + Vector3 t = (w_cross_v - rotation * w_cross_v + t_parallel) / theta2; return t; - } else { - return v; } } From f7a678e54e551b66b8aec8325f5c5085e903d69a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 15 Dec 2024 12:11:56 -0500 Subject: [PATCH 083/112] Address comments --- gtsam/navigation/NavState.cpp | 2 -- gtsam/navigation/tests/testNavState.cpp | 10 +++++----- 2 files changed, 5 insertions(+), 7 deletions(-) diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index 7c7aa373b..b9d48a3cb 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -264,8 +264,6 @@ Vector9 NavState::ChartAtOrigin::Local(const NavState& state, //------------------------------------------------------------------------------ NavState NavState::retract(const Vector9& xi, // OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const { - // return LieGroup::retract(xi, H1, H2); - Rot3 nRb = R_; Point3 n_t = t_, n_v = v_; Matrix3 D_bRc_xi, D_R_nRb, D_t_nRb, D_v_nRb; diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp index 8934129ee..427531415 100644 --- a/gtsam/navigation/tests/testNavState.cpp +++ b/gtsam/navigation/tests/testNavState.cpp @@ -490,7 +490,7 @@ TEST(NavState, Expmap_b) { (Vector(9) << 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0).finished()); NavState expected(Rot3::Rodrigues(0.0, 0.0, 0.1), Point3(-100.0, 0.0, 0.0), Point3(100.0, 0.0, 0.0)); - EXPECT(assert_equal(expected, p2, 1e-2)); + EXPECT(assert_equal(expected, p2)); } /* ************************************************************************* */ @@ -566,7 +566,7 @@ TEST(NavState, retract_localCoordinates2) { EXPECT(assert_equal(t2, t1.retract(d12))); Vector d21 = t2.localCoordinates(t1); EXPECT(assert_equal(t1, t2.retract(d21))); - // EXPECT(assert_equal(d12, -d21)); + // NOTE(FRANK): d12 !== -d21 for arbitrary retractions. } /* ************************************************************************* */ TEST(NavState, manifold_expmap) { @@ -675,9 +675,9 @@ TEST(NavState, ChartDerivatives) { NavState id; if (ROT3_DEFAULT_COORDINATES_MODE == Rot3::EXPMAP) { CHECK_CHART_DERIVATIVES(id, id); - // CHECK_CHART_DERIVATIVES(id,T2); - // CHECK_CHART_DERIVATIVES(T2,id); - // CHECK_CHART_DERIVATIVES(T2,T3); + CHECK_CHART_DERIVATIVES(id,T2); + CHECK_CHART_DERIVATIVES(T2,id); + CHECK_CHART_DERIVATIVES(T2,T3); } } From 440c3ea64bbcd750c692bca7d8cd348db43bb7f4 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 14 Dec 2024 14:28:44 -0500 Subject: [PATCH 084/112] Simplify functor according to Eade --- gtsam/geometry/SO3.cpp | 47 +++++++++++++++++++++++------------------- gtsam/geometry/SO3.h | 27 ++++++++++++++++-------- 2 files changed, 44 insertions(+), 30 deletions(-) diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index 2585c37be..d4c30119e 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -48,12 +48,15 @@ GTSAM_EXPORT Matrix3 compose(const Matrix3& M, const SO3& R, OptionalJacobian<9, } void ExpmapFunctor::init(bool nearZeroApprox) { + WW = W * W; nearZero = nearZeroApprox || (theta2 <= std::numeric_limits::epsilon()); if (!nearZero) { sin_theta = std::sin(theta); const double s2 = std::sin(theta / 2.0); one_minus_cos = 2.0 * s2 * s2; // numerically better than [1 - cos(theta)] + A = sin_theta / theta; + B = one_minus_cos / theta2; } } @@ -62,39 +65,39 @@ ExpmapFunctor::ExpmapFunctor(const Vector3& omega, bool nearZeroApprox) const double wx = omega.x(), wy = omega.y(), wz = omega.z(); W << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0; init(nearZeroApprox); - if (!nearZero) { - K = W / theta; - KK = K * K; - } } ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle, bool nearZeroApprox) : theta2(angle * angle), theta(angle) { const double ax = axis.x(), ay = axis.y(), az = axis.z(); - K << 0.0, -az, +ay, +az, 0.0, -ax, -ay, +ax, 0.0; - W = K * angle; + W << 0.0, -az, +ay, +az, 0.0, -ax, -ay, +ax, 0.0; + W *= angle; init(nearZeroApprox); - if (!nearZero) { - KK = K * K; - } } SO3 ExpmapFunctor::expmap() const { if (nearZero) - return SO3(I_3x3 + W); + return SO3(I_3x3 + W + 0.5 * WW); else - return SO3(I_3x3 + sin_theta * K + one_minus_cos * KK); + return SO3(I_3x3 + A * W + B * WW); +} + +Matrix3 DexpFunctor::leftJacobian() const { + if (nearZero) { + return I_3x3 + 0.5 * W + one_sixth * WW; + } else { + return I_3x3 + B * W + C * WW; + } } DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox) : ExpmapFunctor(omega, nearZeroApprox), omega(omega) { if (nearZero) { - dexp_ = I_3x3 - 0.5 * W; + rightJacobian_ = I_3x3 - 0.5 * W + one_sixth * WW; } else { - a = one_minus_cos / theta; - b = 1.0 - sin_theta / theta; - dexp_ = I_3x3 - a * K + b * KK; + C = (1 - A) / theta2; + rightJacobian_ = I_3x3 - B * W + C * WW; } } @@ -105,21 +108,23 @@ Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1, *H1 = 0.5 * skewSymmetric(v); } else { // TODO(frank): Iserles hints that there should be a form I + c*K + d*KK - const Vector3 Kv = K * v; + double a = B * theta; + double b = C * theta2; + const Vector3 Kv = W * v / theta; const double Da = (sin_theta - 2.0 * a) / theta2; const double Db = (one_minus_cos - 3.0 * b) / theta2; - *H1 = (Db * K - Da * I_3x3) * Kv * omega.transpose() - + *H1 = (Db * W / theta - Da * I_3x3) * Kv * omega.transpose() - skewSymmetric(Kv * b / theta) + - (a * I_3x3 - b * K) * skewSymmetric(v / theta); + (a * I_3x3 - b * W / theta) * skewSymmetric(v / theta); } } - if (H2) *H2 = dexp_; - return dexp_ * v; + if (H2) *H2 = rightJacobian_; + return rightJacobian_ * v; } Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const { - const Matrix3 invDexp = dexp_.inverse(); + const Matrix3 invDexp = rightJacobian_.inverse(); const Vector3 c = invDexp * v; if (H1) { Matrix3 D_dexpv_omega; diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index cd67bfb8c..8d4401c31 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -136,7 +136,8 @@ GTSAM_EXPORT Matrix99 Dcompose(const SO3& R); class GTSAM_EXPORT ExpmapFunctor { protected: const double theta2; - Matrix3 W, K, KK; + Matrix3 W, WW; + double A, B; // Ethan Eade's constants bool nearZero; double theta, sin_theta, one_minus_cos; // only defined if !nearZero @@ -155,13 +156,16 @@ class GTSAM_EXPORT ExpmapFunctor { /// Functor that implements Exponential map *and* its derivatives class DexpFunctor : public ExpmapFunctor { + protected: + static constexpr double one_sixth = 1.0 / 6.0; const Vector3 omega; - double a, b; - Matrix3 dexp_; + double C; // Ethan Eade's C constant + Matrix3 rightJacobian_; public: /// Constructor with element of Lie algebra so(3) - GTSAM_EXPORT explicit DexpFunctor(const Vector3& omega, bool nearZeroApprox = false); + GTSAM_EXPORT explicit DexpFunctor(const Vector3& omega, + bool nearZeroApprox = false); // NOTE(luca): Right Jacobian for Exponential map in SO(3) - equation // (10.86) and following equations in G.S. Chirikjian, "Stochastic Models, @@ -169,16 +173,21 @@ class DexpFunctor : public ExpmapFunctor { // expmap(omega + v) \approx expmap(omega) * expmap(dexp * v) // This maps a perturbation v in the tangent space to // a perturbation on the manifold Expmap(dexp * v) */ - const Matrix3& dexp() const { return dexp_; } + const Matrix3& dexp() const { return rightJacobian_; } /// Multiplies with dexp(), with optional derivatives - GTSAM_EXPORT Vector3 applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = {}, - OptionalJacobian<3, 3> H2 = {}) const; + GTSAM_EXPORT Vector3 applyDexp(const Vector3& v, + OptionalJacobian<3, 3> H1 = {}, + OptionalJacobian<3, 3> H2 = {}) const; /// Multiplies with dexp().inverse(), with optional derivatives GTSAM_EXPORT Vector3 applyInvDexp(const Vector3& v, - OptionalJacobian<3, 3> H1 = {}, - OptionalJacobian<3, 3> H2 = {}) const; + OptionalJacobian<3, 3> H1 = {}, + OptionalJacobian<3, 3> H2 = {}) const; + + // Compute the left Jacobian for Exponential map in SO(3) + // Note precomputed, as not used as much + Matrix3 leftJacobian() const; }; } // namespace so3 From 7f1e101c6b1f934e23cbd3914a40396b6d282efb Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 14 Dec 2024 14:28:56 -0500 Subject: [PATCH 085/112] Use functor as in SO3 --- gtsam/geometry/Pose3.cpp | 73 +++++++++++++++++------------- gtsam/geometry/tests/testPose3.cpp | 23 +++++++++- 2 files changed, 63 insertions(+), 33 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index bb1483432..f867b3088 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -238,14 +238,50 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) { #endif } +/* ************************************************************************* */ +namespace pose3 { +class ExpmapFunctor : public so3::DexpFunctor { + private: + static constexpr double one_twenty_fourth = 1.0 / 24.0; + static constexpr double one_one_hundred_twentieth = 1.0 / 120.0; + + public: + ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false) + : so3::DexpFunctor(omega, nearZeroApprox) {} + + // Compute the Jacobian Q with respect to w + Matrix3 computeQ(const Vector3& v) const { + const Matrix3 V = skewSymmetric(v); + const Matrix3 WVW = W * V * W; + + if (!nearZero) { + const double theta3 = theta2 * theta; + const double theta4 = theta2 * theta2; + const double theta5 = theta4 * theta; + const double s = sin_theta; + const double a = one_minus_cos - theta2 / 2; + + // The closed-form formula in Barfoot14tro eq. (102) + return -0.5 * V + (theta - s) / theta3 * (W * V + V * W - WVW) + + a / theta4 * (WW * V + V * WW - 3 * WVW) - + 0.5 * + (a / theta4 - 3 * (theta - s - theta3 * one_sixth) / theta5) * + (WVW * W + W * WVW); + } else { + return -0.5 * V + one_sixth * (W * V + V * W - WVW) - + one_twenty_fourth * (WW * V + V * WW - 3 * WVW) + + one_one_hundred_twentieth * (WVW * W + W * WVW); + } + } +}; +} // namespace pose3 + /* ************************************************************************* */ Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi, double nearZeroThreshold) { - Matrix3 Q; const auto w = xi.head<3>(); const auto v = xi.tail<3>(); - ExpmapTranslation(w, v, Q, {}, nearZeroThreshold); - return Q; + return pose3::ExpmapFunctor(w).computeQ(v); } /* ************************************************************************* */ @@ -256,39 +292,12 @@ Vector3 Pose3::ExpmapTranslation(const Vector3& w, const Vector3& v, const double theta2 = w.dot(w); bool nearZero = (theta2 <= nearZeroThreshold); - if (Q) { - const Matrix3 V = skewSymmetric(v); - const Matrix3 W = skewSymmetric(w); - const Matrix3 WVW = W * V * W; - const double theta = w.norm(); + if (Q) *Q = pose3::ExpmapFunctor(w, nearZero).computeQ(v); - if (nearZero) { - static constexpr double one_sixth = 1. / 6.; - static constexpr double one_twenty_fourth = 1. / 24.; - static constexpr double one_one_hundred_twentieth = 1. / 120.; - - *Q = -0.5 * V + one_sixth * (W * V + V * W - WVW) - - one_twenty_fourth * (W * W * V + V * W * W - 3 * WVW) + - one_one_hundred_twentieth * (WVW * W + W * WVW); - } else { - const double s = sin(theta), c = cos(theta); - const double theta3 = theta2 * theta, theta4 = theta3 * theta, - theta5 = theta4 * theta; - - // Invert the sign of odd-order terms to have the right Jacobian - *Q = -0.5 * V + (theta - s) / theta3 * (W * V + V * W - WVW) + - (1 - theta2 / 2 - c) / theta4 * (W * W * V + V * W * W - 3 * WVW) - - 0.5 * - ((1 - theta2 / 2 - c) / theta4 - - 3 * (theta - s - theta3 / 6.) / theta5) * - (WVW * W + W * WVW); - } - } - - // TODO(Frank): this threshold is *different*. Why? if (nearZero) { return v + 0.5 * w.cross(v); } else { + // Geometric intuition and faster than using functor. Vector3 t_parallel = w * w.dot(v); // translation parallel to axis Vector3 w_cross_v = w.cross(v); // translation orthogonal to axis Rot3 rotation = R.value_or(Rot3::Expmap(w)); diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index c9851f761..4ff4f9a85 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -845,7 +845,28 @@ TEST( Pose3, ExpmapDerivative1) { } /* ************************************************************************* */ -TEST(Pose3, ExpmapDerivative2) { +TEST( Pose3, ExpmapDerivative2) { + Matrix6 actualH; + Vector6 w; w << 1.0, -2.0, 3.0, -10.0, -20.0, 30.0; + Pose3::Expmap(w,actualH); + Matrix expectedH = numericalDerivative21 >(&Pose3::Expmap, w, {}); + EXPECT(assert_equal(expectedH, actualH)); +} + +/* ************************************************************************* */ +TEST( Pose3, ExpmapDerivative3) { + Matrix6 actualH; + Vector6 w; w << 0.0, 0.0, 0.0, -10.0, -20.0, 30.0; + Pose3::Expmap(w,actualH); + Matrix expectedH = numericalDerivative21 >(&Pose3::Expmap, w, {}); + // Small angle approximation is not as precise as numerical derivative? + EXPECT(assert_equal(expectedH, actualH, 1e-5)); +} + +/* ************************************************************************* */ +TEST(Pose3, ExpmapDerivative4) { // Iserles05an (Lie-group Methods) says: // scalar is easy: d exp(a(t)) / dt = exp(a(t)) a'(t) // matrix is hard: d exp(A(t)) / dt = exp(A(t)) dexp[-A(t)] A'(t) From d1634c033520920496c117548013339e2c5cddbe Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 14 Dec 2024 15:48:55 -0500 Subject: [PATCH 086/112] Simplified Jacobians with ABC --- gtsam/geometry/Pose3.cpp | 37 +++++++++++++++++++------------------ gtsam/geometry/SO3.cpp | 19 ++++++++----------- 2 files changed, 27 insertions(+), 29 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index f867b3088..16ea55665 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -246,33 +246,34 @@ class ExpmapFunctor : public so3::DexpFunctor { static constexpr double one_one_hundred_twentieth = 1.0 / 120.0; public: - ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false) - : so3::DexpFunctor(omega, nearZeroApprox) {} + ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false, + bool includeHigherOrder = false) + : so3::DexpFunctor(omega, nearZeroApprox), + includeHigherOrder(includeHigherOrder) {} - // Compute the Jacobian Q with respect to w Matrix3 computeQ(const Vector3& v) const { const Matrix3 V = skewSymmetric(v); const Matrix3 WVW = W * V * W; if (!nearZero) { - const double theta3 = theta2 * theta; - const double theta4 = theta2 * theta2; - const double theta5 = theta4 * theta; - const double s = sin_theta; - const double a = one_minus_cos - theta2 / 2; - - // The closed-form formula in Barfoot14tro eq. (102) - return -0.5 * V + (theta - s) / theta3 * (W * V + V * W - WVW) + - a / theta4 * (WW * V + V * WW - 3 * WVW) - - 0.5 * - (a / theta4 - 3 * (theta - s - theta3 * one_sixth) / theta5) * - (WVW * W + W * WVW); + // Simplified from closed-form formula in Barfoot14tro eq. (102) + // Note dexp = I_3x3 - B * W + C * WW and t = dexp * v + return -0.5 * V + C * (W * V + V * W - WVW) + + (B - 0.5) / theta2 * (WW * V + V * WW - 3 * WVW) - + 0.5 * (B - 3 * C) / theta2 * (WVW * W + W * WVW); } else { - return -0.5 * V + one_sixth * (W * V + V * W - WVW) - - one_twenty_fourth * (WW * V + V * WW - 3 * WVW) + - one_one_hundred_twentieth * (WVW * W + W * WVW); + Matrix3 Q = -0.5 * V + one_sixth * (W * V + V * W - WVW); + Q -= one_twenty_fourth * (WW * V + V * WW - 3 * WVW); + + if (includeHigherOrder) { + Q += one_one_hundred_twentieth * (WVW * W + W * WVW); + } + return Q; } } + + private: + bool includeHigherOrder; }; } // namespace pose3 diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index d4c30119e..626599abf 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -85,7 +85,7 @@ SO3 ExpmapFunctor::expmap() const { Matrix3 DexpFunctor::leftJacobian() const { if (nearZero) { - return I_3x3 + 0.5 * W + one_sixth * WW; + return I_3x3 + 0.5 * W; // + one_sixth * WW; } else { return I_3x3 + B * W + C * WW; } @@ -94,7 +94,7 @@ Matrix3 DexpFunctor::leftJacobian() const { DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox) : ExpmapFunctor(omega, nearZeroApprox), omega(omega) { if (nearZero) { - rightJacobian_ = I_3x3 - 0.5 * W + one_sixth * WW; + rightJacobian_ = I_3x3 - 0.5 * W; // + one_sixth * WW; } else { C = (1 - A) / theta2; rightJacobian_ = I_3x3 - B * W + C * WW; @@ -104,18 +104,15 @@ DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox) Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const { if (H1) { + const Matrix3 V = skewSymmetric(v); if (nearZero) { - *H1 = 0.5 * skewSymmetric(v); + *H1 = 0.5 * V; } else { // TODO(frank): Iserles hints that there should be a form I + c*K + d*KK - double a = B * theta; - double b = C * theta2; - const Vector3 Kv = W * v / theta; - const double Da = (sin_theta - 2.0 * a) / theta2; - const double Db = (one_minus_cos - 3.0 * b) / theta2; - *H1 = (Db * W / theta - Da * I_3x3) * Kv * omega.transpose() - - skewSymmetric(Kv * b / theta) + - (a * I_3x3 - b * W / theta) * skewSymmetric(v / theta); + const double Da = (A - 2.0 * B) / theta2; + const double Db = (B - 3.0 * C) / theta2; + *H1 = (Db * WW - Da * W) * v * omega.transpose() - + C * skewSymmetric(W * v) + B * V - C * W * V; } } if (H2) *H2 = rightJacobian_; From 8040a0a31e188d99583a747de4be2dd6887a37f4 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 14 Dec 2024 17:22:09 -0500 Subject: [PATCH 087/112] doubleCross, tested --- gtsam/geometry/Point3.cpp | 10 ++++++++++ gtsam/geometry/Point3.h | 7 ++++++- gtsam/geometry/tests/testPoint3.cpp | 13 +++++++++++++ 3 files changed, 29 insertions(+), 1 deletion(-) diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index ef91108eb..79fd6b9bf 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -69,6 +69,16 @@ Point3 cross(const Point3 &p, const Point3 &q, OptionalJacobian<3, 3> H1, p.x() * q.y() - p.y() * q.x()); } +Point3 doubleCross(const Point3 &p, const Point3 &q, // + OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) { + if (H1) *H1 = q.dot(p) * I_3x3 + p * q.transpose() - 2 * q * p.transpose(); + if (H2) { + const Matrix3 W = skewSymmetric(p); + *H2 = W * W; + } + return gtsam::cross(p, gtsam::cross(p, q)); +} + double dot(const Point3 &p, const Point3 &q, OptionalJacobian<1, 3> H1, OptionalJacobian<1, 3> H2) { if (H1) *H1 << q.x(), q.y(), q.z(); diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 8da83817d..0fa53b1b2 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -55,11 +55,16 @@ GTSAM_EXPORT double norm3(const Point3& p, OptionalJacobian<1, 3> H = {}); /// normalize, with optional Jacobian GTSAM_EXPORT Point3 normalize(const Point3& p, OptionalJacobian<3, 3> H = {}); -/// cross product @return this x q +/// cross product @return p x q GTSAM_EXPORT Point3 cross(const Point3& p, const Point3& q, OptionalJacobian<3, 3> H_p = {}, OptionalJacobian<3, 3> H_q = {}); +/// double cross product @return p x (p x q) +Point3 doubleCross(const Point3& p, const Point3& q, + OptionalJacobian<3, 3> H1 = {}, + OptionalJacobian<3, 3> H2 = {}); + /// dot product GTSAM_EXPORT double dot(const Point3& p, const Point3& q, OptionalJacobian<1, 3> H_p = {}, diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index dcfb99105..bb49486a8 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -18,6 +18,8 @@ #include #include #include +#include "gtsam/base/Matrix.h" +#include "gtsam/base/OptionalJacobian.h" using namespace std::placeholders; using namespace gtsam; @@ -154,6 +156,17 @@ TEST( Point3, cross2) { } } +/* ************************************************************************* */ +TEST(Point3, doubleCross) { + Matrix aH1, aH2; + std::function f = + [](const Point3& p, const Point3& q) { return doubleCross(p, q); }; + const Point3 omega(1, 2, 3), theta(4, 5, 6); + doubleCross(omega, theta, aH1, aH2); + EXPECT(assert_equal(numericalDerivative21(f, omega, theta), aH1)); + EXPECT(assert_equal(numericalDerivative22(f, omega, theta), aH2)); +} + //************************************************************************* TEST (Point3, normalize) { Matrix actualH; From 78f17aabc466b81554b513ff22d7d3df42b643ed Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 14 Dec 2024 23:53:51 -0500 Subject: [PATCH 088/112] Simplified applyDexp --- gtsam/geometry/SO3.cpp | 82 ++++++++++++++++++++++++++++-------------- gtsam/geometry/SO3.h | 17 +++++---- 2 files changed, 66 insertions(+), 33 deletions(-) diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index 626599abf..bc2e1e6ec 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -18,13 +18,14 @@ * @date December 2014 */ +#include +#include #include +#include #include #include - #include -#include #include namespace gtsam { @@ -41,7 +42,8 @@ GTSAM_EXPORT Matrix99 Dcompose(const SO3& Q) { return H; } -GTSAM_EXPORT Matrix3 compose(const Matrix3& M, const SO3& R, OptionalJacobian<9, 9> H) { +GTSAM_EXPORT Matrix3 compose(const Matrix3& M, const SO3& R, + OptionalJacobian<9, 9> H) { Matrix3 MR = M * R.matrix(); if (H) *H = Dcompose(R); return MR; @@ -83,45 +85,54 @@ SO3 ExpmapFunctor::expmap() const { return SO3(I_3x3 + A * W + B * WW); } -Matrix3 DexpFunctor::leftJacobian() const { - if (nearZero) { - return I_3x3 + 0.5 * W; // + one_sixth * WW; - } else { - return I_3x3 + B * W + C * WW; - } -} - DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox) : ExpmapFunctor(omega, nearZeroApprox), omega(omega) { + C = (1 - A) / theta2; +} + +Matrix3 DexpFunctor::rightJacobian() const { if (nearZero) { - rightJacobian_ = I_3x3 - 0.5 * W; // + one_sixth * WW; + return I_3x3 - 0.5 * W; // + one_sixth * WW; } else { - C = (1 - A) / theta2; - rightJacobian_ = I_3x3 - B * W + C * WW; + return I_3x3 - B * W + C * WW; } } +// Derivative of w x w x v in w: +static Matrix3 doubleCrossJacobian(const Vector3& w, const Vector3& v) { + return v.dot(w) * I_3x3 + w * v.transpose() - 2 * v * w.transpose(); +} + +// Multiplies v with left Jacobian through vector operations only. +// When Jacobian H1 wrt omega is requested, product rule is invoked twice, once +// for (B * Wv) and (C * WWv). The Jacobian H2 wrt v is just the right Jacobian. Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const { - if (H1) { - const Matrix3 V = skewSymmetric(v); - if (nearZero) { - *H1 = 0.5 * V; - } else { - // TODO(frank): Iserles hints that there should be a form I + c*K + d*KK - const double Da = (A - 2.0 * B) / theta2; - const double Db = (B - 3.0 * C) / theta2; - *H1 = (Db * WW - Da * W) * v * omega.transpose() - - C * skewSymmetric(W * v) + B * V - C * W * V; + // Wv = omega x * v, with Jacobian -V in w + const Vector3 Wv = gtsam::cross(omega, v); + + if (nearZero) { + if (H1) *H1 = 0.5 * skewSymmetric(v); + if (H2) *H2 = I_3x3 - 0.5 * W; + return v - 0.5 * Wv; + } else { + // WWv = omega x (omega x * v), with Jacobian doubleCrossJacobian + const Vector3 WWv = gtsam::cross(omega, Wv); + if (H1) { + // 1x3 Jacobians of B and C with respect to theta + const Matrix13 HB = (A - 2.0 * B) / theta2 * omega.transpose(); + const Matrix13 HC = (B - 3.0 * C) / theta2 * omega.transpose(); + *H1 = -Wv * HB + B * skewSymmetric(v) + + C * doubleCrossJacobian(omega, v) + WWv * HC; } + if (H2) *H2 = rightJacobian(); + return v - B * Wv + C * WWv; } - if (H2) *H2 = rightJacobian_; - return rightJacobian_ * v; } Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const { - const Matrix3 invDexp = rightJacobian_.inverse(); + const Matrix3 invDexp = rightJacobian().inverse(); const Vector3 c = invDexp * v; if (H1) { Matrix3 D_dexpv_omega; @@ -132,6 +143,23 @@ Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1, return c; } +Matrix3 DexpFunctor::leftJacobian() const { + if (nearZero) { + return I_3x3 + 0.5 * W; // + one_sixth * WW; + } else { + return I_3x3 + B * W + C * WW; + } +} + +Vector3 DexpFunctor::applyLeftJacobian(const Vector3& v, + OptionalJacobian<3, 3> H1, + OptionalJacobian<3, 3> H2) const { + const Matrix3 Jw = leftJacobian(); + if (H1) H1->setZero(); + if (H2) *H2 = Jw; + return Jw * v; +} + } // namespace so3 //****************************************************************************** diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index 8d4401c31..bd54f0cc4 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -26,7 +26,6 @@ #include #include -#include #include namespace gtsam { @@ -160,7 +159,6 @@ class DexpFunctor : public ExpmapFunctor { static constexpr double one_sixth = 1.0 / 6.0; const Vector3 omega; double C; // Ethan Eade's C constant - Matrix3 rightJacobian_; public: /// Constructor with element of Lie algebra so(3) @@ -172,8 +170,11 @@ class DexpFunctor : public ExpmapFunctor { // Information Theory, and Lie Groups", Volume 2, 2008. // expmap(omega + v) \approx expmap(omega) * expmap(dexp * v) // This maps a perturbation v in the tangent space to - // a perturbation on the manifold Expmap(dexp * v) */ - const Matrix3& dexp() const { return rightJacobian_; } + // a perturbation on the manifold Expmap(dexp * v) + GTSAM_EXPORT Matrix3 rightJacobian() const; + + /// differential of expmap == right Jacobian + GTSAM_EXPORT Matrix3 dexp() const { return rightJacobian(); } /// Multiplies with dexp(), with optional derivatives GTSAM_EXPORT Vector3 applyDexp(const Vector3& v, @@ -186,8 +187,12 @@ class DexpFunctor : public ExpmapFunctor { OptionalJacobian<3, 3> H2 = {}) const; // Compute the left Jacobian for Exponential map in SO(3) - // Note precomputed, as not used as much - Matrix3 leftJacobian() const; + GTSAM_EXPORT Matrix3 leftJacobian() const; + + /// Multiplies with leftJacobian(), with optional derivatives + GTSAM_EXPORT Vector3 applyLeftJacobian(const Vector3& v, + OptionalJacobian<3, 3> H1 = {}, + OptionalJacobian<3, 3> H2 = {}) const; }; } // namespace so3 From c7f651d98db1f0a7d3bea7b1a047cd1e4f93c7e1 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 15 Dec 2024 00:50:38 -0500 Subject: [PATCH 089/112] applyLeftJacobian --- gtsam/geometry/SO3.cpp | 67 +++++++++++++++++++++----------- gtsam/geometry/SO3.h | 6 +++ gtsam/geometry/tests/testSO3.cpp | 24 ++++++++++++ 3 files changed, 74 insertions(+), 23 deletions(-) diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index bc2e1e6ec..13600f884 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -26,6 +26,7 @@ #include #include +#include #include namespace gtsam { @@ -98,35 +99,47 @@ Matrix3 DexpFunctor::rightJacobian() const { } } -// Derivative of w x w x v in w: -static Matrix3 doubleCrossJacobian(const Vector3& w, const Vector3& v) { - return v.dot(w) * I_3x3 + w * v.transpose() - 2 * v * w.transpose(); +Vector3 DexpFunctor::cross(const Vector3& v, OptionalJacobian<3, 3> H) const { + // Wv = omega x * v + const Vector3 Wv = gtsam::cross(omega, v); + if (H) { + // 1x3 Jacobian of B with respect to omega + const Matrix13 HB = (A - 2.0 * B) / theta2 * omega.transpose(); + // Apply product rule: + *H = Wv * HB - B * skewSymmetric(v); + } + return B * Wv; +} + +Vector3 DexpFunctor::doubleCross(const Vector3& v, + OptionalJacobian<3, 3> H) const { + // WWv = omega x (omega x * v) + Matrix3 doubleCrossJacobian; + const Vector3 WWv = + gtsam::doubleCross(omega, v, H ? &doubleCrossJacobian : nullptr); + if (H) { + // 1x3 Jacobian of C with respect to omega + const Matrix13 HC = (B - 3.0 * C) / theta2 * omega.transpose(); + // Apply product rule: + *H = WWv * HC + C * doubleCrossJacobian; + } + return C * WWv; } // Multiplies v with left Jacobian through vector operations only. -// When Jacobian H1 wrt omega is requested, product rule is invoked twice, once -// for (B * Wv) and (C * WWv). The Jacobian H2 wrt v is just the right Jacobian. Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const { - // Wv = omega x * v, with Jacobian -V in w - const Vector3 Wv = gtsam::cross(omega, v); - if (nearZero) { if (H1) *H1 = 0.5 * skewSymmetric(v); if (H2) *H2 = I_3x3 - 0.5 * W; - return v - 0.5 * Wv; + return v - 0.5 * gtsam::cross(omega, v); } else { - // WWv = omega x (omega x * v), with Jacobian doubleCrossJacobian - const Vector3 WWv = gtsam::cross(omega, Wv); - if (H1) { - // 1x3 Jacobians of B and C with respect to theta - const Matrix13 HB = (A - 2.0 * B) / theta2 * omega.transpose(); - const Matrix13 HC = (B - 3.0 * C) / theta2 * omega.transpose(); - *H1 = -Wv * HB + B * skewSymmetric(v) + - C * doubleCrossJacobian(omega, v) + WWv * HC; - } + Matrix3 D_BWv_omega, D_CWWv_omega; + const Vector3 BWv = cross(v, D_BWv_omega); + const Vector3 CWWv = doubleCross(v, D_CWWv_omega); + if (H1) *H1 = - D_BWv_omega + D_CWWv_omega; if (H2) *H2 = rightJacobian(); - return v - B * Wv + C * WWv; + return v - BWv + CWWv; } } @@ -154,10 +167,18 @@ Matrix3 DexpFunctor::leftJacobian() const { Vector3 DexpFunctor::applyLeftJacobian(const Vector3& v, OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const { - const Matrix3 Jw = leftJacobian(); - if (H1) H1->setZero(); - if (H2) *H2 = Jw; - return Jw * v; + if (nearZero) { + if (H1) *H1 = - 0.5 * skewSymmetric(v); + if (H2) *H2 = I_3x3 + 0.5 * W; + return v + 0.5 * gtsam::cross(omega, v); + } else { + Matrix3 D_BWv_omega, D_CWWv_omega; + const Vector3 BWv = cross(v, D_BWv_omega); + const Vector3 CWWv = doubleCross(v, D_CWWv_omega); + if (H1) *H1 = D_BWv_omega + D_CWWv_omega; + if (H2) *H2 = leftJacobian(); + return v + BWv + CWWv; + } } } // namespace so3 diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index bd54f0cc4..8766fecbe 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -160,6 +160,12 @@ class DexpFunctor : public ExpmapFunctor { const Vector3 omega; double C; // Ethan Eade's C constant + /// Computes B * (omega x v). + Vector3 cross(const Vector3& v, OptionalJacobian<3, 3> H = {}) const; + + /// Computes C * (omega x (omega x v)). + Vector3 doubleCross(const Vector3& v, OptionalJacobian<3, 3> H = {}) const; + public: /// Constructor with element of Lie algebra so(3) GTSAM_EXPORT explicit DexpFunctor(const Vector3& omega, diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index 27c143d31..8773e287a 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -19,6 +19,7 @@ #include #include #include +#include using namespace std::placeholders; using namespace std; @@ -326,6 +327,29 @@ TEST(SO3, ApplyDexp) { } } +//****************************************************************************** +TEST(SO3, ApplyLeftJacobian) { + Matrix aH1, aH2; + for (bool nearZeroApprox : {false, true}) { + std::function f = + [=](const Vector3& omega, const Vector3& v) { + return so3::DexpFunctor(omega, nearZeroApprox).applyLeftJacobian(v); + }; + for (Vector3 omega : {Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 1, 0), + Vector3(0, 0, 1), Vector3(0.1, 0.2, 0.3)}) { + so3::DexpFunctor local(omega, nearZeroApprox); + for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1), + Vector3(0.4, 0.3, 0.2)}) { + CHECK(assert_equal(Vector3(local.leftJacobian() * v), + local.applyLeftJacobian(v, aH1, aH2))); + CHECK(assert_equal(numericalDerivative21(f, omega, v), aH1)); + CHECK(assert_equal(numericalDerivative22(f, omega, v), aH2)); + CHECK(assert_equal(local.leftJacobian(), aH2)); + } + } + } +} + //****************************************************************************** TEST(SO3, ApplyInvDexp) { Matrix aH1, aH2; From 82e13806035327385d66b13bbbe36bb1d91b87dd Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 15 Dec 2024 01:53:02 -0500 Subject: [PATCH 090/112] Cleanup --- gtsam/geometry/Pose3.cpp | 30 ++++++++++++----------------- gtsam/geometry/tests/testPoint3.cpp | 2 -- gtsam/geometry/tests/testSO3.cpp | 1 - 3 files changed, 12 insertions(+), 21 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 16ea55665..d87e3164c 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -168,14 +168,14 @@ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi) { const Vector3 w = xi.head<3>(), v = xi.tail<3>(); // Compute rotation using Expmap - Rot3 R = Rot3::Expmap(w); + Matrix3 Jw; + Rot3 R = Rot3::Expmap(w, Hxi ? &Jw : nullptr); // Compute translation and optionally its Jacobian in w Matrix3 Q; const Vector3 t = ExpmapTranslation(w, v, Hxi ? &Q : nullptr, R); if (Hxi) { - const Matrix3 Jw = Rot3::ExpmapDerivative(w); *Hxi << Jw, Z_3x3, Q, Jw; } @@ -246,34 +246,27 @@ class ExpmapFunctor : public so3::DexpFunctor { static constexpr double one_one_hundred_twentieth = 1.0 / 120.0; public: - ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false, - bool includeHigherOrder = false) - : so3::DexpFunctor(omega, nearZeroApprox), - includeHigherOrder(includeHigherOrder) {} + ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false) + : so3::DexpFunctor(omega, nearZeroApprox) {} + // Compute the bottom-left 3x3 block of the SE(3) Expmap derivative + // TODO(Frank): t = applyLeftJacobian(v), it would be nice to understand + // how to compute mess below from its derivatives in w and v. Matrix3 computeQ(const Vector3& v) const { const Matrix3 V = skewSymmetric(v); const Matrix3 WVW = W * V * W; if (!nearZero) { // Simplified from closed-form formula in Barfoot14tro eq. (102) - // Note dexp = I_3x3 - B * W + C * WW and t = dexp * v return -0.5 * V + C * (W * V + V * W - WVW) + (B - 0.5) / theta2 * (WW * V + V * WW - 3 * WVW) - 0.5 * (B - 3 * C) / theta2 * (WVW * W + W * WVW); } else { - Matrix3 Q = -0.5 * V + one_sixth * (W * V + V * W - WVW); - Q -= one_twenty_fourth * (WW * V + V * WW - 3 * WVW); - - if (includeHigherOrder) { - Q += one_one_hundred_twentieth * (WVW * W + W * WVW); - } - return Q; + return -0.5 * V + one_sixth * (W * V + V * W - WVW) - + one_twenty_fourth * (WW * V + V * WW - 3 * WVW) + + one_one_hundred_twentieth * (WVW * W + W * WVW); } } - - private: - bool includeHigherOrder; }; } // namespace pose3 @@ -298,7 +291,8 @@ Vector3 Pose3::ExpmapTranslation(const Vector3& w, const Vector3& v, if (nearZero) { return v + 0.5 * w.cross(v); } else { - // Geometric intuition and faster than using functor. + // NOTE(Frank): t can also be computed by calling applyLeftJacobian(v), but + // formulas below convey geometric insight and creating functor is not free. Vector3 t_parallel = w * w.dot(v); // translation parallel to axis Vector3 w_cross_v = w.cross(v); // translation orthogonal to axis Rot3 rotation = R.value_or(Rot3::Expmap(w)); diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index bb49486a8..b3b751ea1 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -18,8 +18,6 @@ #include #include #include -#include "gtsam/base/Matrix.h" -#include "gtsam/base/OptionalJacobian.h" using namespace std::placeholders; using namespace gtsam; diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index 8773e287a..1e105ceca 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -19,7 +19,6 @@ #include #include #include -#include using namespace std::placeholders; using namespace std; From fa1249bf14dc815402061bc826b7adbfc2db620f Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 15 Dec 2024 17:53:42 -0500 Subject: [PATCH 091/112] Add export --- gtsam/geometry/Point3.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 0fa53b1b2..d5c32816e 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -61,9 +61,9 @@ GTSAM_EXPORT Point3 cross(const Point3& p, const Point3& q, OptionalJacobian<3, 3> H_q = {}); /// double cross product @return p x (p x q) -Point3 doubleCross(const Point3& p, const Point3& q, - OptionalJacobian<3, 3> H1 = {}, - OptionalJacobian<3, 3> H2 = {}); +GTSAM_EXPORT Point3 doubleCross(const Point3& p, const Point3& q, + OptionalJacobian<3, 3> H1 = {}, + OptionalJacobian<3, 3> H2 = {}); /// dot product GTSAM_EXPORT double dot(const Point3& p, const Point3& q, From e96d8487e493d9cce4f60a09ab7ec20c2412210e Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 15 Dec 2024 17:54:30 -0500 Subject: [PATCH 092/112] New constants for cross and doubleCross --- gtsam/geometry/SO3.cpp | 65 ++++++++++++------------ gtsam/geometry/SO3.h | 22 ++++++--- gtsam/geometry/tests/testSO3.cpp | 84 +++++++++++++++++++++++--------- 3 files changed, 108 insertions(+), 63 deletions(-) diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index 13600f884..e341911b9 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -51,49 +51,51 @@ GTSAM_EXPORT Matrix3 compose(const Matrix3& M, const SO3& R, } void ExpmapFunctor::init(bool nearZeroApprox) { - WW = W * W; nearZero = nearZeroApprox || (theta2 <= std::numeric_limits::epsilon()); if (!nearZero) { - sin_theta = std::sin(theta); - const double s2 = std::sin(theta / 2.0); - one_minus_cos = 2.0 * s2 * s2; // numerically better than [1 - cos(theta)] + const double sin_theta = std::sin(theta); A = sin_theta / theta; + const double s2 = std::sin(theta / 2.0); + const double one_minus_cos = + 2.0 * s2 * s2; // numerically better than [1 - cos(theta)] B = one_minus_cos / theta2; + } else { + // Limits as theta -> 0: + A = 1.0; + B = 0.5; } } ExpmapFunctor::ExpmapFunctor(const Vector3& omega, bool nearZeroApprox) - : theta2(omega.dot(omega)), theta(std::sqrt(theta2)) { - const double wx = omega.x(), wy = omega.y(), wz = omega.z(); - W << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0; + : theta2(omega.dot(omega)), + theta(std::sqrt(theta2)), + W(skewSymmetric(omega)), + WW(W * W) { init(nearZeroApprox); } ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle, bool nearZeroApprox) - : theta2(angle * angle), theta(angle) { - const double ax = axis.x(), ay = axis.y(), az = axis.z(); - W << 0.0, -az, +ay, +az, 0.0, -ax, -ay, +ax, 0.0; - W *= angle; + : theta2(angle * angle), + theta(angle), + W(skewSymmetric(axis * angle)), + WW(W * W) { init(nearZeroApprox); } -SO3 ExpmapFunctor::expmap() const { - if (nearZero) - return SO3(I_3x3 + W + 0.5 * WW); - else - return SO3(I_3x3 + A * W + B * WW); -} +SO3 ExpmapFunctor::expmap() const { return SO3(I_3x3 + A * W + B * WW); } DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox) : ExpmapFunctor(omega, nearZeroApprox), omega(omega) { - C = (1 - A) / theta2; + C = nearZero ? one_sixth : (1 - A) / theta2; + D = nearZero ? _one_twelfth : (A - 2.0 * B) / theta2; + E = nearZero ? _one_sixtieth : (B - 3.0 * C) / theta2; } Matrix3 DexpFunctor::rightJacobian() const { if (nearZero) { - return I_3x3 - 0.5 * W; // + one_sixth * WW; + return I_3x3 - B * W; // + C * WW; } else { return I_3x3 - B * W + C * WW; } @@ -103,10 +105,10 @@ Vector3 DexpFunctor::cross(const Vector3& v, OptionalJacobian<3, 3> H) const { // Wv = omega x * v const Vector3 Wv = gtsam::cross(omega, v); if (H) { - // 1x3 Jacobian of B with respect to omega - const Matrix13 HB = (A - 2.0 * B) / theta2 * omega.transpose(); // Apply product rule: - *H = Wv * HB - B * skewSymmetric(v); + // D * omega.transpose() is 1x3 Jacobian of B with respect to omega + // - skewSymmetric(v) is 3x3 Jacobian of B gtsam::cross(omega, v) + *H = Wv * D * omega.transpose() - B * skewSymmetric(v); } return B * Wv; } @@ -118,10 +120,10 @@ Vector3 DexpFunctor::doubleCross(const Vector3& v, const Vector3 WWv = gtsam::doubleCross(omega, v, H ? &doubleCrossJacobian : nullptr); if (H) { - // 1x3 Jacobian of C with respect to omega - const Matrix13 HC = (B - 3.0 * C) / theta2 * omega.transpose(); // Apply product rule: - *H = WWv * HC + C * doubleCrossJacobian; + // E * omega.transpose() is 1x3 Jacobian of C with respect to omega + // doubleCrossJacobian is 3x3 Jacobian of C gtsam::doubleCross(omega, v) + *H = WWv * E * omega.transpose() + C * doubleCrossJacobian; } return C * WWv; } @@ -134,12 +136,12 @@ Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1, if (H2) *H2 = I_3x3 - 0.5 * W; return v - 0.5 * gtsam::cross(omega, v); } else { - Matrix3 D_BWv_omega, D_CWWv_omega; + Matrix3 D_BWv_omega, D_CWWv_omega; const Vector3 BWv = cross(v, D_BWv_omega); const Vector3 CWWv = doubleCross(v, D_CWWv_omega); if (H1) *H1 = - D_BWv_omega + D_CWWv_omega; - if (H2) *H2 = rightJacobian(); - return v - BWv + CWWv; + if (H2) *H2 = rightJacobian(); + return v - BWv + CWWv; } } @@ -219,12 +221,7 @@ SO3 SO3::ChordalMean(const std::vector& rotations) { template <> GTSAM_EXPORT Matrix3 SO3::Hat(const Vector3& xi) { - // skew symmetric matrix X = xi^ - Matrix3 Y = Z_3x3; - Y(0, 1) = -xi(2); - Y(0, 2) = +xi(1); - Y(1, 2) = -xi(0); - return Y - Y.transpose(); + return skewSymmetric(xi); } //****************************************************************************** diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index 8766fecbe..6403c3ae0 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -134,11 +134,12 @@ GTSAM_EXPORT Matrix99 Dcompose(const SO3& R); /// Functor implementing Exponential map class GTSAM_EXPORT ExpmapFunctor { protected: - const double theta2; - Matrix3 W, WW; - double A, B; // Ethan Eade's constants + const double theta2, theta; + const Matrix3 W, WW; bool nearZero; - double theta, sin_theta, one_minus_cos; // only defined if !nearZero + // Ethan Eade's constants: + double A; // A = sin(theta) / theta or 1 for theta->0 + double B; // B = (1 - cos(theta)) / theta^2 or 0.5 for theta->0 void init(bool nearZeroApprox = false); @@ -156,17 +157,19 @@ class GTSAM_EXPORT ExpmapFunctor { /// Functor that implements Exponential map *and* its derivatives class DexpFunctor : public ExpmapFunctor { protected: - static constexpr double one_sixth = 1.0 / 6.0; const Vector3 omega; - double C; // Ethan Eade's C constant + double C; // Ethan's C constant: (1 - A) / theta^2 or 1/6 for theta->0 + // Constants used in cross and doubleCross + double D; // (A - 2.0 * B) / theta2 or -1/12 for theta->0 + double E; // (B - 3.0 * C) / theta2 or -1/60 for theta->0 + public: /// Computes B * (omega x v). Vector3 cross(const Vector3& v, OptionalJacobian<3, 3> H = {}) const; /// Computes C * (omega x (omega x v)). Vector3 doubleCross(const Vector3& v, OptionalJacobian<3, 3> H = {}) const; - public: /// Constructor with element of Lie algebra so(3) GTSAM_EXPORT explicit DexpFunctor(const Vector3& omega, bool nearZeroApprox = false); @@ -199,6 +202,11 @@ class DexpFunctor : public ExpmapFunctor { GTSAM_EXPORT Vector3 applyLeftJacobian(const Vector3& v, OptionalJacobian<3, 3> H1 = {}, OptionalJacobian<3, 3> H2 = {}) const; + + protected: + static constexpr double one_sixth = 1.0 / 6.0; + static constexpr double _one_twelfth = -1.0 / 12.0; + static constexpr double _one_sixtieth = -1.0 / 60.0; }; } // namespace so3 diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index 1e105ceca..e8de2e93d 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -303,19 +303,63 @@ TEST(SO3, JacobianLogmap) { EXPECT(assert_equal(Jexpected, Jactual)); } +namespace test_cases { +std::vector nearZeros{ + {0, 0, 0}, {1e-5, 0, 0}, {0, 1e-5, 0}, {0, 0, 1e-5}}; +std::vector omegas{ + {0, 0, 0}, {1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0.1, 0.2, 0.3}}; +std::vector vs{{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0.4, 0.3, 0.2}}; +} // namespace test_cases + +//****************************************************************************** +TEST(SO3, Cross) { + Matrix aH1; + for (bool nearZero : {true, false}) { + std::function f = + [=](const Vector3& omega, const Vector3& v) { + return so3::DexpFunctor(omega, nearZero).cross(v); + }; + const auto& omegas = nearZero ? test_cases::nearZeros : test_cases::omegas; + for (Vector3 omega : omegas) { + so3::DexpFunctor local(omega, nearZero); + for (Vector3 v : test_cases::vs) { + local.cross(v, aH1); + EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); + } + } + } +} + +//****************************************************************************** +TEST(SO3, DoubleCross) { + Matrix aH1; + for (bool nearZero : {true, false}) { + std::function f = + [=](const Vector3& omega, const Vector3& v) { + return so3::DexpFunctor(omega, nearZero).doubleCross(v); + }; + const auto& omegas = nearZero ? test_cases::nearZeros : test_cases::omegas; + for (Vector3 omega : omegas) { + so3::DexpFunctor local(omega, nearZero); + for (Vector3 v : test_cases::vs) { + local.doubleCross(v, aH1); + EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); + } + } + } +} + //****************************************************************************** TEST(SO3, ApplyDexp) { Matrix aH1, aH2; - for (bool nearZeroApprox : {true, false}) { + for (bool nearZero : {true, false}) { std::function f = [=](const Vector3& omega, const Vector3& v) { - return so3::DexpFunctor(omega, nearZeroApprox).applyDexp(v); + return so3::DexpFunctor(omega, nearZero).applyDexp(v); }; - for (Vector3 omega : {Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 1, 0), - Vector3(0, 0, 1), Vector3(0.1, 0.2, 0.3)}) { - so3::DexpFunctor local(omega, nearZeroApprox); - for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1), - Vector3(0.4, 0.3, 0.2)}) { + for (Vector3 omega : test_cases::omegas) { + so3::DexpFunctor local(omega, nearZero); + for (Vector3 v : test_cases::vs) { EXPECT(assert_equal(Vector3(local.dexp() * v), local.applyDexp(v, aH1, aH2))); EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); @@ -329,18 +373,16 @@ TEST(SO3, ApplyDexp) { //****************************************************************************** TEST(SO3, ApplyLeftJacobian) { Matrix aH1, aH2; - for (bool nearZeroApprox : {false, true}) { + for (bool nearZero : {true, false}) { std::function f = [=](const Vector3& omega, const Vector3& v) { - return so3::DexpFunctor(omega, nearZeroApprox).applyLeftJacobian(v); + return so3::DexpFunctor(omega, nearZero).applyLeftJacobian(v); }; - for (Vector3 omega : {Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 1, 0), - Vector3(0, 0, 1), Vector3(0.1, 0.2, 0.3)}) { - so3::DexpFunctor local(omega, nearZeroApprox); - for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1), - Vector3(0.4, 0.3, 0.2)}) { + for (Vector3 omega : test_cases::omegas) { + so3::DexpFunctor local(omega, nearZero); + for (Vector3 v : test_cases::vs) { CHECK(assert_equal(Vector3(local.leftJacobian() * v), - local.applyLeftJacobian(v, aH1, aH2))); + local.applyLeftJacobian(v, aH1, aH2))); CHECK(assert_equal(numericalDerivative21(f, omega, v), aH1)); CHECK(assert_equal(numericalDerivative22(f, omega, v), aH2)); CHECK(assert_equal(local.leftJacobian(), aH2)); @@ -352,17 +394,15 @@ TEST(SO3, ApplyLeftJacobian) { //****************************************************************************** TEST(SO3, ApplyInvDexp) { Matrix aH1, aH2; - for (bool nearZeroApprox : {true, false}) { + for (bool nearZero : {true, false}) { std::function f = [=](const Vector3& omega, const Vector3& v) { - return so3::DexpFunctor(omega, nearZeroApprox).applyInvDexp(v); + return so3::DexpFunctor(omega, nearZero).applyInvDexp(v); }; - for (Vector3 omega : {Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 1, 0), - Vector3(0, 0, 1), Vector3(0.1, 0.2, 0.3)}) { - so3::DexpFunctor local(omega, nearZeroApprox); + for (Vector3 omega : test_cases::omegas) { + so3::DexpFunctor local(omega, nearZero); Matrix invDexp = local.dexp().inverse(); - for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1), - Vector3(0.4, 0.3, 0.2)}) { + for (Vector3 v : test_cases::vs) { EXPECT(assert_equal(Vector3(invDexp * v), local.applyInvDexp(v, aH1, aH2))); EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); From 9b0f3c3b38f9eaaa2cc2e9be24a0fa94a3e5c04b Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 15 Dec 2024 17:55:22 -0500 Subject: [PATCH 093/112] Incredible simplification using E and F --- gtsam/geometry/Pose3.cpp | 21 ++++++++------------- 1 file changed, 8 insertions(+), 13 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index d87e3164c..7faf433bb 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -243,11 +243,15 @@ namespace pose3 { class ExpmapFunctor : public so3::DexpFunctor { private: static constexpr double one_twenty_fourth = 1.0 / 24.0; - static constexpr double one_one_hundred_twentieth = 1.0 / 120.0; + + // Constant used in computeQ + double F; // (B - 0.5) / theta2 or -1/24 for theta->0 public: ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false) - : so3::DexpFunctor(omega, nearZeroApprox) {} + : so3::DexpFunctor(omega, nearZeroApprox) { + F = nearZero ? - one_twenty_fourth : (B - 0.5) / theta2; + } // Compute the bottom-left 3x3 block of the SE(3) Expmap derivative // TODO(Frank): t = applyLeftJacobian(v), it would be nice to understand @@ -255,17 +259,8 @@ class ExpmapFunctor : public so3::DexpFunctor { Matrix3 computeQ(const Vector3& v) const { const Matrix3 V = skewSymmetric(v); const Matrix3 WVW = W * V * W; - - if (!nearZero) { - // Simplified from closed-form formula in Barfoot14tro eq. (102) - return -0.5 * V + C * (W * V + V * W - WVW) + - (B - 0.5) / theta2 * (WW * V + V * WW - 3 * WVW) - - 0.5 * (B - 3 * C) / theta2 * (WVW * W + W * WVW); - } else { - return -0.5 * V + one_sixth * (W * V + V * W - WVW) - - one_twenty_fourth * (WW * V + V * WW - 3 * WVW) + - one_one_hundred_twentieth * (WVW * W + W * WVW); - } + return -0.5 * V + C * (W * V + V * W - WVW) + + F * (WW * V + V * WW - 3 * WVW) - 0.5 * E * (WVW * W + W * WVW); } }; } // namespace pose3 From a32bb7bf096d6ccd7432563de28b2fdfd96deb39 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 15 Dec 2024 23:39:45 -0500 Subject: [PATCH 094/112] Export --- gtsam/geometry/Pose3.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 7faf433bb..f317cd4e8 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -240,18 +240,16 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) { /* ************************************************************************* */ namespace pose3 { -class ExpmapFunctor : public so3::DexpFunctor { - private: - static constexpr double one_twenty_fourth = 1.0 / 24.0; - +class GTSAM_EXPORT ExpmapFunctor : public so3::DexpFunctor { + protected: // Constant used in computeQ double F; // (B - 0.5) / theta2 or -1/24 for theta->0 public: ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false) : so3::DexpFunctor(omega, nearZeroApprox) { - F = nearZero ? - one_twenty_fourth : (B - 0.5) / theta2; - } + F = nearZero ? _one_twenty_fourth : (B - 0.5) / theta2; + } // Compute the bottom-left 3x3 block of the SE(3) Expmap derivative // TODO(Frank): t = applyLeftJacobian(v), it would be nice to understand @@ -262,6 +260,9 @@ class ExpmapFunctor : public so3::DexpFunctor { return -0.5 * V + C * (W * V + V * W - WVW) + F * (WW * V + V * WW - 3 * WVW) - 0.5 * E * (WVW * W + W * WVW); } + + protected: + static constexpr double _one_twenty_fourth = - 1.0 / 24.0; }; } // namespace pose3 From bcfb7d8444ae4fd1f8883c27e7f81a6b559c2a27 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 15 Dec 2024 23:42:59 -0500 Subject: [PATCH 095/112] Fix test cases --- gtsam/geometry/tests/testSO3.cpp | 50 +++++++++++++++----------------- 1 file changed, 24 insertions(+), 26 deletions(-) diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index e8de2e93d..3ecabe84b 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -304,26 +304,25 @@ TEST(SO3, JacobianLogmap) { } namespace test_cases { -std::vector nearZeros{ - {0, 0, 0}, {1e-5, 0, 0}, {0, 1e-5, 0}, {0, 0, 1e-5}}; -std::vector omegas{ +std::vector small{{0, 0, 0}, {1e-5, 0, 0}, {0, 1e-5, 0}, {0, 0, 1e-5}}; +std::vector large{ {0, 0, 0}, {1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0.1, 0.2, 0.3}}; +auto omegas = [](bool nearZero) { return nearZero ? small : large; }; std::vector vs{{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0.4, 0.3, 0.2}}; } // namespace test_cases //****************************************************************************** -TEST(SO3, Cross) { +TEST(SO3, CrossB) { Matrix aH1; for (bool nearZero : {true, false}) { std::function f = [=](const Vector3& omega, const Vector3& v) { - return so3::DexpFunctor(omega, nearZero).cross(v); + return so3::DexpFunctor(omega, nearZero).crossB(v); }; - const auto& omegas = nearZero ? test_cases::nearZeros : test_cases::omegas; - for (Vector3 omega : omegas) { + for (const Vector3& omega : test_cases::omegas(nearZero)) { so3::DexpFunctor local(omega, nearZero); - for (Vector3 v : test_cases::vs) { - local.cross(v, aH1); + for (const Vector3& v : test_cases::vs) { + local.crossB(v, aH1); EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); } } @@ -331,18 +330,17 @@ TEST(SO3, Cross) { } //****************************************************************************** -TEST(SO3, DoubleCross) { +TEST(SO3, DoubleCrossC) { Matrix aH1; for (bool nearZero : {true, false}) { std::function f = [=](const Vector3& omega, const Vector3& v) { - return so3::DexpFunctor(omega, nearZero).doubleCross(v); + return so3::DexpFunctor(omega, nearZero).doubleCrossC(v); }; - const auto& omegas = nearZero ? test_cases::nearZeros : test_cases::omegas; - for (Vector3 omega : omegas) { + for (const Vector3& omega : test_cases::omegas(nearZero)) { so3::DexpFunctor local(omega, nearZero); - for (Vector3 v : test_cases::vs) { - local.doubleCross(v, aH1); + for (const Vector3& v : test_cases::vs) { + local.doubleCrossC(v, aH1); EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); } } @@ -357,9 +355,9 @@ TEST(SO3, ApplyDexp) { [=](const Vector3& omega, const Vector3& v) { return so3::DexpFunctor(omega, nearZero).applyDexp(v); }; - for (Vector3 omega : test_cases::omegas) { + for (const Vector3& omega : test_cases::omegas(nearZero)) { so3::DexpFunctor local(omega, nearZero); - for (Vector3 v : test_cases::vs) { + for (const Vector3& v : test_cases::vs) { EXPECT(assert_equal(Vector3(local.dexp() * v), local.applyDexp(v, aH1, aH2))); EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); @@ -378,14 +376,14 @@ TEST(SO3, ApplyLeftJacobian) { [=](const Vector3& omega, const Vector3& v) { return so3::DexpFunctor(omega, nearZero).applyLeftJacobian(v); }; - for (Vector3 omega : test_cases::omegas) { + for (const Vector3& omega : test_cases::omegas(nearZero)) { so3::DexpFunctor local(omega, nearZero); - for (Vector3 v : test_cases::vs) { - CHECK(assert_equal(Vector3(local.leftJacobian() * v), - local.applyLeftJacobian(v, aH1, aH2))); - CHECK(assert_equal(numericalDerivative21(f, omega, v), aH1)); - CHECK(assert_equal(numericalDerivative22(f, omega, v), aH2)); - CHECK(assert_equal(local.leftJacobian(), aH2)); + for (const Vector3& v : test_cases::vs) { + EXPECT(assert_equal(Vector3(local.leftJacobian() * v), + local.applyLeftJacobian(v, aH1, aH2))); + EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); + EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2)); + EXPECT(assert_equal(local.leftJacobian(), aH2)); } } } @@ -399,10 +397,10 @@ TEST(SO3, ApplyInvDexp) { [=](const Vector3& omega, const Vector3& v) { return so3::DexpFunctor(omega, nearZero).applyInvDexp(v); }; - for (Vector3 omega : test_cases::omegas) { + for (const Vector3& omega : test_cases::omegas(nearZero)) { so3::DexpFunctor local(omega, nearZero); Matrix invDexp = local.dexp().inverse(); - for (Vector3 v : test_cases::vs) { + for (const Vector3& v : test_cases::vs) { EXPECT(assert_equal(Vector3(invDexp * v), local.applyInvDexp(v, aH1, aH2))); EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); From d547fe2ec137ce3f6727d84bd9f1e5b9d7508867 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 15 Dec 2024 23:43:59 -0500 Subject: [PATCH 096/112] Remove all nearZero paths --- gtsam/geometry/SO3.cpp | 52 ++++++++++-------------------------------- gtsam/geometry/SO3.h | 50 +++++++++++++++++++--------------------- 2 files changed, 35 insertions(+), 67 deletions(-) diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index e341911b9..ef1fa374b 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -93,15 +93,7 @@ DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox) E = nearZero ? _one_sixtieth : (B - 3.0 * C) / theta2; } -Matrix3 DexpFunctor::rightJacobian() const { - if (nearZero) { - return I_3x3 - B * W; // + C * WW; - } else { - return I_3x3 - B * W + C * WW; - } -} - -Vector3 DexpFunctor::cross(const Vector3& v, OptionalJacobian<3, 3> H) const { +Vector3 DexpFunctor::crossB(const Vector3& v, OptionalJacobian<3, 3> H) const { // Wv = omega x * v const Vector3 Wv = gtsam::cross(omega, v); if (H) { @@ -113,8 +105,8 @@ Vector3 DexpFunctor::cross(const Vector3& v, OptionalJacobian<3, 3> H) const { return B * Wv; } -Vector3 DexpFunctor::doubleCross(const Vector3& v, - OptionalJacobian<3, 3> H) const { +Vector3 DexpFunctor::doubleCrossC(const Vector3& v, + OptionalJacobian<3, 3> H) const { // WWv = omega x (omega x * v) Matrix3 doubleCrossJacobian; const Vector3 WWv = @@ -131,18 +123,12 @@ Vector3 DexpFunctor::doubleCross(const Vector3& v, // Multiplies v with left Jacobian through vector operations only. Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const { - if (nearZero) { - if (H1) *H1 = 0.5 * skewSymmetric(v); - if (H2) *H2 = I_3x3 - 0.5 * W; - return v - 0.5 * gtsam::cross(omega, v); - } else { Matrix3 D_BWv_omega, D_CWWv_omega; - const Vector3 BWv = cross(v, D_BWv_omega); - const Vector3 CWWv = doubleCross(v, D_CWWv_omega); - if (H1) *H1 = - D_BWv_omega + D_CWWv_omega; + const Vector3 BWv = crossB(v, D_BWv_omega); + const Vector3 CWWv = doubleCrossC(v, D_CWWv_omega); + if (H1) *H1 = -D_BWv_omega + D_CWWv_omega; if (H2) *H2 = rightJacobian(); return v - BWv + CWWv; - } } Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1, @@ -158,29 +144,15 @@ Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1, return c; } -Matrix3 DexpFunctor::leftJacobian() const { - if (nearZero) { - return I_3x3 + 0.5 * W; // + one_sixth * WW; - } else { - return I_3x3 + B * W + C * WW; - } -} - Vector3 DexpFunctor::applyLeftJacobian(const Vector3& v, OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const { - if (nearZero) { - if (H1) *H1 = - 0.5 * skewSymmetric(v); - if (H2) *H2 = I_3x3 + 0.5 * W; - return v + 0.5 * gtsam::cross(omega, v); - } else { - Matrix3 D_BWv_omega, D_CWWv_omega; - const Vector3 BWv = cross(v, D_BWv_omega); - const Vector3 CWWv = doubleCross(v, D_CWWv_omega); - if (H1) *H1 = D_BWv_omega + D_CWWv_omega; - if (H2) *H2 = leftJacobian(); - return v + BWv + CWWv; - } + Matrix3 D_BWv_omega, D_CWWv_omega; + const Vector3 BWv = crossB(v, D_BWv_omega); + const Vector3 CWWv = doubleCrossC(v, D_CWWv_omega); + if (H1) *H1 = D_BWv_omega + D_CWWv_omega; + if (H2) *H2 = leftJacobian(); + return v + BWv + CWWv; } } // namespace so3 diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index 6403c3ae0..efb947e73 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -155,7 +155,7 @@ class GTSAM_EXPORT ExpmapFunctor { }; /// Functor that implements Exponential map *and* its derivatives -class DexpFunctor : public ExpmapFunctor { +class GTSAM_EXPORT DexpFunctor : public ExpmapFunctor { protected: const Vector3 omega; double C; // Ethan's C constant: (1 - A) / theta^2 or 1/6 for theta->0 @@ -164,15 +164,8 @@ class DexpFunctor : public ExpmapFunctor { double E; // (B - 3.0 * C) / theta2 or -1/60 for theta->0 public: - /// Computes B * (omega x v). - Vector3 cross(const Vector3& v, OptionalJacobian<3, 3> H = {}) const; - - /// Computes C * (omega x (omega x v)). - Vector3 doubleCross(const Vector3& v, OptionalJacobian<3, 3> H = {}) const; - /// Constructor with element of Lie algebra so(3) - GTSAM_EXPORT explicit DexpFunctor(const Vector3& omega, - bool nearZeroApprox = false); + explicit DexpFunctor(const Vector3& omega, bool nearZeroApprox = false); // NOTE(luca): Right Jacobian for Exponential map in SO(3) - equation // (10.86) and following equations in G.S. Chirikjian, "Stochastic Models, @@ -180,28 +173,31 @@ class DexpFunctor : public ExpmapFunctor { // expmap(omega + v) \approx expmap(omega) * expmap(dexp * v) // This maps a perturbation v in the tangent space to // a perturbation on the manifold Expmap(dexp * v) - GTSAM_EXPORT Matrix3 rightJacobian() const; - - /// differential of expmap == right Jacobian - GTSAM_EXPORT Matrix3 dexp() const { return rightJacobian(); } - - /// Multiplies with dexp(), with optional derivatives - GTSAM_EXPORT Vector3 applyDexp(const Vector3& v, - OptionalJacobian<3, 3> H1 = {}, - OptionalJacobian<3, 3> H2 = {}) const; - - /// Multiplies with dexp().inverse(), with optional derivatives - GTSAM_EXPORT Vector3 applyInvDexp(const Vector3& v, - OptionalJacobian<3, 3> H1 = {}, - OptionalJacobian<3, 3> H2 = {}) const; + Matrix3 rightJacobian() const { return I_3x3 - B * W + C * WW; } // Compute the left Jacobian for Exponential map in SO(3) - GTSAM_EXPORT Matrix3 leftJacobian() const; + Matrix3 leftJacobian() const { return I_3x3 + B * W + C * WW; } + + /// differential of expmap == right Jacobian + inline Matrix3 dexp() const { return rightJacobian(); } + + /// Computes B * (omega x v). + Vector3 crossB(const Vector3& v, OptionalJacobian<3, 3> H = {}) const; + + /// Computes C * (omega x (omega x v)). + Vector3 doubleCrossC(const Vector3& v, OptionalJacobian<3, 3> H = {}) const; + + /// Multiplies with dexp(), with optional derivatives + Vector3 applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = {}, + OptionalJacobian<3, 3> H2 = {}) const; + + /// Multiplies with dexp().inverse(), with optional derivatives + Vector3 applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = {}, + OptionalJacobian<3, 3> H2 = {}) const; /// Multiplies with leftJacobian(), with optional derivatives - GTSAM_EXPORT Vector3 applyLeftJacobian(const Vector3& v, - OptionalJacobian<3, 3> H1 = {}, - OptionalJacobian<3, 3> H2 = {}) const; + Vector3 applyLeftJacobian(const Vector3& v, OptionalJacobian<3, 3> H1 = {}, + OptionalJacobian<3, 3> H2 = {}) const; protected: static constexpr double one_sixth = 1.0 / 6.0; From 0c1f087dba7374e13440461e260ee47086d39c31 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 16 Dec 2024 00:10:26 -0500 Subject: [PATCH 097/112] Final touches --- gtsam/geometry/Pose3.cpp | 7 ++----- gtsam/geometry/SO3.cpp | 25 ++++++++++++------------- gtsam/geometry/SO3.h | 27 ++++++++++++--------------- 3 files changed, 26 insertions(+), 33 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index f317cd4e8..5f0e8e3ce 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -240,12 +240,10 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) { /* ************************************************************************* */ namespace pose3 { -class GTSAM_EXPORT ExpmapFunctor : public so3::DexpFunctor { - protected: +struct GTSAM_EXPORT ExpmapFunctor : public so3::DexpFunctor { // Constant used in computeQ double F; // (B - 0.5) / theta2 or -1/24 for theta->0 - public: ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false) : so3::DexpFunctor(omega, nearZeroApprox) { F = nearZero ? _one_twenty_fourth : (B - 0.5) / theta2; @@ -253,7 +251,7 @@ class GTSAM_EXPORT ExpmapFunctor : public so3::DexpFunctor { // Compute the bottom-left 3x3 block of the SE(3) Expmap derivative // TODO(Frank): t = applyLeftJacobian(v), it would be nice to understand - // how to compute mess below from its derivatives in w and v. + // how to compute mess below from applyLeftJacobian derivatives in w and v. Matrix3 computeQ(const Vector3& v) const { const Matrix3 V = skewSymmetric(v); const Matrix3 WVW = W * V * W; @@ -261,7 +259,6 @@ class GTSAM_EXPORT ExpmapFunctor : public so3::DexpFunctor { F * (WW * V + V * WW - 3 * WVW) - 0.5 * E * (WVW * W + W * WVW); } - protected: static constexpr double _one_twenty_fourth = - 1.0 / 24.0; }; } // namespace pose3 diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index ef1fa374b..c54306ae5 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -26,7 +26,6 @@ #include #include -#include #include namespace gtsam { @@ -94,7 +93,7 @@ DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox) } Vector3 DexpFunctor::crossB(const Vector3& v, OptionalJacobian<3, 3> H) const { - // Wv = omega x * v + // Wv = omega x v const Vector3 Wv = gtsam::cross(omega, v); if (H) { // Apply product rule: @@ -123,10 +122,10 @@ Vector3 DexpFunctor::doubleCrossC(const Vector3& v, // Multiplies v with left Jacobian through vector operations only. Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const { - Matrix3 D_BWv_omega, D_CWWv_omega; - const Vector3 BWv = crossB(v, D_BWv_omega); - const Vector3 CWWv = doubleCrossC(v, D_CWWv_omega); - if (H1) *H1 = -D_BWv_omega + D_CWWv_omega; + Matrix3 D_BWv_w, D_CWWv_w; + const Vector3 BWv = crossB(v, H1 ? &D_BWv_w : nullptr); + const Vector3 CWWv = doubleCrossC(v, H1 ? &D_CWWv_w : nullptr); + if (H1) *H1 = -D_BWv_w + D_CWWv_w; if (H2) *H2 = rightJacobian(); return v - BWv + CWWv; } @@ -136,9 +135,9 @@ Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1, const Matrix3 invDexp = rightJacobian().inverse(); const Vector3 c = invDexp * v; if (H1) { - Matrix3 D_dexpv_omega; - applyDexp(c, D_dexpv_omega); // get derivative H of forward mapping - *H1 = -invDexp * D_dexpv_omega; + Matrix3 D_dexpv_w; + applyDexp(c, D_dexpv_w); // get derivative H of forward mapping + *H1 = -invDexp * D_dexpv_w; } if (H2) *H2 = invDexp; return c; @@ -147,10 +146,10 @@ Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1, Vector3 DexpFunctor::applyLeftJacobian(const Vector3& v, OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const { - Matrix3 D_BWv_omega, D_CWWv_omega; - const Vector3 BWv = crossB(v, D_BWv_omega); - const Vector3 CWWv = doubleCrossC(v, D_CWWv_omega); - if (H1) *H1 = D_BWv_omega + D_CWWv_omega; + Matrix3 D_BWv_w, D_CWWv_w; + const Vector3 BWv = crossB(v, H1 ? &D_BWv_w : nullptr); + const Vector3 CWWv = doubleCrossC(v, H1 ? &D_CWWv_w : nullptr); + if (H1) *H1 = D_BWv_w + D_CWWv_w; if (H2) *H2 = leftJacobian(); return v + BWv + CWWv; } diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index efb947e73..f831aa426 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -132,18 +132,15 @@ GTSAM_EXPORT Matrix99 Dcompose(const SO3& R); // functor also implements dedicated methods to apply dexp and/or inv(dexp). /// Functor implementing Exponential map -class GTSAM_EXPORT ExpmapFunctor { - protected: +struct GTSAM_EXPORT ExpmapFunctor { const double theta2, theta; const Matrix3 W, WW; bool nearZero; + // Ethan Eade's constants: - double A; // A = sin(theta) / theta or 1 for theta->0 - double B; // B = (1 - cos(theta)) / theta^2 or 0.5 for theta->0 + double A; // A = sin(theta) / theta or 1 for theta->0 + double B; // B = (1 - cos(theta)) / theta^2 or 0.5 for theta->0 - void init(bool nearZeroApprox = false); - - public: /// Constructor with element of Lie algebra so(3) explicit ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false); @@ -152,33 +149,34 @@ class GTSAM_EXPORT ExpmapFunctor { /// Rodrigues formula SO3 expmap() const; + + protected: + void init(bool nearZeroApprox = false); }; /// Functor that implements Exponential map *and* its derivatives -class GTSAM_EXPORT DexpFunctor : public ExpmapFunctor { - protected: +struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor { const Vector3 omega; double C; // Ethan's C constant: (1 - A) / theta^2 or 1/6 for theta->0 // Constants used in cross and doubleCross double D; // (A - 2.0 * B) / theta2 or -1/12 for theta->0 double E; // (B - 3.0 * C) / theta2 or -1/60 for theta->0 - public: /// Constructor with element of Lie algebra so(3) explicit DexpFunctor(const Vector3& omega, bool nearZeroApprox = false); // NOTE(luca): Right Jacobian for Exponential map in SO(3) - equation // (10.86) and following equations in G.S. Chirikjian, "Stochastic Models, // Information Theory, and Lie Groups", Volume 2, 2008. - // expmap(omega + v) \approx expmap(omega) * expmap(dexp * v) - // This maps a perturbation v in the tangent space to - // a perturbation on the manifold Expmap(dexp * v) + // Expmap(xi + dxi) \approx Expmap(xi) * Expmap(dexp * dxi) + // This maps a perturbation dxi=(w,v) in the tangent space to + // a perturbation on the manifold Expmap(dexp * xi) Matrix3 rightJacobian() const { return I_3x3 - B * W + C * WW; } // Compute the left Jacobian for Exponential map in SO(3) Matrix3 leftJacobian() const { return I_3x3 + B * W + C * WW; } - /// differential of expmap == right Jacobian + /// Differential of expmap == right Jacobian inline Matrix3 dexp() const { return rightJacobian(); } /// Computes B * (omega x v). @@ -199,7 +197,6 @@ class GTSAM_EXPORT DexpFunctor : public ExpmapFunctor { Vector3 applyLeftJacobian(const Vector3& v, OptionalJacobian<3, 3> H1 = {}, OptionalJacobian<3, 3> H2 = {}) const; - protected: static constexpr double one_sixth = 1.0 / 6.0; static constexpr double _one_twelfth = -1.0 / 12.0; static constexpr double _one_sixtieth = -1.0 / 60.0; From 76c953784704c084ffba34e4d3fdc233a67b2ba4 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 16 Dec 2024 12:48:34 -0500 Subject: [PATCH 098/112] inverse Jacobians --- gtsam/geometry/SO3.cpp | 32 +++++++++++++++++++++----------- gtsam/geometry/SO3.h | 25 ++++++++++++++++++++----- gtsam/geometry/tests/testSO3.cpp | 19 ++++++++++++++++++- 3 files changed, 59 insertions(+), 17 deletions(-) diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index c54306ae5..6499d4139 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -87,19 +87,29 @@ SO3 ExpmapFunctor::expmap() const { return SO3(I_3x3 + A * W + B * WW); } DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox) : ExpmapFunctor(omega, nearZeroApprox), omega(omega) { - C = nearZero ? one_sixth : (1 - A) / theta2; - D = nearZero ? _one_twelfth : (A - 2.0 * B) / theta2; - E = nearZero ? _one_sixtieth : (B - 3.0 * C) / theta2; + if (!nearZero) { + C = (1 - A) / theta2; + D = (1.0 - A / (2.0 * B)) / theta2; + E = (2.0 * B - A) / theta2; + F = (3.0 * C - B) / theta2; + } else { + // Limit as theta -> 0 + // TODO(Frank): flipping signs here does not trigger any tests: harden! + C = one_sixth; + D = one_twelfth; + E = one_twelfth; + F = one_sixtieth; + } } Vector3 DexpFunctor::crossB(const Vector3& v, OptionalJacobian<3, 3> H) const { // Wv = omega x v const Vector3 Wv = gtsam::cross(omega, v); if (H) { - // Apply product rule: - // D * omega.transpose() is 1x3 Jacobian of B with respect to omega - // - skewSymmetric(v) is 3x3 Jacobian of B gtsam::cross(omega, v) - *H = Wv * D * omega.transpose() - B * skewSymmetric(v); + // Apply product rule to (B Wv) + // - E * omega.transpose() is 1x3 Jacobian of B with respect to omega + // - skewSymmetric(v) is 3x3 Jacobian of Wv = gtsam::cross(omega, v) + *H = - Wv * E * omega.transpose() - B * skewSymmetric(v); } return B * Wv; } @@ -111,10 +121,10 @@ Vector3 DexpFunctor::doubleCrossC(const Vector3& v, const Vector3 WWv = gtsam::doubleCross(omega, v, H ? &doubleCrossJacobian : nullptr); if (H) { - // Apply product rule: - // E * omega.transpose() is 1x3 Jacobian of C with respect to omega - // doubleCrossJacobian is 3x3 Jacobian of C gtsam::doubleCross(omega, v) - *H = WWv * E * omega.transpose() + C * doubleCrossJacobian; + // Apply product rule to (C WWv) + // - F * omega.transpose() is 1x3 Jacobian of C with respect to omega + // doubleCrossJacobian is 3x3 Jacobian of WWv = gtsam::doubleCross(omega, v) + *H = - WWv * F * omega.transpose() + C * doubleCrossJacobian; } return C * WWv; } diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index f831aa426..488dea12e 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -157,10 +157,16 @@ struct GTSAM_EXPORT ExpmapFunctor { /// Functor that implements Exponential map *and* its derivatives struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor { const Vector3 omega; - double C; // Ethan's C constant: (1 - A) / theta^2 or 1/6 for theta->0 + + // Ethan's C constant used in Jacobians + double C; // (1 - A) / theta^2 or 1/6 for theta->0 + + // Constant used in inverse Jacobians + double D; // 1.0 - A / (2.0 * B)) / theta2 or 1/12 for theta->0 + // Constants used in cross and doubleCross - double D; // (A - 2.0 * B) / theta2 or -1/12 for theta->0 - double E; // (B - 3.0 * C) / theta2 or -1/60 for theta->0 + double E; // (A - 2.0 * B) / theta2 or -1/12 for theta->0 + double F; // (B - 3.0 * C) / theta2 or -1/60 for theta->0 /// Constructor with element of Lie algebra so(3) explicit DexpFunctor(const Vector3& omega, bool nearZeroApprox = false); @@ -179,6 +185,15 @@ struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor { /// Differential of expmap == right Jacobian inline Matrix3 dexp() const { return rightJacobian(); } + /// Inverse of right Jacobian + Matrix3 rightJacobianInverse() const { return I_3x3 + 0.5 * W + D * WW; } + + // Inverse of left Jacobian + Matrix3 leftJacobianInverse() const { return I_3x3 - 0.5 * W + D * WW; } + + /// Synonym for rightJacobianInverse + inline Matrix3 invDexp() const { return rightJacobianInverse(); } + /// Computes B * (omega x v). Vector3 crossB(const Vector3& v, OptionalJacobian<3, 3> H = {}) const; @@ -198,8 +213,8 @@ struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor { OptionalJacobian<3, 3> H2 = {}) const; static constexpr double one_sixth = 1.0 / 6.0; - static constexpr double _one_twelfth = -1.0 / 12.0; - static constexpr double _one_sixtieth = -1.0 / 60.0; + static constexpr double one_twelfth = 1.0 / 12.0; + static constexpr double one_sixtieth = 1.0 / 60.0; }; } // namespace so3 diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index 3ecabe84b..96872eae9 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -19,6 +19,7 @@ #include #include #include +#include using namespace std::placeholders; using namespace std; @@ -304,13 +305,29 @@ TEST(SO3, JacobianLogmap) { } namespace test_cases { -std::vector small{{0, 0, 0}, {1e-5, 0, 0}, {0, 1e-5, 0}, {0, 0, 1e-5}}; +std::vector small{{0, 0, 0}, // + {1e-5, 0, 0}, {0, 1e-5, 0}, {0, 0, 1e-5}, //, + {1e-4, 0, 0}, {0, 1e-4, 0}, {0, 0, 1e-4}}; std::vector large{ {0, 0, 0}, {1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0.1, 0.2, 0.3}}; auto omegas = [](bool nearZero) { return nearZero ? small : large; }; std::vector vs{{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0.4, 0.3, 0.2}}; } // namespace test_cases +//****************************************************************************** +TEST(SO3, JacobianInverses) { + Matrix HR, HL; + for (bool nearZero : {true, false}) { + for (const Vector3& omega : test_cases::omegas(nearZero)) { + so3::DexpFunctor local(omega, nearZero); + EXPECT(assert_equal(local.rightJacobian().inverse(), + local.rightJacobianInverse())); + EXPECT(assert_equal(local.leftJacobian().inverse(), + local.leftJacobianInverse())); + } + } +} + //****************************************************************************** TEST(SO3, CrossB) { Matrix aH1; From 6c84a2b539c144ed06c9a3aec5fa2ba697a29b5d Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 16 Dec 2024 12:48:52 -0500 Subject: [PATCH 099/112] Use X to map left to right --- gtsam/geometry/Pose3.cpp | 20 +++++++------------- 1 file changed, 7 insertions(+), 13 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 5f0e8e3ce..cb8de111e 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -22,6 +22,7 @@ #include #include #include +#include "gtsam/geometry/Rot3.h" namespace gtsam { @@ -241,25 +242,18 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) { /* ************************************************************************* */ namespace pose3 { struct GTSAM_EXPORT ExpmapFunctor : public so3::DexpFunctor { - // Constant used in computeQ - double F; // (B - 0.5) / theta2 or -1/24 for theta->0 - ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false) : so3::DexpFunctor(omega, nearZeroApprox) { - F = nearZero ? _one_twenty_fourth : (B - 0.5) / theta2; } - // Compute the bottom-left 3x3 block of the SE(3) Expmap derivative - // TODO(Frank): t = applyLeftJacobian(v), it would be nice to understand - // how to compute mess below from applyLeftJacobian derivatives in w and v. + // Compute the bottom-left 3x3 block of the SE(3) Expmap derivative. Matrix3 computeQ(const Vector3& v) const { - const Matrix3 V = skewSymmetric(v); - const Matrix3 WVW = W * V * W; - return -0.5 * V + C * (W * V + V * W - WVW) + - F * (WW * V + V * WW - 3 * WVW) - 0.5 * E * (WVW * W + W * WVW); + // X translate from left to right for our right expmap convention: + Matrix X = rightJacobian() * leftJacobianInverse(); + Matrix3 H; + applyLeftJacobian(v, H); + return X * H; } - - static constexpr double _one_twenty_fourth = - 1.0 / 24.0; }; } // namespace pose3 From 98697251bd9d8558258493f18c266c032b2c1dc3 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 16 Dec 2024 12:57:27 -0500 Subject: [PATCH 100/112] applyLeftJacobianInverse --- gtsam/geometry/SO3.cpp | 26 ++++++++++++++++++------ gtsam/geometry/SO3.h | 7 ++++++- gtsam/geometry/tests/testSO3.cpp | 34 ++++++++++++++++++++++++++------ 3 files changed, 54 insertions(+), 13 deletions(-) diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index 6499d4139..93bf072c5 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -142,14 +142,14 @@ Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1, Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const { - const Matrix3 invDexp = rightJacobian().inverse(); - const Vector3 c = invDexp * v; + const Matrix3 invJr = rightJacobianInverse(); + const Vector3 c = invJr * v; if (H1) { - Matrix3 D_dexpv_w; - applyDexp(c, D_dexpv_w); // get derivative H of forward mapping - *H1 = -invDexp * D_dexpv_w; + Matrix3 H; + applyDexp(c, H); // get derivative H of forward mapping + *H1 = -invJr * H; } - if (H2) *H2 = invDexp; + if (H2) *H2 = invJr; return c; } @@ -164,6 +164,20 @@ Vector3 DexpFunctor::applyLeftJacobian(const Vector3& v, return v + BWv + CWWv; } +Vector3 DexpFunctor::applyLeftJacobianInverse(const Vector3& v, + OptionalJacobian<3, 3> H1, + OptionalJacobian<3, 3> H2) const { + const Matrix3 invJl = leftJacobianInverse(); + const Vector3 c = invJl * v; + if (H1) { + Matrix3 H; + applyLeftJacobian(c, H); // get derivative H of forward mapping + *H1 = -invJl * H; + } + if (H2) *H2 = invJl; + return c; +} + } // namespace so3 //****************************************************************************** diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index 488dea12e..2845fcce7 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -162,7 +162,7 @@ struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor { double C; // (1 - A) / theta^2 or 1/6 for theta->0 // Constant used in inverse Jacobians - double D; // 1.0 - A / (2.0 * B)) / theta2 or 1/12 for theta->0 + double D; // (1 - A/2B) / theta2 or 1/12 for theta->0 // Constants used in cross and doubleCross double E; // (A - 2.0 * B) / theta2 or -1/12 for theta->0 @@ -212,6 +212,11 @@ struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor { Vector3 applyLeftJacobian(const Vector3& v, OptionalJacobian<3, 3> H1 = {}, OptionalJacobian<3, 3> H2 = {}) const; + /// Multiplies with leftJacobianInverse(), with optional derivatives + Vector3 applyLeftJacobianInverse(const Vector3& v, + OptionalJacobian<3, 3> H1 = {}, + OptionalJacobian<3, 3> H2 = {}) const; + static constexpr double one_sixth = 1.0 / 6.0; static constexpr double one_twelfth = 1.0 / 12.0; static constexpr double one_sixtieth = 1.0 / 60.0; diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index 96872eae9..d3313b7f1 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -385,6 +385,28 @@ TEST(SO3, ApplyDexp) { } } +//****************************************************************************** +TEST(SO3, ApplyInvDexp) { + Matrix aH1, aH2; + for (bool nearZero : {true, false}) { + std::function f = + [=](const Vector3& omega, const Vector3& v) { + return so3::DexpFunctor(omega, nearZero).applyInvDexp(v); + }; + for (const Vector3& omega : test_cases::omegas(nearZero)) { + so3::DexpFunctor local(omega, nearZero); + Matrix invJr = local.rightJacobianInverse(); + for (const Vector3& v : test_cases::vs) { + EXPECT( + assert_equal(Vector3(invJr * v), local.applyInvDexp(v, aH1, aH2))); + EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); + EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2)); + EXPECT(assert_equal(invJr, aH2)); + } + } + } +} + //****************************************************************************** TEST(SO3, ApplyLeftJacobian) { Matrix aH1, aH2; @@ -407,22 +429,22 @@ TEST(SO3, ApplyLeftJacobian) { } //****************************************************************************** -TEST(SO3, ApplyInvDexp) { +TEST(SO3, ApplyLeftJacobianInverse) { Matrix aH1, aH2; for (bool nearZero : {true, false}) { std::function f = [=](const Vector3& omega, const Vector3& v) { - return so3::DexpFunctor(omega, nearZero).applyInvDexp(v); + return so3::DexpFunctor(omega, nearZero).applyLeftJacobianInverse(v); }; for (const Vector3& omega : test_cases::omegas(nearZero)) { so3::DexpFunctor local(omega, nearZero); - Matrix invDexp = local.dexp().inverse(); + Matrix invJl = local.leftJacobianInverse(); for (const Vector3& v : test_cases::vs) { - EXPECT(assert_equal(Vector3(invDexp * v), - local.applyInvDexp(v, aH1, aH2))); + EXPECT(assert_equal(Vector3(invJl * v), + local.applyLeftJacobianInverse(v, aH1, aH2))); EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2)); - EXPECT(assert_equal(invDexp, aH2)); + EXPECT(assert_equal(invJl, aH2)); } } } From b11387167d9d8988e6a35c920518863c9d14fe44 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 16 Dec 2024 13:28:19 -0500 Subject: [PATCH 101/112] No more functor for Q --- gtsam/geometry/Pose3.cpp | 59 +++++++++++++----------------- gtsam/geometry/tests/testPose3.cpp | 11 ------ 2 files changed, 26 insertions(+), 44 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index cb8de111e..9f81204d6 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -22,6 +22,7 @@ #include #include #include + #include "gtsam/geometry/Rot3.h" namespace gtsam { @@ -112,7 +113,7 @@ Matrix6 Pose3::adjointMap(const Vector6& xi) { /* ************************************************************************* */ Vector6 Pose3::adjoint(const Vector6& xi, const Vector6& y, - OptionalJacobian<6, 6> Hxi, OptionalJacobian<6, 6> H_y) { + OptionalJacobian<6, 6> Hxi, OptionalJacobian<6, 6> H_y) { if (Hxi) { Hxi->setZero(); for (int i = 0; i < 6; ++i) { @@ -159,7 +160,7 @@ bool Pose3::equals(const Pose3& pose, double tol) const { /* ************************************************************************* */ Pose3 Pose3::interpolateRt(const Pose3& T, double t) const { return Pose3(interpolate(R_, T.R_, t), - interpolate(t_, T.t_, t)); + interpolate(t_, T.t_, t)); } /* ************************************************************************* */ @@ -239,30 +240,14 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) { #endif } -/* ************************************************************************* */ -namespace pose3 { -struct GTSAM_EXPORT ExpmapFunctor : public so3::DexpFunctor { - ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false) - : so3::DexpFunctor(omega, nearZeroApprox) { - } - - // Compute the bottom-left 3x3 block of the SE(3) Expmap derivative. - Matrix3 computeQ(const Vector3& v) const { - // X translate from left to right for our right expmap convention: - Matrix X = rightJacobian() * leftJacobianInverse(); - Matrix3 H; - applyLeftJacobian(v, H); - return X * H; - } -}; -} // namespace pose3 - /* ************************************************************************* */ Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi, double nearZeroThreshold) { const auto w = xi.head<3>(); const auto v = xi.tail<3>(); - return pose3::ExpmapFunctor(w).computeQ(v); + Matrix3 Q; + ExpmapTranslation(w, v, Q, {}, nearZeroThreshold); + return Q; } /* ************************************************************************* */ @@ -273,13 +258,22 @@ Vector3 Pose3::ExpmapTranslation(const Vector3& w, const Vector3& v, const double theta2 = w.dot(w); bool nearZero = (theta2 <= nearZeroThreshold); - if (Q) *Q = pose3::ExpmapFunctor(w, nearZero).computeQ(v); - - if (nearZero) { - return v + 0.5 * w.cross(v); + if (Q) { + // Instantiate functor for Dexp-related operations: + so3::DexpFunctor local(w, nearZero); + // X translate from left to right for our right expmap convention: + Matrix3 X = local.rightJacobian() * local.leftJacobianInverse(); + Matrix3 H; + Vector t = local.applyLeftJacobian(v, H); + *Q = X * H; + return t; + } else if (nearZero) { + // (I_3x3 + B * W + C * WW) * v with B->0.5, C->1/6 + Vector3 Wv = w.cross(v); + return v + 0.5 * Wv + w.cross(Wv) * so3::DexpFunctor::one_sixth; } else { - // NOTE(Frank): t can also be computed by calling applyLeftJacobian(v), but - // formulas below convey geometric insight and creating functor is not free. + // NOTE(Frank): if Q is not asked we use formulas below instead of calling + // applyLeftJacobian(v), as they convey geometric insight and are faster. Vector3 t_parallel = w * w.dot(v); // translation parallel to axis Vector3 w_cross_v = w.cross(v); // translation orthogonal to axis Rot3 rotation = R.value_or(Rot3::Expmap(w)); @@ -314,7 +308,6 @@ const Point3& Pose3::translation(OptionalJacobian<3, 6> Hself) const { } /* ************************************************************************* */ - const Rot3& Pose3::rotation(OptionalJacobian<3, 6> Hself) const { if (Hself) { *Hself << I_3x3, Z_3x3; @@ -332,14 +325,14 @@ Matrix4 Pose3::matrix() const { /* ************************************************************************* */ Pose3 Pose3::transformPoseFrom(const Pose3& aTb, OptionalJacobian<6, 6> Hself, - OptionalJacobian<6, 6> HaTb) const { + OptionalJacobian<6, 6> HaTb) const { const Pose3& wTa = *this; return wTa.compose(aTb, Hself, HaTb); } /* ************************************************************************* */ Pose3 Pose3::transformPoseTo(const Pose3& wTb, OptionalJacobian<6, 6> Hself, - OptionalJacobian<6, 6> HwTb) const { + OptionalJacobian<6, 6> HwTb) const { if (Hself) *Hself = -wTb.inverse().AdjointMap() * AdjointMap(); if (HwTb) *HwTb = I_6x6; const Pose3& wTa = *this; @@ -348,7 +341,7 @@ Pose3 Pose3::transformPoseTo(const Pose3& wTb, OptionalJacobian<6, 6> Hself, /* ************************************************************************* */ Point3 Pose3::transformFrom(const Point3& point, OptionalJacobian<3, 6> Hself, - OptionalJacobian<3, 3> Hpoint) const { + OptionalJacobian<3, 3> Hpoint) const { // Only get matrix once, to avoid multiple allocations, // as well as multiple conversions in the Quaternion case const Matrix3 R = R_.matrix(); @@ -372,7 +365,7 @@ Matrix Pose3::transformFrom(const Matrix& points) const { /* ************************************************************************* */ Point3 Pose3::transformTo(const Point3& point, OptionalJacobian<3, 6> Hself, - OptionalJacobian<3, 3> Hpoint) const { + OptionalJacobian<3, 3> Hpoint) const { // Only get transpose once, to avoid multiple allocations, // as well as multiple conversions in the Quaternion case const Matrix3 Rt = R_.transpose(); @@ -478,7 +471,7 @@ std::optional Pose3::Align(const Point3Pairs &abPointPairs) { std::optional Pose3::Align(const Matrix& a, const Matrix& b) { if (a.rows() != 3 || b.rows() != 3 || a.cols() != b.cols()) { throw std::invalid_argument( - "Pose3:Align expects 3*N matrices of equal shape."); + "Pose3:Align expects 3*N matrices of equal shape."); } Point3Pairs abPointPairs; for (Eigen::Index j = 0; j < a.cols(); j++) { diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 4ff4f9a85..3b3de6792 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -900,17 +900,6 @@ TEST(Pose3, ExpmapDerivative4) { } } -TEST( Pose3, ExpmapDerivativeQr) { - Vector6 w = Vector6::Random(); - w.head<3>().normalize(); - w.head<3>() = w.head<3>() * 0.9e-2; - Matrix3 actualQr = Pose3::ComputeQforExpmapDerivative(w, 0.01); - Matrix expectedH = numericalDerivative21 >(&Pose3::Expmap, w, {}); - Matrix3 expectedQr = expectedH.bottomLeftCorner<3, 3>(); - EXPECT(assert_equal(expectedQr, actualQr, 1e-6)); -} - /* ************************************************************************* */ TEST( Pose3, LogmapDerivative) { Matrix6 actualH; From 2aa36d4f7ab89e59138fbaa2c9914643069b77ef Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 16 Dec 2024 14:08:02 -0500 Subject: [PATCH 102/112] Modernize ExmapTranslation --- gtsam/geometry/Pose3.cpp | 56 +++++++++++++++++--------------- gtsam/geometry/Pose3.h | 10 +++--- gtsam/geometry/tests/testSO3.cpp | 1 + gtsam/navigation/NavState.cpp | 27 ++++++++------- 4 files changed, 50 insertions(+), 44 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 9f81204d6..6fbd2468f 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -164,22 +164,22 @@ Pose3 Pose3::interpolateRt(const Pose3& T, double t) const { } /* ************************************************************************* */ -/** Modified from Murray94book version (which assumes w and v normalized?) */ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi) { // Get angular velocity omega and translational velocity v from twist xi const Vector3 w = xi.head<3>(), v = xi.tail<3>(); // Compute rotation using Expmap - Matrix3 Jw; - Rot3 R = Rot3::Expmap(w, Hxi ? &Jw : nullptr); + Matrix3 Jr; + Rot3 R = Rot3::Expmap(w, Hxi ? &Jr : nullptr); - // Compute translation and optionally its Jacobian in w + // Compute translation and optionally its Jacobian Q in w + // The Jacobian in v is the right Jacobian Jr of SO(3), which we already have. Matrix3 Q; - const Vector3 t = ExpmapTranslation(w, v, Hxi ? &Q : nullptr, R); + const Vector3 t = ExpmapTranslation(w, v, Hxi ? &Q : nullptr); if (Hxi) { - *Hxi << Jw, Z_3x3, - Q, Jw; + *Hxi << Jr, Z_3x3, // + Q, Jr; } return Pose3(R, t); @@ -251,35 +251,39 @@ Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi, } /* ************************************************************************* */ +// NOTE(Frank): t = applyLeftJacobian(v) does the same as the intuitive formulas +// t_parallel = w * w.dot(v); // translation parallel to axis +// w_cross_v = w.cross(v); // translation orthogonal to axis +// t = (w_cross_v - Rot3::Expmap(w) * w_cross_v + t_parallel) / theta2; +// but functor does not need R, deals automatically with the case where theta2 +// is near zero, and also gives us the machinery for the Jacobians. Vector3 Pose3::ExpmapTranslation(const Vector3& w, const Vector3& v, OptionalJacobian<3, 3> Q, - const std::optional& R, + OptionalJacobian<3, 3> J, double nearZeroThreshold) { const double theta2 = w.dot(w); bool nearZero = (theta2 <= nearZeroThreshold); + // Instantiate functor for Dexp-related operations: + so3::DexpFunctor local(w, nearZero); + + // Call applyLeftJacobian which is faster than local.leftJacobian() * v if you + // don't need Jacobians, and returns Jacobian of t with respect to w if asked. + Matrix3 H; + Vector t = local.applyLeftJacobian(v, Q ? &H : nullptr); + + // We return Jacobians for use in Expmap, so we multiply with X, that + // translates from left to right for our right expmap convention: if (Q) { - // Instantiate functor for Dexp-related operations: - so3::DexpFunctor local(w, nearZero); - // X translate from left to right for our right expmap convention: Matrix3 X = local.rightJacobian() * local.leftJacobianInverse(); - Matrix3 H; - Vector t = local.applyLeftJacobian(v, H); *Q = X * H; - return t; - } else if (nearZero) { - // (I_3x3 + B * W + C * WW) * v with B->0.5, C->1/6 - Vector3 Wv = w.cross(v); - return v + 0.5 * Wv + w.cross(Wv) * so3::DexpFunctor::one_sixth; - } else { - // NOTE(Frank): if Q is not asked we use formulas below instead of calling - // applyLeftJacobian(v), as they convey geometric insight and are faster. - Vector3 t_parallel = w * w.dot(v); // translation parallel to axis - Vector3 w_cross_v = w.cross(v); // translation orthogonal to axis - Rot3 rotation = R.value_or(Rot3::Expmap(w)); - Vector3 t = (w_cross_v - rotation * w_cross_v + t_parallel) / theta2; - return t; } + + if (J) { + *J = local.rightJacobian(); // = X * local.leftJacobian(); + } + + return t; } /* ************************************************************************* */ diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index d7c280c80..04504de65 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -223,17 +223,19 @@ public: const Vector6& xi, double nearZeroThreshold = 1e-5); /** - * Compute the translation part of the exponential map, with derivative. + * Compute the translation part of the exponential map, with Jacobians. * @param w 3D angular velocity * @param v 3D velocity * @param Q Optionally, compute 3x3 Jacobian wrpt w - * @param R Optionally, precomputed as Rot3::Expmap(w) + * @param J Optionally, compute 3x3 Jacobian wrpt v = right Jacobian of SO(3) * @param nearZeroThreshold threshold for small values - * Note Q is 3x3 bottom-left block of SE3 Expmap right derivative matrix + * @note This function returns Jacobians Q and J corresponding to the bottom + * blocks of the SE(3) exponential, and translated from left to right from the + * applyLeftJacobian Jacobians. */ static Vector3 ExpmapTranslation(const Vector3& w, const Vector3& v, OptionalJacobian<3, 3> Q = {}, - const std::optional& R = {}, + OptionalJacobian<3, 3> J = {}, double nearZeroThreshold = 1e-5); using LieGroup::inverse; // version with derivative diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index d3313b7f1..200642456 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -304,6 +304,7 @@ TEST(SO3, JacobianLogmap) { EXPECT(assert_equal(Jexpected, Jactual)); } +//****************************************************************************** namespace test_cases { std::vector small{{0, 0, 0}, // {1e-5, 0, 0}, {0, 1e-5, 0}, {0, 0, 1e-5}, //, diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index b9d48a3cb..c9cd244d3 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -114,24 +114,23 @@ NavState NavState::inverse() const { //------------------------------------------------------------------------------ NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi) { - // Get angular velocity w, translational velocity v, and acceleration a - Vector3 w = xi.head<3>(); - Vector3 rho = xi.segment<3>(3); - Vector3 nu = xi.tail<3>(); + // Get angular velocity w and components rho and nu from xi + Vector3 w = xi.head<3>(), rho = xi.segment<3>(3), nu = xi.tail<3>(); // Compute rotation using Expmap - Rot3 R = Rot3::Expmap(w); + Matrix3 Jr; + Rot3 R = Rot3::Expmap(w, Hxi ? &Jr : nullptr); - // Compute translations and optionally their Jacobians + // Compute translations and optionally their Jacobians Q in w + // The Jacobians with respect to rho and nu are equal to Jr Matrix3 Qt, Qv; - Vector3 t = Pose3::ExpmapTranslation(w, rho, Hxi ? &Qt : nullptr, R); - Vector3 v = Pose3::ExpmapTranslation(w, nu, Hxi ? &Qv : nullptr, R); + Vector3 t = Pose3::ExpmapTranslation(w, rho, Hxi ? &Qt : nullptr); + Vector3 v = Pose3::ExpmapTranslation(w, nu, Hxi ? &Qv : nullptr); if (Hxi) { - const Matrix3 Jw = Rot3::ExpmapDerivative(w); - *Hxi << Jw, Z_3x3, Z_3x3, - Qt, Jw, Z_3x3, - Qv, Z_3x3, Jw; + *Hxi << Jr, Z_3x3, Z_3x3, // + Qt, Jr, Z_3x3, // + Qv, Z_3x3, Jr; } return NavState(R, t, v); @@ -235,8 +234,8 @@ Matrix9 NavState::LogmapDerivative(const NavState& state) { Matrix3 Qt, Qv; const Rot3 R = Rot3::Expmap(w); - Pose3::ExpmapTranslation(w, rho, Qt, R); - Pose3::ExpmapTranslation(w, nu, Qv, R); + Pose3::ExpmapTranslation(w, rho, Qt); + Pose3::ExpmapTranslation(w, nu, Qv); const Matrix3 Jw = Rot3::LogmapDerivative(w); const Matrix3 Qt2 = -Jw * Qt * Jw; const Matrix3 Qv2 = -Jw * Qv * Jw; From db5b9dee65442245a8de1708fb1a21d72ee16e55 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 16 Dec 2024 18:35:17 -0500 Subject: [PATCH 103/112] Taylor expansion --- gtsam/geometry/SO3.cpp | 25 ++++--- gtsam/geometry/SO3.h | 16 ++--- gtsam/geometry/tests/testPose3.cpp | 101 +++++++++++++++++------------ 3 files changed, 83 insertions(+), 59 deletions(-) diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index 93bf072c5..41f7ce810 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -33,6 +33,15 @@ namespace gtsam { //****************************************************************************** namespace so3 { +static constexpr double one_6th = 1.0 / 6.0; +static constexpr double one_12th = 1.0 / 12.0; +static constexpr double one_24th = 1.0 / 24.0; +static constexpr double one_60th = 1.0 / 60.0; +static constexpr double one_120th = 1.0 / 120.0; +static constexpr double one_180th = 1.0 / 180.0; +static constexpr double one_720th = 1.0 / 720.0; +static constexpr double one_1260th = 1.0 / 1260.0; + GTSAM_EXPORT Matrix99 Dcompose(const SO3& Q) { Matrix99 H; auto R = Q.matrix(); @@ -60,9 +69,9 @@ void ExpmapFunctor::init(bool nearZeroApprox) { 2.0 * s2 * s2; // numerically better than [1 - cos(theta)] B = one_minus_cos / theta2; } else { - // Limits as theta -> 0: - A = 1.0; - B = 0.5; + // Taylor expansion at 0 + A = 1.0 - theta2 * one_6th; + B = 0.5 - theta2 * one_24th; } } @@ -93,12 +102,12 @@ DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox) E = (2.0 * B - A) / theta2; F = (3.0 * C - B) / theta2; } else { - // Limit as theta -> 0 + // Taylor expansion at 0 // TODO(Frank): flipping signs here does not trigger any tests: harden! - C = one_sixth; - D = one_twelfth; - E = one_twelfth; - F = one_sixtieth; + C = one_6th - theta2 * one_120th; + D = one_12th + theta2 * one_720th; + E = one_12th - theta2 * one_180th; + F = one_60th - theta2 * one_1260th; } } diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index 2845fcce7..6e2e38101 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -138,8 +138,8 @@ struct GTSAM_EXPORT ExpmapFunctor { bool nearZero; // Ethan Eade's constants: - double A; // A = sin(theta) / theta or 1 for theta->0 - double B; // B = (1 - cos(theta)) / theta^2 or 0.5 for theta->0 + double A; // A = sin(theta) / theta + double B; // B = (1 - cos(theta)) /// Constructor with element of Lie algebra so(3) explicit ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false); @@ -159,14 +159,14 @@ struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor { const Vector3 omega; // Ethan's C constant used in Jacobians - double C; // (1 - A) / theta^2 or 1/6 for theta->0 + double C; // (1 - A) / theta^2 // Constant used in inverse Jacobians - double D; // (1 - A/2B) / theta2 or 1/12 for theta->0 + double D; // (1 - A/2B) / theta2 // Constants used in cross and doubleCross - double E; // (A - 2.0 * B) / theta2 or -1/12 for theta->0 - double F; // (B - 3.0 * C) / theta2 or -1/60 for theta->0 + double E; // (2B - A) / theta2 + double F; // (3C - B) / theta2 /// Constructor with element of Lie algebra so(3) explicit DexpFunctor(const Vector3& omega, bool nearZeroApprox = false); @@ -216,10 +216,6 @@ struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor { Vector3 applyLeftJacobianInverse(const Vector3& v, OptionalJacobian<3, 3> H1 = {}, OptionalJacobian<3, 3> H2 = {}) const; - - static constexpr double one_sixth = 1.0 / 6.0; - static constexpr double one_twelfth = 1.0 / 12.0; - static constexpr double one_sixtieth = 1.0 / 60.0; }; } // namespace so3 diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 3b3de6792..ce74091f0 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -835,38 +835,7 @@ TEST(Pose3, Align2) { } /* ************************************************************************* */ -TEST( Pose3, ExpmapDerivative1) { - Matrix6 actualH; - Vector6 w; w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0; - Pose3::Expmap(w,actualH); - Matrix expectedH = numericalDerivative21 >(&Pose3::Expmap, w, {}); - EXPECT(assert_equal(expectedH, actualH)); -} - -/* ************************************************************************* */ -TEST( Pose3, ExpmapDerivative2) { - Matrix6 actualH; - Vector6 w; w << 1.0, -2.0, 3.0, -10.0, -20.0, 30.0; - Pose3::Expmap(w,actualH); - Matrix expectedH = numericalDerivative21 >(&Pose3::Expmap, w, {}); - EXPECT(assert_equal(expectedH, actualH)); -} - -/* ************************************************************************* */ -TEST( Pose3, ExpmapDerivative3) { - Matrix6 actualH; - Vector6 w; w << 0.0, 0.0, 0.0, -10.0, -20.0, 30.0; - Pose3::Expmap(w,actualH); - Matrix expectedH = numericalDerivative21 >(&Pose3::Expmap, w, {}); - // Small angle approximation is not as precise as numerical derivative? - EXPECT(assert_equal(expectedH, actualH, 1e-5)); -} - -/* ************************************************************************* */ -TEST(Pose3, ExpmapDerivative4) { +TEST(Pose3, ExpmapDerivative) { // Iserles05an (Lie-group Methods) says: // scalar is easy: d exp(a(t)) / dt = exp(a(t)) a'(t) // matrix is hard: d exp(A(t)) / dt = exp(A(t)) dexp[-A(t)] A'(t) @@ -900,15 +869,65 @@ TEST(Pose3, ExpmapDerivative4) { } } -/* ************************************************************************* */ -TEST( Pose3, LogmapDerivative) { - Matrix6 actualH; - Vector6 w; w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0; - Pose3 p = Pose3::Expmap(w); - EXPECT(assert_equal(w, Pose3::Logmap(p,actualH), 1e-5)); - Matrix expectedH = numericalDerivative21 >(&Pose3::Logmap, p, {}); - EXPECT(assert_equal(expectedH, actualH)); +//****************************************************************************** +namespace test_cases { +std::vector small{{0, 0, 0}, // + {1e-5, 0, 0}, {0, 1e-5, 0}, {0, 0, 1e-5}, //, + {1e-4, 0, 0}, {0, 1e-4, 0}, {0, 0, 1e-4}}; +std::vector large{{0, 0, 0}, {1, 0, 0}, {0, 1, 0}, + {0, 0, 1}, {.1, .2, .3}, {1, -2, 3}}; +auto omegas = [](bool nearZero) { return nearZero ? small : large; }; +std::vector vs{{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, + {.4, .3, .2}, {4, 5, 6}, {-10, -20, 30}}; +} // namespace test_cases + +//****************************************************************************** +TEST(Pose3, ExpmapDerivatives) { + for (bool nearZero : {true, false}) { + for (const Vector3& w : test_cases::omegas(nearZero)) { + for (Vector3 v : test_cases::vs) { + const Vector6 xi = (Vector6() << w, v).finished(); + const Matrix6 expectedH = + numericalDerivative21 >( + &Pose3::Expmap, xi, {}); + Matrix actualH; + Pose3::Expmap(xi, actualH); + EXPECT(assert_equal(expectedH, actualH)); + } + } + } +} + +//****************************************************************************** +// Check logmap for all small values, as we don't want wrapping. +TEST(Pose3, Logmap) { + static constexpr bool nearZero = true; + for (const Vector3& w : test_cases::omegas(nearZero)) { + for (Vector3 v : test_cases::vs) { + const Vector6 xi = (Vector6() << w, v).finished(); + Pose3 pose = Pose3::Expmap(xi); + EXPECT(assert_equal(xi, Pose3::Logmap(pose), 1e-5)); + } + } +} + +//****************************************************************************** +// Check logmap derivatives for all values +TEST(Pose3, LogmapDerivatives) { + for (bool nearZero : {true, false}) { + for (const Vector3& w : test_cases::omegas(nearZero)) { + for (Vector3 v : test_cases::vs) { + const Vector6 xi = (Vector6() << w, v).finished(); + Pose3 pose = Pose3::Expmap(xi); + const Matrix6 expectedH = + numericalDerivative21 >( + &Pose3::Logmap, pose, {}); + Matrix actualH; + Pose3::Logmap(pose, actualH); + EXPECT(assert_equal(expectedH, actualH)); + } + } + } } /* ************************************************************************* */ From 49c2a040095a4d4d682a2c846f8a79bb8f87584c Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 16 Dec 2024 18:51:29 -0500 Subject: [PATCH 104/112] Address review comments --- gtsam/geometry/Pose3.cpp | 13 +++++++------ gtsam/geometry/Pose3.h | 2 +- gtsam/geometry/SO3.h | 4 ++++ gtsam/geometry/tests/testSO3.cpp | 1 - gtsam/navigation/NavState.cpp | 2 +- 5 files changed, 13 insertions(+), 9 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 6fbd2468f..16dda3d1e 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -11,20 +11,19 @@ /** * @file Pose3.cpp - * @brief 3D Pose + * @brief 3D Pose manifold SO(3) x R^3 and group SE(3) */ -#include -#include -#include #include +#include +#include +#include +#include #include #include #include -#include "gtsam/geometry/Rot3.h" - namespace gtsam { /** instantiate concept checks */ @@ -164,6 +163,8 @@ Pose3 Pose3::interpolateRt(const Pose3& T, double t) const { } /* ************************************************************************* */ +// Expmap is implemented in so3::ExpmapFunctor::expmap, based on Ethan Eade's +// elegant Lie group document, at https://www.ethaneade.org/lie.pdf. Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi) { // Get angular velocity omega and translational velocity v from twist xi const Vector3 w = xi.head<3>(), v = xi.tail<3>(); diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 04504de65..81a9848e2 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -11,7 +11,7 @@ /** *@file Pose3.h - *@brief 3D Pose + * @brief 3D Pose manifold SO(3) x R^3 and group SE(3) */ // \callgraph diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index 6e2e38101..163ae6623 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -132,6 +132,8 @@ GTSAM_EXPORT Matrix99 Dcompose(const SO3& R); // functor also implements dedicated methods to apply dexp and/or inv(dexp). /// Functor implementing Exponential map +/// Math is based on Ethan Eade's elegant Lie group document, at +/// https://www.ethaneade.org/lie.pdf. struct GTSAM_EXPORT ExpmapFunctor { const double theta2, theta; const Matrix3 W, WW; @@ -155,6 +157,8 @@ struct GTSAM_EXPORT ExpmapFunctor { }; /// Functor that implements Exponential map *and* its derivatives +/// Math extends Ethan theme of elegant I + aW + bWW expressions. +/// See https://www.ethaneade.org/lie.pdf expmap (82) and left Jacobian (83). struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor { const Vector3 omega; diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index 200642456..c6fd52161 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -19,7 +19,6 @@ #include #include #include -#include using namespace std::placeholders; using namespace std; diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index c9cd244d3..bd482132e 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -114,7 +114,7 @@ NavState NavState::inverse() const { //------------------------------------------------------------------------------ NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi) { - // Get angular velocity w and components rho and nu from xi + // Get angular velocity w and components rho (for t) and nu (for v) from xi Vector3 w = xi.head<3>(), rho = xi.segment<3>(3), nu = xi.tail<3>(); // Compute rotation using Expmap From 8e473c04e82eacbef373cc8efdc8be59eb01e4e1 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 16 Dec 2024 22:46:16 -0500 Subject: [PATCH 105/112] Address Quaternion on Ubuntu accuracy --- gtsam/geometry/tests/testPose3.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index ce74091f0..b04fb1982 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -906,7 +906,7 @@ TEST(Pose3, Logmap) { for (Vector3 v : test_cases::vs) { const Vector6 xi = (Vector6() << w, v).finished(); Pose3 pose = Pose3::Expmap(xi); - EXPECT(assert_equal(xi, Pose3::Logmap(pose), 1e-5)); + EXPECT(assert_equal(xi, Pose3::Logmap(pose))); } } } @@ -924,7 +924,13 @@ TEST(Pose3, LogmapDerivatives) { &Pose3::Logmap, pose, {}); Matrix actualH; Pose3::Logmap(pose, actualH); +#ifdef GTSAM_USE_QUATERNIONS + // TODO(Frank): Figure out why quaternions are not as accurate. + // Hint: 6 cases fail on Ubuntu 22.04, but none on MacOS. + EXPECT(assert_equal(expectedH, actualH, 1e-7)); +#else EXPECT(assert_equal(expectedH, actualH)); +#endif } } } From 004cab1542c807e16218bc8cf85c4feb759ca8cb Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 17 Dec 2024 00:14:17 -0500 Subject: [PATCH 106/112] add serialize() to various preintegration classes --- gtsam/navigation/navigation.i | 28 +++++++++++++++++++--------- 1 file changed, 19 insertions(+), 9 deletions(-) diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i index 831788073..3e7aa0fd6 100644 --- a/gtsam/navigation/navigation.i +++ b/gtsam/navigation/navigation.i @@ -77,6 +77,9 @@ virtual class PreintegratedRotationParams { std::optional getOmegaCoriolis() const; std::optional getBodyPSensor() const; + + // enabling serialization functionality + void serialize() const; }; #include @@ -170,22 +173,26 @@ virtual class PreintegrationCombinedParams : gtsam::PreintegrationParams { gtsam::Matrix getBiasAccCovariance() const ; gtsam::Matrix getBiasOmegaCovariance() const ; gtsam::Matrix getBiasAccOmegaInit() const; - + + // enabling serialization functionality + void serialize() const; }; class PreintegratedCombinedMeasurements { -// Constructors - PreintegratedCombinedMeasurements(const gtsam::PreintegrationCombinedParams* params); - PreintegratedCombinedMeasurements(const gtsam::PreintegrationCombinedParams* params, - const gtsam::imuBias::ConstantBias& bias); + // Constructors + PreintegratedCombinedMeasurements( + const gtsam::PreintegrationCombinedParams* params); + PreintegratedCombinedMeasurements( + const gtsam::PreintegrationCombinedParams* params, + const gtsam::imuBias::ConstantBias& bias); // Testable void print(string s = "Preintegrated Measurements:") const; bool equals(const gtsam::PreintegratedCombinedMeasurements& expected, - double tol); + double tol); // Standard Interface - void integrateMeasurement(gtsam::Vector measuredAcc, gtsam::Vector measuredOmega, - double deltaT); + void integrateMeasurement(gtsam::Vector measuredAcc, + gtsam::Vector measuredOmega, double deltaT); void resetIntegration(); void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias& biasHat); @@ -197,7 +204,10 @@ class PreintegratedCombinedMeasurements { gtsam::imuBias::ConstantBias biasHat() const; gtsam::Vector biasHatVector() const; gtsam::NavState predict(const gtsam::NavState& state_i, - const gtsam::imuBias::ConstantBias& bias) const; + const gtsam::imuBias::ConstantBias& bias) const; + + // enabling serialization functionality + void serialize() const; }; virtual class CombinedImuFactor: gtsam::NoiseModelFactor { From c5554c836f79ec52bebadf31659dde5baf48d004 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 17 Dec 2024 00:15:09 -0500 Subject: [PATCH 107/112] python tests --- python/gtsam/tests/test_Serialization.py | 47 ++++++++++++++++++++++++ 1 file changed, 47 insertions(+) create mode 100644 python/gtsam/tests/test_Serialization.py diff --git a/python/gtsam/tests/test_Serialization.py b/python/gtsam/tests/test_Serialization.py new file mode 100644 index 000000000..be8dd7bec --- /dev/null +++ b/python/gtsam/tests/test_Serialization.py @@ -0,0 +1,47 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +KalmanFilter unit tests. +Author: Frank Dellaert & Duy Nguyen Ta (Python) +""" +import unittest +from copy import deepcopy + +import numpy as np +from gtsam.utils.test_case import GtsamTestCase + +import gtsam + + +class TestSerialization(GtsamTestCase): + """Tests for serialization of various GTSAM objects.""" + + def test_PreintegratedImuMeasurements(self): + """ + Test the serialization of `PreintegratedImuMeasurements` by performing a deepcopy. + """ + params = gtsam.PreintegrationParams(np.asarray([0, 0, -9.81])) + pim = gtsam.PreintegratedImuMeasurements(params) + + # If serialization failed, then this will throw an error + pim2 = deepcopy(pim) + self.assertEqual(pim, pim2) + + def test_PreintegratedCombinedMeasurements(self): + """ + Test the serialization of `PreintegratedCombinedMeasurements` by performing a deepcopy. + """ + params = gtsam.PreintegrationCombinedParams(np.asarray([0, 0, -9.81])) + pim = gtsam.PreintegratedCombinedMeasurements(params) + + # If serialization failed, then this will throw an error + pim2 = deepcopy(pim) + self.assertEqual(pim, pim2) + + +if __name__ == "__main__": + unittest.main() From dfcbba822e673fa60527409cf33db7b8731cf08b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 17 Dec 2024 09:20:33 -0500 Subject: [PATCH 108/112] move default AHRSFactor constructor to public for wrapper --- gtsam/navigation/AHRSFactor.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h index 05439bafc..4289b4969 100644 --- a/gtsam/navigation/AHRSFactor.h +++ b/gtsam/navigation/AHRSFactor.h @@ -139,9 +139,6 @@ class GTSAM_EXPORT AHRSFactor: public NoiseModelFactorN { PreintegratedAhrsMeasurements _PIM_; - /** Default constructor - only use for serialization */ - AHRSFactor() {} - public: // Provide access to the Matrix& version of evaluateError: @@ -154,6 +151,9 @@ public: typedef std::shared_ptr shared_ptr; #endif + /** Default constructor - only use for serialization */ + AHRSFactor() {} + /** * Constructor * @param rot_i previous rot key From 8f77bfa13b9688358abc476f467afdb3c9940493 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 17 Dec 2024 09:22:47 -0500 Subject: [PATCH 109/112] serialize navigation factors --- gtsam/navigation/navigation.i | 29 +++++++++++++++++++++++- python/gtsam/tests/test_Serialization.py | 13 +++++++++++ 2 files changed, 41 insertions(+), 1 deletion(-) diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i index 3e7aa0fd6..adb8bf2bb 100644 --- a/gtsam/navigation/navigation.i +++ b/gtsam/navigation/navigation.i @@ -151,6 +151,9 @@ virtual class ImuFactor: gtsam::NonlinearFactor { gtsam::Vector evaluateError(const gtsam::Pose3& pose_i, gtsam::Vector vel_i, const gtsam::Pose3& pose_j, gtsam::Vector vel_j, const gtsam::imuBias::ConstantBias& bias); + + // enable serialization functionality + void serialize() const; }; #include @@ -206,7 +209,7 @@ class PreintegratedCombinedMeasurements { gtsam::NavState predict(const gtsam::NavState& state_i, const gtsam::imuBias::ConstantBias& bias) const; - // enabling serialization functionality + // enable serialization functionality void serialize() const; }; @@ -221,6 +224,9 @@ virtual class CombinedImuFactor: gtsam::NoiseModelFactor { const gtsam::Pose3& pose_j, gtsam::Vector vel_j, const gtsam::imuBias::ConstantBias& bias_i, const gtsam::imuBias::ConstantBias& bias_j); + + // enable serialization functionality + void serialize() const; }; #include @@ -247,6 +253,9 @@ class PreintegratedAhrsMeasurements { // Standard Interface void integrateMeasurement(gtsam::Vector measuredOmega, double deltaT); void resetIntegration() ; + + // enable serialization functionality + void serialize() const; }; virtual class AHRSFactor : gtsam::NonlinearFactor { @@ -263,6 +272,9 @@ virtual class AHRSFactor : gtsam::NonlinearFactor { gtsam::Rot3 predict(const gtsam::Rot3& rot_i, gtsam::Vector bias, const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, gtsam::Vector omegaCoriolis) const; + + // enable serialization functionality + void serialize() const; }; #include @@ -276,6 +288,9 @@ virtual class Rot3AttitudeFactor : gtsam::NoiseModelFactor { bool equals(const gtsam::NonlinearFactor& expected, double tol) const; gtsam::Unit3 nRef() const; gtsam::Unit3 bMeasured() const; + + // enable serialization functionality + void serialize() const; }; virtual class Pose3AttitudeFactor : gtsam::NoiseModelFactor { @@ -290,6 +305,9 @@ virtual class Pose3AttitudeFactor : gtsam::NoiseModelFactor { bool equals(const gtsam::NonlinearFactor& expected, double tol) const; gtsam::Unit3 nRef() const; gtsam::Unit3 bMeasured() const; + + // enable serialization functionality + void serialize() const; }; #include @@ -304,6 +322,9 @@ virtual class GPSFactor : gtsam::NonlinearFactor{ // Standard Interface gtsam::Point3 measurementIn() const; + + // enable serialization functionality + void serialize() const; }; virtual class GPSFactor2 : gtsam::NonlinearFactor { @@ -317,6 +338,9 @@ virtual class GPSFactor2 : gtsam::NonlinearFactor { // Standard Interface gtsam::Point3 measurementIn() const; + + // enable serialization functionality + void serialize() const; }; #include @@ -334,6 +358,9 @@ virtual class BarometricFactor : gtsam::NonlinearFactor { const double& measurementIn() const; double heightOut(double n) const; double baroOut(const double& meters) const; + + // enable serialization functionality + void serialize() const; }; #include diff --git a/python/gtsam/tests/test_Serialization.py b/python/gtsam/tests/test_Serialization.py index be8dd7bec..3e22e6603 100644 --- a/python/gtsam/tests/test_Serialization.py +++ b/python/gtsam/tests/test_Serialization.py @@ -12,6 +12,7 @@ import unittest from copy import deepcopy import numpy as np +from gtsam.symbol_shorthand import B, V, X from gtsam.utils.test_case import GtsamTestCase import gtsam @@ -31,6 +32,18 @@ class TestSerialization(GtsamTestCase): pim2 = deepcopy(pim) self.assertEqual(pim, pim2) + def test_ImuFactor(self): + """ + Test the serialization of `ImuFactor` by performing a deepcopy. + """ + params = gtsam.PreintegrationParams(np.asarray([0, 0, -9.81])) + pim = gtsam.PreintegratedImuMeasurements(params) + imu_odom = gtsam.ImuFactor(X(0), V(0), X(1), V(1), B(0), pim) + + # If serialization failed, then this will throw an error + imu_odom2 = deepcopy(imu_odom) + self.assertEqual(imu_odom, imu_odom2) + def test_PreintegratedCombinedMeasurements(self): """ Test the serialization of `PreintegratedCombinedMeasurements` by performing a deepcopy. From 60b00ebdda6da38bf26c35d112cee6d964bd5da8 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 17 Dec 2024 10:08:19 -0500 Subject: [PATCH 110/112] improve testing for deep copy --- python/gtsam/tests/test_Serialization.py | 30 ++++++++++-------------- python/gtsam/tests/test_pickle.py | 10 ++++---- python/gtsam/utils/test_case.py | 16 +++++++++++-- 3 files changed, 32 insertions(+), 24 deletions(-) diff --git a/python/gtsam/tests/test_Serialization.py b/python/gtsam/tests/test_Serialization.py index 3e22e6603..e935f1f62 100644 --- a/python/gtsam/tests/test_Serialization.py +++ b/python/gtsam/tests/test_Serialization.py @@ -5,11 +5,11 @@ All Rights Reserved See LICENSE for the license information -KalmanFilter unit tests. -Author: Frank Dellaert & Duy Nguyen Ta (Python) +Serialization and deep copy tests. + +Author: Varun Agrawal """ import unittest -from copy import deepcopy import numpy as np from gtsam.symbol_shorthand import B, V, X @@ -18,42 +18,36 @@ from gtsam.utils.test_case import GtsamTestCase import gtsam -class TestSerialization(GtsamTestCase): - """Tests for serialization of various GTSAM objects.""" +class TestDeepCopy(GtsamTestCase): + """Tests for deep copy of various GTSAM objects.""" def test_PreintegratedImuMeasurements(self): """ - Test the serialization of `PreintegratedImuMeasurements` by performing a deepcopy. + Test the deep copy of `PreintegratedImuMeasurements` by performing a deepcopy. """ params = gtsam.PreintegrationParams(np.asarray([0, 0, -9.81])) pim = gtsam.PreintegratedImuMeasurements(params) - # If serialization failed, then this will throw an error - pim2 = deepcopy(pim) - self.assertEqual(pim, pim2) + self.assertDeepCopyEquality(pim) def test_ImuFactor(self): """ - Test the serialization of `ImuFactor` by performing a deepcopy. + Test the deep copy of `ImuFactor` by performing a deepcopy. """ params = gtsam.PreintegrationParams(np.asarray([0, 0, -9.81])) pim = gtsam.PreintegratedImuMeasurements(params) - imu_odom = gtsam.ImuFactor(X(0), V(0), X(1), V(1), B(0), pim) + imu_factor = gtsam.ImuFactor(X(0), V(0), X(1), V(1), B(0), pim) - # If serialization failed, then this will throw an error - imu_odom2 = deepcopy(imu_odom) - self.assertEqual(imu_odom, imu_odom2) + self.assertDeepCopyEquality(imu_factor) def test_PreintegratedCombinedMeasurements(self): """ - Test the serialization of `PreintegratedCombinedMeasurements` by performing a deepcopy. + Test the deep copy of `PreintegratedCombinedMeasurements` by performing a deepcopy. """ params = gtsam.PreintegrationCombinedParams(np.asarray([0, 0, -9.81])) pim = gtsam.PreintegratedCombinedMeasurements(params) - # If serialization failed, then this will throw an error - pim2 = deepcopy(pim) - self.assertEqual(pim, pim2) + self.assertDeepCopyEquality(pim) if __name__ == "__main__": diff --git a/python/gtsam/tests/test_pickle.py b/python/gtsam/tests/test_pickle.py index a6a5745bc..e51617b00 100644 --- a/python/gtsam/tests/test_pickle.py +++ b/python/gtsam/tests/test_pickle.py @@ -9,24 +9,26 @@ Unit tests to check pickling. Author: Ayush Baid """ -from gtsam import Cal3Bundler, PinholeCameraCal3Bundler, Point2, Point3, Pose3, Rot3, SfmTrack, Unit3 - from gtsam.utils.test_case import GtsamTestCase +from gtsam import (Cal3Bundler, PinholeCameraCal3Bundler, Point2, Point3, + Pose3, Rot3, SfmTrack, Unit3) + + class TestPickle(GtsamTestCase): """Tests pickling on some of the classes.""" def test_cal3Bundler_roundtrip(self): obj = Cal3Bundler(fx=100, k1=0.1, k2=0.2, u0=100, v0=70) self.assertEqualityOnPickleRoundtrip(obj) - + def test_pinholeCameraCal3Bundler_roundtrip(self): obj = PinholeCameraCal3Bundler( Pose3(Rot3.RzRyRx(0, 0.1, -0.05), Point3(1, 1, 0)), Cal3Bundler(fx=100, k1=0.1, k2=0.2, u0=100, v0=70), ) self.assertEqualityOnPickleRoundtrip(obj) - + def test_rot3_roundtrip(self): obj = Rot3.RzRyRx(0, 0.05, 0.1) self.assertEqualityOnPickleRoundtrip(obj) diff --git a/python/gtsam/utils/test_case.py b/python/gtsam/utils/test_case.py index 50af004f4..74eaff1db 100644 --- a/python/gtsam/utils/test_case.py +++ b/python/gtsam/utils/test_case.py @@ -10,6 +10,7 @@ Author: Frank Dellaert """ import pickle import unittest +from copy import deepcopy class GtsamTestCase(unittest.TestCase): @@ -28,8 +29,8 @@ class GtsamTestCase(unittest.TestCase): else: equal = actual.equals(expected, tol) if not equal: - raise self.failureException( - "Values are not equal:\n{}!={}".format(actual, expected)) + raise self.failureException("Values are not equal:\n{}!={}".format( + actual, expected)) def assertEqualityOnPickleRoundtrip(self, obj: object, tol=1e-9) -> None: """ Performs a round-trip using pickle and asserts equality. @@ -41,3 +42,14 @@ class GtsamTestCase(unittest.TestCase): """ roundTripObj = pickle.loads(pickle.dumps(obj)) self.gtsamAssertEquals(roundTripObj, obj) + + def assertDeepCopyEquality(self, obj): + """Perform assertion by checking if a + deep copied version of `obj` is equal to itself. + + Args: + obj: The object to check is deep-copyable. + """ + # If deep copy failed, then this will throw an error + obj2 = deepcopy(obj) + self.gtsamAssertEquals(obj, obj2) From 8a30aacf6ca997df0073d633cb3eaa4ff0a5510b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 21 Dec 2024 13:10:29 -0500 Subject: [PATCH 111/112] fix serialization of ImuFactor --- python/gtsam/tests/test_Serialization.py | 26 ++++++++++++++++-------- 1 file changed, 18 insertions(+), 8 deletions(-) diff --git a/python/gtsam/tests/test_Serialization.py b/python/gtsam/tests/test_Serialization.py index e935f1f62..4f5c57b0a 100644 --- a/python/gtsam/tests/test_Serialization.py +++ b/python/gtsam/tests/test_Serialization.py @@ -23,26 +23,36 @@ class TestDeepCopy(GtsamTestCase): def test_PreintegratedImuMeasurements(self): """ - Test the deep copy of `PreintegratedImuMeasurements` by performing a deepcopy. + Test the deep copy of `PreintegratedImuMeasurements`. """ - params = gtsam.PreintegrationParams(np.asarray([0, 0, -9.81])) + params = gtsam.PreintegrationParams.MakeSharedD(9.81) pim = gtsam.PreintegratedImuMeasurements(params) self.assertDeepCopyEquality(pim) def test_ImuFactor(self): """ - Test the deep copy of `ImuFactor` by performing a deepcopy. + Test the deep copy of `ImuFactor`. """ - params = gtsam.PreintegrationParams(np.asarray([0, 0, -9.81])) - pim = gtsam.PreintegratedImuMeasurements(params) - imu_factor = gtsam.ImuFactor(X(0), V(0), X(1), V(1), B(0), pim) + params = gtsam.PreintegrationParams.MakeSharedD(9.81) + params.setAccelerometerCovariance(1e-7 * np.eye(3)) + params.setGyroscopeCovariance(1e-8 * np.eye(3)) + params.setIntegrationCovariance(1e-9 * np.eye(3)) + priorBias = gtsam.imuBias.ConstantBias(np.zeros(3), np.zeros(3)) + pim = gtsam.PreintegratedImuMeasurements(params, priorBias) - self.assertDeepCopyEquality(imu_factor) + # Preintegrate a single measurement for serialization to work. + pim.integrateMeasurement(measuredAcc=np.zeros(3), + measuredOmega=np.zeros(3), + deltaT=0.005) + + factor = gtsam.ImuFactor(0, 1, 2, 3, 4, pim) + + self.assertDeepCopyEquality(factor) def test_PreintegratedCombinedMeasurements(self): """ - Test the deep copy of `PreintegratedCombinedMeasurements` by performing a deepcopy. + Test the deep copy of `PreintegratedCombinedMeasurements`. """ params = gtsam.PreintegrationCombinedParams(np.asarray([0, 0, -9.81])) pim = gtsam.PreintegratedCombinedMeasurements(params) From 60506e2440e825fb7f41ef2331c1993df512ccd3 Mon Sep 17 00:00:00 2001 From: Matt Date: Sun, 22 Dec 2024 09:25:06 -0800 Subject: [PATCH 112/112] [cephes] use GTSAM_LIBRARY_TYPE --- gtsam/3rdparty/cephes/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/3rdparty/cephes/CMakeLists.txt b/gtsam/3rdparty/cephes/CMakeLists.txt index 190b73db9..aeac7b403 100644 --- a/gtsam/3rdparty/cephes/CMakeLists.txt +++ b/gtsam/3rdparty/cephes/CMakeLists.txt @@ -89,7 +89,7 @@ set(CEPHES_SOURCES cephes/zetac.c) # Add library source files -add_library(cephes-gtsam SHARED ${CEPHES_SOURCES}) +add_library(cephes-gtsam ${GTSAM_LIBRARY_TYPE} ${CEPHES_SOURCES}) # Add include directory (aka headers) target_include_directories(