From 0f951643bd1395a11068a6132f8e773d40c2d268 Mon Sep 17 00:00:00 2001 From: Grady Williams Date: Tue, 15 Feb 2022 13:52:12 -0800 Subject: [PATCH 001/206] Adding failing tests for ISAM2 marginalization --- tests/testGaussianISAM2.cpp | 150 ++++++++++++++++++++++++++++++++++++ 1 file changed, 150 insertions(+) diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index e8e916f04..efe34ee31 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -11,6 +11,7 @@ #include #include #include +#include #include #include #include @@ -662,6 +663,76 @@ namespace { bool ok = treeEqual && /*linEqual &&*/ nonlinEqual && /*linCorrect &&*/ /*afterLinCorrect &&*/ afterNonlinCorrect; return ok; } + + boost::optional> createOrderingConstraints(const ISAM2& isam, const KeyVector& newKeys, const KeySet& marginalizableKeys) + { + if (marginalizableKeys.empty()) { + return boost::none; + } else { + FastMap constrainedKeys = FastMap(); + // Generate ordering constraints so that the marginalizable variables will be eliminated first + // Set all existing and new variables to Group1 + for (const auto& key_val : isam.getDelta()) { + constrainedKeys.emplace(key_val.first, 1); + } + for (const auto& key : newKeys) { + constrainedKeys.emplace(key, 1); + } + // And then re-assign the marginalizable variables to Group0 so that they'll all be leaf nodes + for (const auto& key : marginalizableKeys) { + constrainedKeys.at(key) = 0; + } + return constrainedKeys; + } + } + + void markAffectedKeys(const Key& key, const ISAM2Clique::shared_ptr& rootClique, KeyList& additionalKeys) + { + std::stack frontier; + frontier.push(rootClique); + // Basic DFS to find additional keys + while (!frontier.empty()) { + // Get the top of the stack + const ISAM2Clique::shared_ptr clique = frontier.top(); + frontier.pop(); + // Check if we have more keys and children to add + if (std::find(clique->conditional()->beginParents(), clique->conditional()->endParents(), key) != + clique->conditional()->endParents()) { + for (Key i : clique->conditional()->frontals()) { + additionalKeys.push_back(i); + } + for (const ISAM2Clique::shared_ptr& child : clique->children) { + frontier.push(child); + } + } + } + } + + bool updateAndMarginalize(const NonlinearFactorGraph& newFactors, const Values& newValues, const KeySet& marginalizableKeys, ISAM2& isam) + { + // Force ISAM2 to put marginalizable variables at the beginning + const boost::optional> orderingConstraints = createOrderingConstraints(isam, newValues.keys(), marginalizableKeys); + + // Mark additional keys between the marginalized keys and the leaves + KeyList additionalMarkedKeys; + for (Key key : marginalizableKeys) { + ISAM2Clique::shared_ptr clique = isam[key]; + for (const ISAM2Clique::shared_ptr& child : clique->children) { + markAffectedKeys(key, child, additionalMarkedKeys); + } + } + + // Update + isam.update(newFactors, newValues, FactorIndices{}, orderingConstraints, boost::none, additionalMarkedKeys); + + if (!marginalizableKeys.empty()) { + FastList leafKeys(marginalizableKeys.begin(), marginalizableKeys.end()); + return checkMarginalizeLeaves(isam, leafKeys); + } + else { + return true; + } + } } /* ************************************************************************* */ @@ -795,6 +866,85 @@ TEST(ISAM2, marginalizeLeaves5) EXPECT(checkMarginalizeLeaves(isam, marginalizeKeys)); } +/* ************************************************************************* */ +TEST(ISAM2, marginalizeLeaves6) +{ + const boost::shared_ptr nm = noiseModel::Isotropic::Sigma(6, 1.0); + + int gridDim = 10; + + auto idxToKey = [gridDim](int i, int j){return i * gridDim + j;}; + + NonlinearFactorGraph factors; + Values values; + ISAM2 isam; + + // Create a grid of pose variables + for (int i = 0; i < gridDim; ++i) { + for (int j = 0; j < gridDim; ++j) { + Pose3 pose = Pose3(Rot3::identity(), Point3(i, j, 0)); + Key key = idxToKey(i, j); + // Place a prior on the first pose + factors.addPrior(key, Pose3(Rot3::identity(), Point3(i, j, 0)), nm); + values.insert(key, pose); + if (i > 0) { + factors.emplace_shared>(idxToKey(i - 1, j), key, Pose3(Rot3::identity(), Point3(1, 0, 0)),nm); + } + if (j > 0) { + factors.emplace_shared>(idxToKey(i, j - 1), key, Pose3(Rot3::identity(), Point3(0, 1, 0)),nm); + } + } + } + + // Optimize the graph + EXPECT(updateAndMarginalize(factors, values, {}, isam)); + auto estimate = isam.calculateBestEstimate(); + + // Get the list of keys + std::vector key_list(gridDim * gridDim); + std::iota(key_list.begin(), key_list.end(), 0); + + // Shuffle the keys -> we will marginalize the keys one by one in this order + std::shuffle(key_list.begin(), key_list.end(), std::default_random_engine(1234)); + + for (const auto& key : key_list) { + KeySet marginalKeys; + marginalKeys.insert(key); + EXPECT(updateAndMarginalize({}, {}, marginalKeys, isam)); + estimate = isam.calculateBestEstimate(); + } +} + +/* ************************************************************************* */ +TEST(ISAM2, MarginalizeRoot) +{ + const boost::shared_ptr nm = noiseModel::Isotropic::Sigma(6, 1.0); + + NonlinearFactorGraph factors; + Values values; + ISAM2 isam; + + // Create a factor graph with one variable and a prior + Pose3 root = Pose3::identity(); + Key rootKey(0); + values.insert(rootKey, root); + factors.addPrior(rootKey, Pose3::identity(), nm); + + // Optimize the graph + EXPECT(updateAndMarginalize(factors, values, {}, isam)); + auto estimate = isam.calculateBestEstimate(); + EXPECT(estimate.size() == 1); + + // And remove the node from the graph + KeySet marginalizableKeys; + marginalizableKeys.insert(rootKey); + + EXPECT(updateAndMarginalize({}, {}, marginalizableKeys, isam)); + + estimate = isam.calculateBestEstimate(); + EXPECT(estimate.empty()); +} + /* ************************************************************************* */ TEST(ISAM2, marginalCovariance) { From 9e1046f40e21bd66b4db5fe300aa0e46b39365a4 Mon Sep 17 00:00:00 2001 From: Grady Williams Date: Sat, 16 Apr 2022 13:53:39 -0700 Subject: [PATCH 002/206] Test for not increasing graph size when adding marginal factors --- tests/testGaussianISAM2.cpp | 31 +++++++++++++++++++++++++++++++ 1 file changed, 31 insertions(+) diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index efe34ee31..940051b96 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -945,6 +945,37 @@ TEST(ISAM2, MarginalizeRoot) EXPECT(estimate.empty()); } +/* ************************************************************************* */ +TEST(ISAM2, marginalizationSize) +{ + const boost::shared_ptr nm = noiseModel::Isotropic::Sigma(6, 1.0); + + NonlinearFactorGraph factors; + Values values; + ISAM2Params params; + params.findUnusedFactorSlots = true; + ISAM2 isam{params}; + + // Create a pose variable + Key aKey(0); + values.insert(aKey, Pose3::identity()); + factors.addPrior(aKey, Pose3::identity(), nm); + // Create another pose variable linked to the first + Pose3 b = Pose3::identity(); + Key bKey(1); + values.insert(bKey, Pose3::identity()); + factors.emplace_shared>(aKey, bKey, Pose3::identity(), nm); + // Optimize the graph + EXPECT(updateAndMarginalize(factors, values, {}, isam)); + + // Now remove a variable -> we should not see the number of factors increase + gtsam::KeySet to_remove; + to_remove.insert(aKey); + const auto numFactorsBefore = isam.getFactorsUnsafe().size(); + updateAndMarginalize({}, {}, to_remove, isam); + EXPECT(numFactorsBefore == isam.getFactorsUnsafe().size()); +} + /* ************************************************************************* */ TEST(ISAM2, marginalCovariance) { From 66720e0b0276360d1581335332714d2c7e1ecc93 Mon Sep 17 00:00:00 2001 From: Grady Williams Date: Sat, 16 Apr 2022 13:29:53 -0700 Subject: [PATCH 003/206] Bugfixes for ISAM2 --- gtsam/nonlinear/ISAM2.cpp | 66 +++++++++++++++++++++++-------------- tests/testGaussianISAM2.cpp | 7 ++-- 2 files changed, 45 insertions(+), 28 deletions(-) diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index f56b23777..a7feca239 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -552,9 +552,12 @@ void ISAM2::marginalizeLeaves( // We do not need the marginal factors associated with this clique // because their information is already incorporated in the new // marginal factor. So, now associate this marginal factor with the - // parent of this clique. - marginalFactors[clique->parent()->conditional()->front()].push_back( - marginalFactor); + // parent of this clique. If the clique is a root and has no parent, then + // we can discard it without keeping track of the marginal factor. + if (clique->parent()) { + marginalFactors[clique->parent()->conditional()->front()].push_back( + marginalFactor); + } // Now remove this clique and its subtree - all of its marginal // information has been stored in marginalFactors. trackingRemoveSubtree(clique); @@ -632,7 +635,7 @@ void ISAM2::marginalizeLeaves( // Make the clique's matrix appear as a subset const DenseIndex dimToRemove = cg->matrixObject().offset(nToRemove); - cg->matrixObject().firstBlock() = nToRemove; + cg->matrixObject().firstBlock() += nToRemove; cg->matrixObject().rowStart() = dimToRemove; // Change the keys in the clique @@ -658,42 +661,55 @@ void ISAM2::marginalizeLeaves( // At this point we have updated the BayesTree, now update the remaining iSAM2 // data structures + // Remove the factors to remove that will be summarized in marginal factors + NonlinearFactorGraph removedFactors; + for (const auto index : factorIndicesToRemove) { + removedFactors.push_back(nonlinearFactors_[index]); + nonlinearFactors_.remove(index); + if (params_.cacheLinearizedFactors) { + linearFactors_.remove(index); + } + } + variableIndex_.remove(factorIndicesToRemove.begin(), + factorIndicesToRemove.end(), removedFactors); + // Gather factors to add - the new marginal factors - GaussianFactorGraph factorsToAdd; + GaussianFactorGraph factorsToAdd{}; + NonlinearFactorGraph nonlinearFactorsToAdd{}; for (const auto& key_factors : marginalFactors) { for (const auto& factor : key_factors.second) { if (factor) { factorsToAdd.push_back(factor); - if (marginalFactorsIndices) - marginalFactorsIndices->push_back(nonlinearFactors_.size()); - nonlinearFactors_.push_back( - boost::make_shared(factor)); - if (params_.cacheLinearizedFactors) linearFactors_.push_back(factor); + nonlinearFactorsToAdd.emplace_shared(factor); for (Key factorKey : *factor) { fixedVariables_.insert(factorKey); } } } } - variableIndex_.augment(factorsToAdd); // Augment the variable index - - // Remove the factors to remove that have been summarized in the newly-added - // marginal factors - NonlinearFactorGraph removedFactors; - for (const auto index : factorIndicesToRemove) { - removedFactors.push_back(nonlinearFactors_[index]); - nonlinearFactors_.remove(index); - if (params_.cacheLinearizedFactors) linearFactors_.remove(index); + // Add the nonlinear factors and keep track of the new factor indices + auto newFactorIndices = nonlinearFactors_.add_factors(nonlinearFactorsToAdd, + params_.findUnusedFactorSlots); + // Add cached linear factors. + if (params_.cacheLinearizedFactors){ + linearFactors_.resize(nonlinearFactors_.size()); + for (std::size_t i = 0; i < nonlinearFactorsToAdd.size(); ++i){ + linearFactors_[newFactorIndices[i]] = factorsToAdd[i]; + } } - variableIndex_.remove(factorIndicesToRemove.begin(), - factorIndicesToRemove.end(), removedFactors); - - if (deletedFactorsIndices) - deletedFactorsIndices->assign(factorIndicesToRemove.begin(), - factorIndicesToRemove.end()); + // Augment the variable index + variableIndex_.augment(factorsToAdd, newFactorIndices); // Remove the marginalized variables removeVariables(KeySet(leafKeys.begin(), leafKeys.end())); + + if (deletedFactorsIndices) { + deletedFactorsIndices->assign(factorIndicesToRemove.begin(), + factorIndicesToRemove.end()); + } + if (marginalFactorsIndices){ + *marginalFactorsIndices = std::move(newFactorIndices); + } } /* ************************************************************************* */ diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 940051b96..b2a679d65 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -714,16 +714,17 @@ namespace { const boost::optional> orderingConstraints = createOrderingConstraints(isam, newValues.keys(), marginalizableKeys); // Mark additional keys between the marginalized keys and the leaves - KeyList additionalMarkedKeys; + KeyList markedKeys; for (Key key : marginalizableKeys) { + markedKeys.push_back(key); ISAM2Clique::shared_ptr clique = isam[key]; for (const ISAM2Clique::shared_ptr& child : clique->children) { - markAffectedKeys(key, child, additionalMarkedKeys); + markAffectedKeys(key, child, markedKeys); } } // Update - isam.update(newFactors, newValues, FactorIndices{}, orderingConstraints, boost::none, additionalMarkedKeys); + isam.update(newFactors, newValues, FactorIndices{}, orderingConstraints, boost::none, markedKeys); if (!marginalizableKeys.empty()) { FastList leafKeys(marginalizableKeys.begin(), marginalizableKeys.end()); From 8d4fb8b854a3d83819980896e25516006af9a09b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 23 Dec 2022 12:26:11 +0530 Subject: [PATCH 004/206] fix doxygen issues --- gtsam/base/treeTraversal-inst.h | 6 ++++-- gtsam/linear/LossFunctions.h | 26 +++++++++++++------------- 2 files changed, 17 insertions(+), 15 deletions(-) diff --git a/gtsam/base/treeTraversal-inst.h b/gtsam/base/treeTraversal-inst.h index be45a248e..0578143f0 100644 --- a/gtsam/base/treeTraversal-inst.h +++ b/gtsam/base/treeTraversal-inst.h @@ -138,6 +138,8 @@ void DepthFirstForest(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre) { /** Traverse a forest depth-first with pre-order and post-order visits. * @param forest The forest of trees to traverse. The method \c forest.roots() should exist * and return a collection of (shared) pointers to \c FOREST::Node. + * @param rootData The data to pass by reference to \c visitorPre when it is called on each + * root node. * @param visitorPre \c visitorPre(node, parentData) will be called at every node, before * visiting its children, and will be passed, by reference, the \c DATA object returned * by the visit to its parent. Likewise, \c visitorPre should return the \c DATA object @@ -147,8 +149,8 @@ void DepthFirstForest(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre) { * @param visitorPost \c visitorPost(node, data) will be called at every node, after visiting * its children, and will be passed, by reference, the \c DATA object returned by the * call to \c visitorPre (the \c DATA object may be modified by visiting the children). - * @param rootData The data to pass by reference to \c visitorPre when it is called on each - * root node. */ + * @param problemSizeThreshold + */ template void DepthFirstForestParallel(FOREST& forest, DATA& rootData, diff --git a/gtsam/linear/LossFunctions.h b/gtsam/linear/LossFunctions.h index d9cfc1f3c..e227d7162 100644 --- a/gtsam/linear/LossFunctions.h +++ b/gtsam/linear/LossFunctions.h @@ -89,7 +89,7 @@ class GTSAM_EXPORT Base { * functions. It would be better for this function to accept the vector and * internally call the norm if necessary. * - * This returns \rho(x) in \ref mEstimator + * This returns \f$\rho(x)\f$ in \ref mEstimator */ virtual double loss(double distance) const { return 0; } @@ -141,9 +141,9 @@ class GTSAM_EXPORT Base { * * This model has no additional parameters. * - * - Loss \rho(x) = 0.5 x² - * - Derivative \phi(x) = x - * - Weight w(x) = \phi(x)/x = 1 + * - Loss \f$ \rho(x) = 0.5 x² \f$ + * - Derivative \f$ \phi(x) = x \f$ + * - Weight \f$ w(x) = \phi(x)/x = 1 \f$ */ class GTSAM_EXPORT Null : public Base { public: @@ -275,9 +275,9 @@ class GTSAM_EXPORT Cauchy : public Base { * * This model has a scalar parameter "c". * - * - Loss \rho(x) = c² (1 - (1-x²/c²)³)/6 if |x|k, (k+x) if x<-k - * - Weight w(x) = \phi(x)/x = 0 if |x|k, (k+x)/x if x<-k + * - Loss \f$ \rho(x) = 0 \f$ if |x|k, (k+x) if x<-k + * - Weight \f$ w(x) = \phi(x)/x = 0 \f$ if |x|k, (k+x)/x if x<-k */ class GTSAM_EXPORT L2WithDeadZone : public Base { protected: From 242728df02ddde9604f4a54598a4facfa6c0f87f Mon Sep 17 00:00:00 2001 From: Daniel Lu Date: Tue, 18 Apr 2023 20:38:12 -0700 Subject: [PATCH 005/206] add useLOST to triangulateSafe --- gtsam/geometry/geometry.i | 2 ++ gtsam/geometry/triangulation.h | 10 +++++++++- 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index e9929227a..a7fca34d1 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -1146,11 +1146,13 @@ class TriangulationParameters { bool enableEPI; double landmarkDistanceThreshold; double dynamicOutlierRejectionThreshold; + bool useLOST; gtsam::SharedNoiseModel noiseModel; TriangulationParameters(const double rankTolerance = 1.0, const bool enableEPI = false, double landmarkDistanceThreshold = -1, double dynamicOutlierRejectionThreshold = -1, + const bool useLOST = false, const gtsam::SharedNoiseModel& noiseModel = nullptr); }; diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 7e58cee2d..d8578947c 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -571,6 +571,11 @@ struct GTSAM_EXPORT TriangulationParameters { */ double dynamicOutlierRejectionThreshold; + /** + * if true, will use the LOST algorithm instead of DLT + */ + bool useLOST; + SharedNoiseModel noiseModel; ///< used in the nonlinear triangulation /** @@ -585,10 +590,12 @@ struct GTSAM_EXPORT TriangulationParameters { TriangulationParameters(const double _rankTolerance = 1.0, const bool _enableEPI = false, double _landmarkDistanceThreshold = -1, double _dynamicOutlierRejectionThreshold = -1, + const bool _useLOST = false, const SharedNoiseModel& _noiseModel = nullptr) : rankTolerance(_rankTolerance), enableEPI(_enableEPI), // landmarkDistanceThreshold(_landmarkDistanceThreshold), // dynamicOutlierRejectionThreshold(_dynamicOutlierRejectionThreshold), + useLOST(_useLOST), noiseModel(_noiseModel){ } @@ -601,6 +608,7 @@ struct GTSAM_EXPORT TriangulationParameters { << std::endl; os << "dynamicOutlierRejectionThreshold = " << p.dynamicOutlierRejectionThreshold << std::endl; + os << "useLOST = " << p.useLOST << std::endl; os << "noise model" << std::endl; return os; } @@ -698,7 +706,7 @@ TriangulationResult triangulateSafe(const CameraSet& cameras, try { Point3 point = triangulatePoint3(cameras, measured, params.rankTolerance, - params.enableEPI, params.noiseModel); + params.enableEPI, params.noiseModel, params.useLOST); // Check landmark distance and re-projection errors to avoid outliers size_t i = 0; From 3f30ac2aefec137eaee9d3e2d21541172611f751 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 25 Apr 2023 15:01:57 -0400 Subject: [PATCH 006/206] Add missing option in the CMake for Pose2 --- cmake/HandleGeneralOptions.cmake | 1 + gtsam/config.h.in | 2 ++ gtsam/geometry/Pose2.cpp | 4 ++-- gtsam/geometry/geometry.i | 7 +++++++ gtsam/geometry/tests/testPose2.cpp | 4 ++-- 5 files changed, 14 insertions(+), 4 deletions(-) diff --git a/cmake/HandleGeneralOptions.cmake b/cmake/HandleGeneralOptions.cmake index 4a4f1a36e..eab40d16b 100644 --- a/cmake/HandleGeneralOptions.cmake +++ b/cmake/HandleGeneralOptions.cmake @@ -30,6 +30,7 @@ option(GTSAM_ALLOW_DEPRECATED_SINCE_V43 "Allow use of methods/functions depr option(GTSAM_SUPPORT_NESTED_DISSECTION "Support Metis-based nested dissection" ON) option(GTSAM_TANGENT_PREINTEGRATION "Use new ImuFactor with integration on tangent space" ON) option(GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR "Use the slower but correct version of BetweenFactor" OFF) +option(GTSAM_SLOW_BUT_CORRECT_EXPMAP "Use slower but correct expmap" ON) if (GTSAM_FORCE_SHARED_LIB) message(STATUS "GTSAM is a shared library due to GTSAM_FORCE_SHARED_LIB") diff --git a/gtsam/config.h.in b/gtsam/config.h.in index 7f8936d1e..ec06d90c9 100644 --- a/gtsam/config.h.in +++ b/gtsam/config.h.in @@ -83,3 +83,5 @@ // Toggle switch for BetweenFactor jacobian computation #cmakedefine GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR + +#cmakedefine GTSAM_SLOW_BUT_CORRECT_EXPMAP diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 0d9f1bc01..05678dc27 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -97,7 +97,7 @@ Vector3 Pose2::Logmap(const Pose2& p, OptionalJacobian<3, 3> H) { /* ************************************************************************* */ Pose2 Pose2::ChartAtOrigin::Retract(const Vector3& v, ChartJacobian H) { -#ifdef SLOW_BUT_CORRECT_EXPMAP +#ifdef GTSAM_SLOW_BUT_CORRECT_EXPMAP return Expmap(v, H); #else if (H) { @@ -109,7 +109,7 @@ Pose2 Pose2::ChartAtOrigin::Retract(const Vector3& v, ChartJacobian H) { } /* ************************************************************************* */ Vector3 Pose2::ChartAtOrigin::Local(const Pose2& r, ChartJacobian H) { -#ifdef SLOW_BUT_CORRECT_EXPMAP +#ifdef GTSAM_SLOW_BUT_CORRECT_EXPMAP return Logmap(r, H); #else if (H) { diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index e9929227a..480655cc3 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -159,7 +159,9 @@ class Rot2 { // Manifold gtsam::Rot2 retract(Vector v) const; + gtsam::Rot2 retract(Vector v, Eigen::Ref H1, Eigen::Ref H2) const; Vector localCoordinates(const gtsam::Rot2& p) const; + Vector localCoordinates(const gtsam::Rot2& p, Eigen::Ref H1, Eigen::Ref H2) const; // Lie Group static gtsam::Rot2 Expmap(Vector v); @@ -387,19 +389,24 @@ class Pose2 { static gtsam::Pose2 Identity(); gtsam::Pose2 inverse() const; gtsam::Pose2 compose(const gtsam::Pose2& p2) const; + gtsam::Pose2 compose(const gtsam::Pose2& p2, Eigen::Ref H1, Eigen::Ref H2) const; gtsam::Pose2 between(const gtsam::Pose2& p2) const; + gtsam::Pose2 between(const gtsam::Pose2& p2, Eigen::Ref H1, Eigen::Ref H2) const; // Operator Overloads gtsam::Pose2 operator*(const gtsam::Pose2& p2) const; // Manifold gtsam::Pose2 retract(Vector v) const; + gtsam::Pose2 retract(Vector v, Eigen::Ref H1, Eigen::Ref H2) const; Vector localCoordinates(const gtsam::Pose2& p) const; + Vector localCoordinates(const gtsam::Pose2& p, Eigen::Ref H1, Eigen::Ref H2) const; // Lie Group static gtsam::Pose2 Expmap(Vector v); static Vector Logmap(const gtsam::Pose2& p); Vector logmap(const gtsam::Pose2& p); + Vector logmap(const gtsam::Pose2& p, Eigen::Ref H); static Matrix ExpmapDerivative(Vector v); static Matrix LogmapDerivative(const gtsam::Pose2& v); Matrix AdjointMap() const; diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index 8c1bdccc0..56b8a779a 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -66,7 +66,7 @@ TEST(Pose2, manifold) { /* ************************************************************************* */ TEST(Pose2, retract) { Pose2 pose(M_PI/2.0, Point2(1, 2)); -#ifdef SLOW_BUT_CORRECT_EXPMAP +#ifdef GTSAM_SLOW_BUT_CORRECT_EXPMAP Pose2 expected(1.00811, 2.01528, 2.5608); #else Pose2 expected(M_PI/2.0+0.99, Point2(1.015, 2.01)); @@ -204,7 +204,7 @@ TEST(Pose2, Adjoint_hat) { TEST(Pose2, logmap) { Pose2 pose0(M_PI/2.0, Point2(1, 2)); Pose2 pose(M_PI/2.0+0.018, Point2(1.015, 2.01)); -#ifdef SLOW_BUT_CORRECT_EXPMAP +#ifdef GTSAM_SLOW_BUT_CORRECT_EXPMAP Vector3 expected(0.00986473, -0.0150896, 0.018); #else Vector3 expected(0.01, -0.015, 0.018); From b0404aa109eb463c0764b4edded82fd3ab8f513a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 20 Jun 2023 14:36:45 -0400 Subject: [PATCH 007/206] small improvements --- gtsam_unstable/linear/LPInitSolver.cpp | 4 +--- gtsam_unstable/linear/LPInitSolver.h | 3 ++- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/gtsam_unstable/linear/LPInitSolver.cpp b/gtsam_unstable/linear/LPInitSolver.cpp index 9f12d670e..3d24f784b 100644 --- a/gtsam_unstable/linear/LPInitSolver.cpp +++ b/gtsam_unstable/linear/LPInitSolver.cpp @@ -65,9 +65,7 @@ GaussianFactorGraph::shared_ptr LPInitSolver::buildInitOfInitGraph() const { new GaussianFactorGraph(lp_.equalities)); // create factor ||x||^2 and add to the graph - const KeyDimMap& constrainedKeyDim = lp_.constrainedKeyDimMap(); - for (const auto& [key, _] : constrainedKeyDim) { - size_t dim = constrainedKeyDim.at(key); + for (const auto& [key, dim] : lp_.constrainedKeyDimMap()) { initGraph->push_back( JacobianFactor(key, Matrix::Identity(dim, dim), Vector::Zero(dim))); } diff --git a/gtsam_unstable/linear/LPInitSolver.h b/gtsam_unstable/linear/LPInitSolver.h index 7e326117b..9db2a34f0 100644 --- a/gtsam_unstable/linear/LPInitSolver.h +++ b/gtsam_unstable/linear/LPInitSolver.h @@ -19,6 +19,7 @@ #pragma once +#include #include #include @@ -49,7 +50,7 @@ namespace gtsam { * inequality constraint, we can't conclude that the problem is infeasible. * However, whether it is infeasible or unbounded, we don't have a unique solution anyway. */ -class LPInitSolver { +class GTSAM_UNSTABLE_EXPORT LPInitSolver { private: const LP& lp_; From 254e3128e6d199508215ec71cb8626fdadafc45f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 20 Jun 2023 17:28:01 -0400 Subject: [PATCH 008/206] fix for multiply defined symbol error in LPInitSolver --- gtsam_unstable/linear/LP.h | 20 +++++++++++++++++--- 1 file changed, 17 insertions(+), 3 deletions(-) diff --git a/gtsam_unstable/linear/LP.h b/gtsam_unstable/linear/LP.h index dbb744d3e..50065b2dd 100644 --- a/gtsam_unstable/linear/LP.h +++ b/gtsam_unstable/linear/LP.h @@ -81,9 +81,23 @@ public: if (!cachedConstrainedKeyDimMap_.empty()) return cachedConstrainedKeyDimMap_; // Collect key-dim map of all variables in the constraints - cachedConstrainedKeyDimMap_ = collectKeyDim(equalities); - KeyDimMap keysDim2 = collectKeyDim(inequalities); - cachedConstrainedKeyDimMap_.insert(keysDim2.begin(), keysDim2.end()); + //TODO(Varun) seems like the templated function is causing the multiple symbols error on Windows + // cachedConstrainedKeyDimMap_ = collectKeyDim(equalities); + // KeyDimMap keysDim2 = collectKeyDim(inequalities); + // cachedConstrainedKeyDimMap_.insert(keysDim2.begin(), keysDim2.end()); + cachedConstrainedKeyDimMap_.clear(); + for (auto&& factor : equalities) { + if (!factor) continue; + for (Key key : factor->keys()) { + cachedConstrainedKeyDimMap_[key] = factor->getDim(factor->find(key)); + } + } + for (auto&& factor : inequalities) { + if (!factor) continue; + for (Key key : factor->keys()) { + cachedConstrainedKeyDimMap_[key] = factor->getDim(factor->find(key)); + } + } return cachedConstrainedKeyDimMap_; } From 62fe1a937979968d3b8f4349e0a39aa445d13358 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 20 Jun 2023 17:31:49 -0400 Subject: [PATCH 009/206] add todo for error in SmartStereoProjectionFactorPP.h --- .../slam/SmartStereoProjectionFactorPP.h | 42 ++++++++++--------- 1 file changed, 22 insertions(+), 20 deletions(-) diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h index f9db90953..b022ce16f 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h @@ -201,10 +201,9 @@ class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionFactorPP } /// linearize and return a Hessianfactor that is an approximation of error(p) - std::shared_ptr > createHessianFactor( - const Values& values, const double lambda = 0.0, bool diagonalDamping = - false) const { - + std::shared_ptr> createHessianFactor( + const Values& values, const double lambda = 0.0, + bool diagonalDamping = false) const { // we may have multiple cameras sharing the same extrinsic cals, hence the number // of keys may be smaller than 2 * nrMeasurements (which is the upper bound where we // have a body key and an extrinsic calibration key for each measurement) @@ -212,23 +211,22 @@ class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionFactorPP // Create structures for Hessian Factors KeyVector js; - std::vector < Matrix > Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2); + std::vector Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2); std::vector gs(nrUniqueKeys); if (this->measured_.size() != cameras(values).size()) throw std::runtime_error("SmartStereoProjectionHessianFactor: this->" - "measured_.size() inconsistent with input"); + "measured_.size() inconsistent with input"); // triangulate 3D point at given linearization point triangulateSafe(cameras(values)); - if (!result_) { // failed: return "empty/zero" Hessian - for (Matrix& m : Gs) - m = Matrix::Zero(DimPose, DimPose); - for (Vector& v : gs) - v = Vector::Zero(DimPose); - return std::make_shared < RegularHessianFactor - > (keys_, Gs, gs, 0.0); + // failed: return "empty/zero" Hessian + if (!result_) { + for (Matrix& m : Gs) m = Matrix::Zero(DimPose, DimPose); + for (Vector& v : gs) v = Vector::Zero(DimPose); + return std::make_shared>(keys_, Gs, gs, + 0.0); } // compute Jacobian given triangulated 3D Point @@ -239,12 +237,13 @@ class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionFactorPP // Whiten using noise model noiseModel_->WhitenSystem(E, b); - for (size_t i = 0; i < Fs.size(); i++) + for (size_t i = 0; i < Fs.size(); i++) { Fs[i] = noiseModel_->Whiten(Fs[i]); + } // build augmented Hessian (with last row/column being the information vector) Matrix3 P; - Cameras::ComputePointCovariance <3> (P, E, lambda, diagonalDamping); + Cameras::ComputePointCovariance<3>(P, E, lambda, diagonalDamping); // these are the keys that correspond to the blocks in augmentedHessian (output of SchurComplement) KeyVector nonuniqueKeys; @@ -253,12 +252,15 @@ class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionFactorPP nonuniqueKeys.push_back(body_P_cam_keys_.at(i)); } // but we need to get the augumented hessian wrt the unique keys in key_ - SymmetricBlockMatrix augmentedHessianUniqueKeys = - Cameras::SchurComplementAndRearrangeBlocks<3,DimBlock,DimPose>(Fs,E,P,b, - nonuniqueKeys, keys_); + //TODO(Varun) SchurComplementAndRearrangeBlocks is causing the multiply defined symbol error + // SymmetricBlockMatrix augmentedHessianUniqueKeys = + // Base::Cameras::template SchurComplementAndRearrangeBlocks<3, DimBlock, + // DimPose>( + // Fs, E, P, b, nonuniqueKeys, keys_); - return std::make_shared < RegularHessianFactor - > (keys_, augmentedHessianUniqueKeys); + // return std::make_shared>( + // keys_, augmentedHessianUniqueKeys); + return nullptr; } /** From fbf155d91e4ccbf0590fe2f693f50c7da6e528a3 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 21 Jun 2023 09:55:52 -0400 Subject: [PATCH 010/206] undo changes --- gtsam_unstable/linear/LP.h | 20 +++---------------- .../slam/SmartStereoProjectionFactorPP.h | 14 ++++++------- 2 files changed, 9 insertions(+), 25 deletions(-) diff --git a/gtsam_unstable/linear/LP.h b/gtsam_unstable/linear/LP.h index 50065b2dd..dbb744d3e 100644 --- a/gtsam_unstable/linear/LP.h +++ b/gtsam_unstable/linear/LP.h @@ -81,23 +81,9 @@ public: if (!cachedConstrainedKeyDimMap_.empty()) return cachedConstrainedKeyDimMap_; // Collect key-dim map of all variables in the constraints - //TODO(Varun) seems like the templated function is causing the multiple symbols error on Windows - // cachedConstrainedKeyDimMap_ = collectKeyDim(equalities); - // KeyDimMap keysDim2 = collectKeyDim(inequalities); - // cachedConstrainedKeyDimMap_.insert(keysDim2.begin(), keysDim2.end()); - cachedConstrainedKeyDimMap_.clear(); - for (auto&& factor : equalities) { - if (!factor) continue; - for (Key key : factor->keys()) { - cachedConstrainedKeyDimMap_[key] = factor->getDim(factor->find(key)); - } - } - for (auto&& factor : inequalities) { - if (!factor) continue; - for (Key key : factor->keys()) { - cachedConstrainedKeyDimMap_[key] = factor->getDim(factor->find(key)); - } - } + cachedConstrainedKeyDimMap_ = collectKeyDim(equalities); + KeyDimMap keysDim2 = collectKeyDim(inequalities); + cachedConstrainedKeyDimMap_.insert(keysDim2.begin(), keysDim2.end()); return cachedConstrainedKeyDimMap_; } diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h index b022ce16f..189c501bb 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h @@ -252,15 +252,13 @@ class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionFactorPP nonuniqueKeys.push_back(body_P_cam_keys_.at(i)); } // but we need to get the augumented hessian wrt the unique keys in key_ - //TODO(Varun) SchurComplementAndRearrangeBlocks is causing the multiply defined symbol error - // SymmetricBlockMatrix augmentedHessianUniqueKeys = - // Base::Cameras::template SchurComplementAndRearrangeBlocks<3, DimBlock, - // DimPose>( - // Fs, E, P, b, nonuniqueKeys, keys_); + SymmetricBlockMatrix augmentedHessianUniqueKeys = + Base::Cameras::template SchurComplementAndRearrangeBlocks<3, DimBlock, + DimPose>( + Fs, E, P, b, nonuniqueKeys, keys_); - // return std::make_shared>( - // keys_, augmentedHessianUniqueKeys); - return nullptr; + return std::make_shared>( + keys_, augmentedHessianUniqueKeys); } /** From 57311281fbea70ab639a5ad754484c55c0063a4b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 21 Jun 2023 09:56:16 -0400 Subject: [PATCH 011/206] use std::map for Key-Dim maps --- gtsam/geometry/CameraSet.h | 2 +- gtsam_unstable/linear/LP.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index 23a4b467e..5f77ebf58 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -270,7 +270,7 @@ class CameraSet : public std::vector> { // Get map from key to location in the new augmented Hessian matrix (the // one including only unique keys). - std::map keyToSlotMap; + std::map keyToSlotMap; for (size_t k = 0; k < nrUniqueKeys; k++) { keyToSlotMap[hessianKeys[k]] = k; } diff --git a/gtsam_unstable/linear/LP.h b/gtsam_unstable/linear/LP.h index dbb744d3e..53247be4c 100644 --- a/gtsam_unstable/linear/LP.h +++ b/gtsam_unstable/linear/LP.h @@ -29,7 +29,7 @@ namespace gtsam { using namespace std; /// Mapping between variable's key and its corresponding dimensionality -using KeyDimMap = std::map; +using KeyDimMap = std::map; /* * Iterates through every factor in a linear graph and generates a * mapping between every factor key and it's corresponding dimensionality. From 429d5de601db382f3a5f025dc84a03ccc4e9b505 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 21 Jun 2023 12:30:56 -0400 Subject: [PATCH 012/206] Actions file formatting --- .github/workflows/build-windows.yml | 30 ++++++++++++++--------------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index 0434577c1..339f10a0a 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -96,23 +96,23 @@ jobs: run: | # Since Visual Studio is a multi-generator, we need to use --config # https://stackoverflow.com/a/24470998/1236990 - cmake --build build -j 4 --config ${{ matrix.build_type }} --target gtsam - cmake --build build -j 4 --config ${{ matrix.build_type }} --target gtsam_unstable - cmake --build build -j 4 --config ${{ matrix.build_type }} --target wrap + cmake --build build -j4 --config ${{ matrix.build_type }} --target gtsam + cmake --build build -j4 --config ${{ matrix.build_type }} --target gtsam_unstable + cmake --build build -j4 --config ${{ matrix.build_type }} --target wrap # Run GTSAM tests - cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.base - cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.basis - cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.discrete - #cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.geometry - cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.inference - cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.linear - cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.navigation - #cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.nonlinear - #cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.sam - cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.sfm - #cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.slam - cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.symbolic + cmake --build build -j4 --config ${{ matrix.build_type }} --target check.base + cmake --build build -j4 --config ${{ matrix.build_type }} --target check.basis + cmake --build build -j4 --config ${{ matrix.build_type }} --target check.discrete + #cmake --build build -j4 --config ${{ matrix.build_type }} --target check.geometry + cmake --build build -j4 --config ${{ matrix.build_type }} --target check.inference + cmake --build build -j4 --config ${{ matrix.build_type }} --target check.linear + cmake --build build -j4 --config ${{ matrix.build_type }} --target check.navigation + #cmake --build build -j4 --config ${{ matrix.build_type }} --target check.nonlinear + cmake --build build -j4 --config ${{ matrix.build_type }} --target check.sam + cmake --build build -j4 --config ${{ matrix.build_type }} --target check.sfm + #cmake --build build -j4 --config ${{ matrix.build_type }} --target check.slam + cmake --build build -j4 --config ${{ matrix.build_type }} --target check.symbolic # Run GTSAM_UNSTABLE tests #cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.base_unstable From f65414d7efc9c8a2a7cf45d1a877815e9184983f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 21 Jun 2023 13:03:54 -0400 Subject: [PATCH 013/206] add GTSAM_EXPORT to additional Ordering methods --- gtsam/inference/Ordering.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/gtsam/inference/Ordering.h b/gtsam/inference/Ordering.h index 884a93f0d..0a3b25441 100644 --- a/gtsam/inference/Ordering.h +++ b/gtsam/inference/Ordering.h @@ -77,9 +77,11 @@ public: * @param keys The key vector to append to this ordering. * @return The ordering variable with appended keys. */ + GTSAM_EXPORT This& operator+=(KeyVector& keys); /// Check if key exists in ordering. + GTSAM_EXPORT bool contains(const Key& key) const; /** From 43a9fbf461ee29ec9478d21b4362d35498ee3550 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 21 Jun 2023 13:29:10 -0400 Subject: [PATCH 014/206] mark Pose2 as GTSAM_EXPORT at the class level --- gtsam/geometry/Pose2.h | 38 +++++++++++++++++++------------------- 1 file changed, 19 insertions(+), 19 deletions(-) diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index f1b38c5a6..1a62fa938 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -36,7 +36,7 @@ namespace gtsam { * @ingroup geometry * \nosubgrouping */ -class Pose2: public LieGroup { +class GTSAM_EXPORT Pose2: public LieGroup { public: @@ -112,10 +112,10 @@ public: /// @{ /** print with optional string */ - GTSAM_EXPORT void print(const std::string& s = "") const; + void print(const std::string& s = "") const; /** assert equality up to a tolerance */ - GTSAM_EXPORT bool equals(const Pose2& pose, double tol = 1e-9) const; + bool equals(const Pose2& pose, double tol = 1e-9) const; /// @} /// @name Group @@ -125,7 +125,7 @@ public: inline static Pose2 Identity() { return Pose2(); } /// inverse - GTSAM_EXPORT Pose2 inverse() const; + Pose2 inverse() const; /// compose syntactic sugar inline Pose2 operator*(const Pose2& p2) const { @@ -137,16 +137,16 @@ public: /// @{ ///Exponential map at identity - create a rotation from canonical coordinates \f$ [T_x,T_y,\theta] \f$ - GTSAM_EXPORT static Pose2 Expmap(const Vector3& xi, ChartJacobian H = {}); + static Pose2 Expmap(const Vector3& xi, ChartJacobian H = {}); ///Log map at identity - return the canonical coordinates \f$ [T_x,T_y,\theta] \f$ of this rotation - GTSAM_EXPORT static Vector3 Logmap(const Pose2& p, ChartJacobian H = {}); + static Vector3 Logmap(const Pose2& p, ChartJacobian H = {}); /** * Calculate Adjoint map * Ad_pose is 3*3 matrix that when applied to twist xi \f$ [T_x,T_y,\theta] \f$, returns Ad_pose(xi) */ - GTSAM_EXPORT Matrix3 AdjointMap() const; + Matrix3 AdjointMap() const; /// Apply AdjointMap to twist xi inline Vector3 Adjoint(const Vector3& xi) const { @@ -156,7 +156,7 @@ public: /** * Compute the [ad(w,v)] operator for SE2 as in [Kobilarov09siggraph], pg 19 */ - GTSAM_EXPORT static Matrix3 adjointMap(const Vector3& v); + static Matrix3 adjointMap(const Vector3& v); /** * Action of the adjointMap on a Lie-algebra vector y, with optional derivatives @@ -192,15 +192,15 @@ public: } /// Derivative of Expmap - GTSAM_EXPORT static Matrix3 ExpmapDerivative(const Vector3& v); + static Matrix3 ExpmapDerivative(const Vector3& v); /// Derivative of Logmap - GTSAM_EXPORT static Matrix3 LogmapDerivative(const Pose2& v); + static Matrix3 LogmapDerivative(const Pose2& v); // Chart at origin, depends on compile-time flag SLOW_BUT_CORRECT_EXPMAP struct ChartAtOrigin { - GTSAM_EXPORT static Pose2 Retract(const Vector3& v, ChartJacobian H = {}); - GTSAM_EXPORT static Vector3 Local(const Pose2& r, ChartJacobian H = {}); + static Pose2 Retract(const Vector3& v, ChartJacobian H = {}); + static Vector3 Local(const Pose2& r, ChartJacobian H = {}); }; using LieGroup::inverse; // version with derivative @@ -210,7 +210,7 @@ public: /// @{ /** Return point coordinates in pose coordinate frame */ - GTSAM_EXPORT Point2 transformTo(const Point2& point, + Point2 transformTo(const Point2& point, OptionalJacobian<2, 3> Dpose = {}, OptionalJacobian<2, 2> Dpoint = {}) const; @@ -222,7 +222,7 @@ public: Matrix transformTo(const Matrix& points) const; /** Return point coordinates in global frame */ - GTSAM_EXPORT Point2 transformFrom(const Point2& point, + Point2 transformFrom(const Point2& point, OptionalJacobian<2, 3> Dpose = {}, OptionalJacobian<2, 2> Dpoint = {}) const; @@ -273,14 +273,14 @@ public: } //// return transformation matrix - GTSAM_EXPORT Matrix3 matrix() const; + Matrix3 matrix() const; /** * Calculate bearing to a landmark * @param point 2D location of landmark * @return 2D rotation \f$ \in SO(2) \f$ */ - GTSAM_EXPORT Rot2 bearing(const Point2& point, + Rot2 bearing(const Point2& point, OptionalJacobian<1, 3> H1={}, OptionalJacobian<1, 2> H2={}) const; /** @@ -288,7 +288,7 @@ public: * @param point SO(2) location of other pose * @return 2D rotation \f$ \in SO(2) \f$ */ - GTSAM_EXPORT Rot2 bearing(const Pose2& pose, + Rot2 bearing(const Pose2& pose, OptionalJacobian<1, 3> H1={}, OptionalJacobian<1, 3> H2={}) const; /** @@ -296,7 +296,7 @@ public: * @param point 2D location of landmark * @return range (double) */ - GTSAM_EXPORT double range(const Point2& point, + double range(const Point2& point, OptionalJacobian<1, 3> H1={}, OptionalJacobian<1, 2> H2={}) const; @@ -305,7 +305,7 @@ public: * @param point 2D location of other pose * @return range (double) */ - GTSAM_EXPORT double range(const Pose2& point, + double range(const Pose2& point, OptionalJacobian<1, 3> H1={}, OptionalJacobian<1, 3> H2={}) const; From c9397c34fcd9c287c905fa091dc254a5d0f054ac Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 21 Jun 2023 13:29:32 -0400 Subject: [PATCH 015/206] CameraSet::project2 SFINAE to resolve overload ambiguity --- gtsam/geometry/CameraSet.h | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index 5f77ebf58..8621d77ad 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -131,12 +131,15 @@ class CameraSet : public std::vector> { return z; } - /** An overload o the project2 function to accept + /** An overload of the project2 function to accept * full matrices and vectors and pass it to the pointer - * version of the function + * version of the function. + * + * Use SFINAE to resolve overload ambiguity. */ template - ZVector project2(const POINT& point, OptArgs&... args) const { + typename std::enable_if<(sizeof...(OptArgs) != 0), ZVector>::type project2( + const POINT& point, OptArgs&... args) const { // pass it to the pointer version of the function return project2(point, (&args)...); } From 5fa889b035c238b95c84de1e7e04cc50a275edcb Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 21 Jun 2023 17:32:26 -0400 Subject: [PATCH 016/206] add GTSAM_EXPORT tags --- gtsam/discrete/SignatureParser.h | 2 +- gtsam_unstable/linear/QPSParser.h | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/gtsam/discrete/SignatureParser.h b/gtsam/discrete/SignatureParser.h index e6b402e44..e005da14a 100644 --- a/gtsam/discrete/SignatureParser.h +++ b/gtsam/discrete/SignatureParser.h @@ -47,7 +47,7 @@ namespace gtsam { * * Also fails if the rows are not of the same size. */ -struct SignatureParser { +struct GTSAM_EXPORT SignatureParser { using Row = std::vector; using Table = std::vector; diff --git a/gtsam_unstable/linear/QPSParser.h b/gtsam_unstable/linear/QPSParser.h index bd4254d0f..e751f34d2 100644 --- a/gtsam_unstable/linear/QPSParser.h +++ b/gtsam_unstable/linear/QPSParser.h @@ -18,12 +18,13 @@ #pragma once +#include #include #include namespace gtsam { -class QPSParser { +class GTSAM_UNSTABLE_EXPORT QPSParser { private: std::string fileName_; From c954e546ef4fb6614344d417a76ad922df45af29 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 21 Jun 2023 17:33:14 -0400 Subject: [PATCH 017/206] enable more Windows tests --- .github/workflows/build-windows.yml | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index 339f10a0a..e85064d08 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -104,15 +104,23 @@ jobs: cmake --build build -j4 --config ${{ matrix.build_type }} --target check.base cmake --build build -j4 --config ${{ matrix.build_type }} --target check.basis cmake --build build -j4 --config ${{ matrix.build_type }} --target check.discrete - #cmake --build build -j4 --config ${{ matrix.build_type }} --target check.geometry + cmake --build build -j4 --config ${{ matrix.build_type }} --target check.geometry cmake --build build -j4 --config ${{ matrix.build_type }} --target check.inference cmake --build build -j4 --config ${{ matrix.build_type }} --target check.linear cmake --build build -j4 --config ${{ matrix.build_type }} --target check.navigation - #cmake --build build -j4 --config ${{ matrix.build_type }} --target check.nonlinear cmake --build build -j4 --config ${{ matrix.build_type }} --target check.sam cmake --build build -j4 --config ${{ matrix.build_type }} --target check.sfm - #cmake --build build -j4 --config ${{ matrix.build_type }} --target check.slam cmake --build build -j4 --config ${{ matrix.build_type }} --target check.symbolic + # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.hybrid + # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.nonlinear + # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.slam # Run GTSAM_UNSTABLE tests - #cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.base_unstable + cmake --build build -j4 --config ${{ matrix.build_type }} --target check.base_unstable + # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.geometry_unstable + # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.linear_unstable + # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.discrete_unstable + # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.dynamics_unstable + # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.nonlinear_unstable + # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.slam_unstable + # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.partition From 0bc08b88bc7598eb5d8f6554a113235d3dd8a012 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 21 Jun 2023 17:49:15 -0400 Subject: [PATCH 018/206] remove GTSAM_EXPORT from SignatureParser struct --- gtsam/discrete/SignatureParser.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/discrete/SignatureParser.h b/gtsam/discrete/SignatureParser.h index e005da14a..e6b402e44 100644 --- a/gtsam/discrete/SignatureParser.h +++ b/gtsam/discrete/SignatureParser.h @@ -47,7 +47,7 @@ namespace gtsam { * * Also fails if the rows are not of the same size. */ -struct GTSAM_EXPORT SignatureParser { +struct SignatureParser { using Row = std::vector; using Table = std::vector; From 1de26020b3c50e6e26d64c821f00230d140f1656 Mon Sep 17 00:00:00 2001 From: IshitaTakeshi Date: Wed, 28 Jun 2023 20:21:55 +0900 Subject: [PATCH 019/206] Add explanation of the StereoPoint constructor --- gtsam/geometry/StereoPoint2.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/gtsam/geometry/StereoPoint2.h b/gtsam/geometry/StereoPoint2.h index bd335dfbb..4383e212e 100644 --- a/gtsam/geometry/StereoPoint2.h +++ b/gtsam/geometry/StereoPoint2.h @@ -46,7 +46,9 @@ public: uL_(0), uR_(0), v_(0) { } - /** constructor */ + /** uL and uR represent the x-axis value of left and right frame coordinates respectively. + v represents the y coordinate value. The y-axis value should be the same under the + stereo constraint. */ StereoPoint2(double uL, double uR, double v) : uL_(uL), uR_(uR), v_(v) { } From 72734d184b28953a14bf150ea13450fad5bba218 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 29 Jun 2023 12:12:48 -0400 Subject: [PATCH 020/206] overload += operator in Ordering --- gtsam/inference/Ordering.cpp | 12 ++++++++++++ gtsam/inference/Ordering.h | 20 +++++++------------- gtsam/inference/tests/testOrdering.cpp | 14 ++++++++++++++ 3 files changed, 33 insertions(+), 13 deletions(-) diff --git a/gtsam/inference/Ordering.cpp b/gtsam/inference/Ordering.cpp index 2956c7575..cb2ca752d 100644 --- a/gtsam/inference/Ordering.cpp +++ b/gtsam/inference/Ordering.cpp @@ -281,6 +281,18 @@ void Ordering::print(const std::string& str, cout.flush(); } +/* ************************************************************************* */ +Ordering::This& Ordering::operator+=(Key key) { + this->push_back(key); + return *this; +} + +/* ************************************************************************* */ +Ordering::This& Ordering::operator,(Key key) { + this->push_back(key); + return *this; +} + /* ************************************************************************* */ Ordering::This& Ordering::operator+=(KeyVector& keys) { this->insert(this->end(), keys.begin(), keys.end()); diff --git a/gtsam/inference/Ordering.h b/gtsam/inference/Ordering.h index 884a93f0d..86d44e072 100644 --- a/gtsam/inference/Ordering.h +++ b/gtsam/inference/Ordering.h @@ -25,10 +25,6 @@ #include #include -#ifdef GTSAM_USE_BOOST_FEATURES -#include -#endif - #include #include @@ -61,15 +57,13 @@ public: Base(keys.begin(), keys.end()) { } -#ifdef GTSAM_USE_BOOST_FEATURES - /// Add new variables to the ordering as ordering += key1, key2, ... Equivalent to calling - /// push_back. - boost::assign::list_inserter > operator+=( - Key key) { - return boost::assign::make_list_inserter( - boost::assign_detail::call_push_back(*this))(key); - } -#endif + /// Add new variables to the ordering as + /// `ordering += key1, key2, ...`. + This& operator+=(Key key); + + /// Overloading the comma operator allows for chaining appends + // e.g. keys += key1, key2 + This& operator,(Key key); /** * @brief Append new keys to the ordering as `ordering += keys`. diff --git a/gtsam/inference/tests/testOrdering.cpp b/gtsam/inference/tests/testOrdering.cpp index 761c330b4..328d383d8 100644 --- a/gtsam/inference/tests/testOrdering.cpp +++ b/gtsam/inference/tests/testOrdering.cpp @@ -196,6 +196,20 @@ TEST(Ordering, csr_format_3) { EXPECT(adjExpected == adjAcutal); } +/* ************************************************************************* */ +TEST(Ordering, AppendKey) { + using symbol_shorthand::X; + Ordering actual; + actual += X(0); + + Ordering expected1{X(0)}; + EXPECT(assert_equal(expected1, actual)); + + actual += X(1), X(2), X(3); + Ordering expected2{X(0), X(1), X(2), X(3)}; + EXPECT(assert_equal(expected2, actual)); +} + /* ************************************************************************* */ TEST(Ordering, AppendVector) { using symbol_shorthand::X; From 9abf6c39c2fae08f574377e99bfdc6736c007967 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 29 Jun 2023 12:13:16 -0400 Subject: [PATCH 021/206] overload += operator in FactorGraph --- gtsam/inference/FactorGraph.h | 77 +++++-------------- .../linear/tests/testGaussianFactorGraph.cpp | 22 ++++++ 2 files changed, 42 insertions(+), 57 deletions(-) diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index f1dc37c37..327fca49a 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -29,10 +29,6 @@ #include // for Eigen::aligned_allocator -#ifdef GTSAM_USE_BOOST_FEATURES -#include -#endif - #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION #include #include @@ -53,45 +49,6 @@ class BayesTree; class HybridValues; -/** Helper */ -template -class CRefCallPushBack { - C& obj; - - public: - explicit CRefCallPushBack(C& obj) : obj(obj) {} - template - void operator()(const A& a) { - obj.push_back(a); - } -}; - -/** Helper */ -template -class RefCallPushBack { - C& obj; - - public: - explicit RefCallPushBack(C& obj) : obj(obj) {} - template - void operator()(A& a) { - obj.push_back(a); - } -}; - -/** Helper */ -template -class CRefCallAddCopy { - C& obj; - - public: - explicit CRefCallAddCopy(C& obj) : obj(obj) {} - template - void operator()(const A& a) { - obj.addCopy(a); - } -}; - /** * A factor graph is a bipartite graph with factor nodes connected to variable * nodes. In this class, however, only factor nodes are kept around. @@ -215,17 +172,26 @@ class FactorGraph { push_back(factor); } -#ifdef GTSAM_USE_BOOST_FEATURES - /// `+=` works well with boost::assign list inserter. + /// Append factor to factor graph template - typename std::enable_if< - std::is_base_of::value, - boost::assign::list_inserter>>::type + typename std::enable_if::value, + This>::type& operator+=(std::shared_ptr factor) { - return boost::assign::make_list_inserter(RefCallPushBack(*this))( - factor); + push_back(factor); + return *this; + } + + /** + * @brief Overload comma operator to allow for append chaining. + * + * E.g. fg += factor1, factor2, ... + */ + template + typename std::enable_if::value, This>::type& operator,( + std::shared_ptr factor) { + push_back(factor); + return *this; } -#endif /// @} /// @name Adding via iterators @@ -276,18 +242,15 @@ class FactorGraph { push_back(factorOrContainer); } -#ifdef GTSAM_USE_BOOST_FEATURES /** * Add a factor or container of factors, including STL collections, * BayesTrees, etc. */ template - boost::assign::list_inserter> operator+=( - const FACTOR_OR_CONTAINER& factorOrContainer) { - return boost::assign::make_list_inserter(CRefCallPushBack(*this))( - factorOrContainer); + This& operator+=(const FACTOR_OR_CONTAINER& factorOrContainer) { + push_back(factorOrContainer); + return *this; } -#endif /// @} /// @name Specialized versions diff --git a/gtsam/linear/tests/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp index e9e626296..41ee9471e 100644 --- a/gtsam/linear/tests/testGaussianFactorGraph.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraph.cpp @@ -70,6 +70,28 @@ TEST(GaussianFactorGraph, initialization) { EQUALITY(expectedIJS, actualIJS); } +/* ************************************************************************* */ +TEST(GaussianFactorGraph, Append) { + // Create empty graph + GaussianFactorGraph fg; + SharedDiagonal unit2 = noiseModel::Unit::Create(2); + + auto f1 = + make_shared(0, 10 * I_2x2, -1.0 * Vector::Ones(2), unit2); + auto f2 = make_shared(0, -10 * I_2x2, 1, 10 * I_2x2, + Vector2(2.0, -1.0), unit2); + auto f3 = make_shared(0, -5 * I_2x2, 2, 5 * I_2x2, + Vector2(0.0, 1.0), unit2); + + fg += f1; + fg += f2; + EXPECT_LONGS_EQUAL(2, fg.size()); + + fg = GaussianFactorGraph(); + fg += f1, f2, f3; + EXPECT_LONGS_EQUAL(3, fg.size()); +} + /* ************************************************************************* */ TEST(GaussianFactorGraph, sparseJacobian) { // Create factor graph: From 0971c75a61381e8232f16690b7d5d910685d6ae8 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 29 Jun 2023 15:27:23 -0400 Subject: [PATCH 022/206] remove unnecessary headers --- gtsam/base/Group.h | 7 ------- gtsam/base/types.h | 7 +------ 2 files changed, 1 insertion(+), 13 deletions(-) diff --git a/gtsam/base/Group.h b/gtsam/base/Group.h index e76a273da..09534efc3 100644 --- a/gtsam/base/Group.h +++ b/gtsam/base/Group.h @@ -22,13 +22,6 @@ #include -#ifdef GTSAM_USE_BOOST_FEATURES -#include -#include -#include -#include -#endif - #include namespace gtsam { diff --git a/gtsam/base/types.h b/gtsam/base/types.h index 3f1c1cd3d..c734678d2 100644 --- a/gtsam/base/types.h +++ b/gtsam/base/types.h @@ -19,16 +19,11 @@ #pragma once +#include // for GTSAM_USE_TBB #include -#ifdef GTSAM_USE_BOOST_FEATURES -#include -#include -#endif -#include // for GTSAM_USE_TBB #include #include - #include #include From 9c0caf60bc6d48cc6d93a6cfa4194b9ed843fa1f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 29 Jun 2023 16:39:08 -0400 Subject: [PATCH 023/206] add header to timing.cpp --- gtsam/base/timing.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/gtsam/base/timing.cpp b/gtsam/base/timing.cpp index 154a564db..b43595066 100644 --- a/gtsam/base/timing.cpp +++ b/gtsam/base/timing.cpp @@ -19,9 +19,10 @@ #include #include +#include +#include #include #include -#include #include #include #include From 2168d0f0867640b7eb2610e8a294a9bb4a04fa45 Mon Sep 17 00:00:00 2001 From: Tal Regev Date: Fri, 30 Jun 2023 19:43:34 +0300 Subject: [PATCH 024/206] Compile with ninja --- .github/workflows/build-windows.yml | 31 +++++++++++++++++------------ 1 file changed, 18 insertions(+), 13 deletions(-) diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index 0434577c1..9326a81b0 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -2,6 +2,10 @@ name: Windows CI on: [pull_request] +concurrency: + group: ${{ github.workflow }}-${{ github.head_ref || github.run_id }} + cancel-in-progress: true + jobs: build: name: ${{ matrix.name }} ${{ matrix.build_type }} @@ -16,19 +20,15 @@ jobs: BOOST_EXE: boost_1_72_0-msvc-14.2 strategy: - fail-fast: true + fail-fast: false matrix: - # Github Actions requires a single row to be added to the build matrix. - # See https://help.github.com/en/articles/workflow-syntax-for-github-actions. - name: [ - windows-2019-cl, - ] - - build_type: [ - Debug, - Release - ] - build_unstable: [ON] + build_type: + - Debug + # - Release + + build_unstable: + - ON + # - OFF include: - name: windows-2019-cl os: windows-2019 @@ -87,10 +87,15 @@ jobs: - name: Checkout uses: actions/checkout@v3 + - name: Setup msbuild + uses: ilammy/msvc-dev-cmd@v1 + with: + arch: x${{ matrix.platform }} + - name: Configuration run: | cmake -E remove_directory build - cmake -B build -S . -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DBOOST_ROOT="${env:BOOST_ROOT}" -DBOOST_INCLUDEDIR="${env:BOOST_ROOT}\boost\include" -DBOOST_LIBRARYDIR="${env:BOOST_ROOT}\lib" + cmake -G Ninja -B build -S . -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DBOOST_ROOT="${env:BOOST_ROOT}" -DBOOST_INCLUDEDIR="${env:BOOST_ROOT}\boost\include" -DBOOST_LIBRARYDIR="${env:BOOST_ROOT}\lib" - name: Build run: | From 92de2273a86bd316205b018d425449d1ca4d4a0e Mon Sep 17 00:00:00 2001 From: Tal Regev Date: Fri, 30 Jun 2023 20:21:11 +0300 Subject: [PATCH 025/206] Fix linkage errors: unresolved external symbol --- gtsam/basis/Basis.h | 2 +- gtsam/discrete/SignatureParser.h | 2 +- gtsam/geometry/Pose2.h | 6 +++--- gtsam/geometry/Pose3.h | 2 +- 4 files changed, 6 insertions(+), 6 deletions(-) diff --git a/gtsam/basis/Basis.h b/gtsam/basis/Basis.h index 41cdeeaaa..0b2b3606b 100644 --- a/gtsam/basis/Basis.h +++ b/gtsam/basis/Basis.h @@ -80,7 +80,7 @@ using Weights = Eigen::Matrix; /* 1xN vector */ * * @ingroup basis */ -Matrix kroneckerProductIdentity(size_t M, const Weights& w); +Matrix GTSAM_EXPORT kroneckerProductIdentity(size_t M, const Weights& w); /** * CRTP Base class for function bases diff --git a/gtsam/discrete/SignatureParser.h b/gtsam/discrete/SignatureParser.h index e6b402e44..e005da14a 100644 --- a/gtsam/discrete/SignatureParser.h +++ b/gtsam/discrete/SignatureParser.h @@ -47,7 +47,7 @@ namespace gtsam { * * Also fails if the rows are not of the same size. */ -struct SignatureParser { +struct GTSAM_EXPORT SignatureParser { using Row = std::vector; using Table = std::vector; diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index f1b38c5a6..b455a81c3 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -198,9 +198,9 @@ public: GTSAM_EXPORT static Matrix3 LogmapDerivative(const Pose2& v); // Chart at origin, depends on compile-time flag SLOW_BUT_CORRECT_EXPMAP - struct ChartAtOrigin { - GTSAM_EXPORT static Pose2 Retract(const Vector3& v, ChartJacobian H = {}); - GTSAM_EXPORT static Vector3 Local(const Pose2& r, ChartJacobian H = {}); + struct GTSAM_EXPORT ChartAtOrigin { + static Pose2 Retract(const Vector3& v, ChartJacobian H = {}); + static Vector3 Local(const Pose2& r, ChartJacobian H = {}); }; using LieGroup::inverse; // version with derivative diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 6c91d7468..8a807cc23 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -204,7 +204,7 @@ public: static Matrix6 LogmapDerivative(const Pose3& xi); // Chart at origin, depends on compile-time flag GTSAM_POSE3_EXPMAP - struct ChartAtOrigin { + struct GTSAM_EXPORT ChartAtOrigin { static Pose3 Retract(const Vector6& xi, ChartJacobian Hxi = {}); static Vector6 Local(const Pose3& pose, ChartJacobian Hpose = {}); }; From a499855eafe945f5559d4712054bd48289c3699d Mon Sep 17 00:00:00 2001 From: Tal Regev Date: Fri, 30 Jun 2023 21:56:24 +0300 Subject: [PATCH 026/206] Add release to windows tests --- .github/workflows/build-windows.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index 9326a81b0..dabdff7f9 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -24,7 +24,7 @@ jobs: matrix: build_type: - Debug - # - Release + - Release build_unstable: - ON From 9aa67b5235f8cb11a6bad8ab08f422f6fe78a7c4 Mon Sep 17 00:00:00 2001 From: Tal Regev Date: Fri, 30 Jun 2023 22:14:43 +0300 Subject: [PATCH 027/206] Add #include --- gtsam/discrete/SignatureParser.h | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/discrete/SignatureParser.h b/gtsam/discrete/SignatureParser.h index e005da14a..fec3b9af2 100644 --- a/gtsam/discrete/SignatureParser.h +++ b/gtsam/discrete/SignatureParser.h @@ -21,6 +21,7 @@ #include #include #include +#include namespace gtsam { /** From 7f46117666cfb35f7a4e90a8ef6cf72b14f0b17c Mon Sep 17 00:00:00 2001 From: Tal Regev Date: Fri, 30 Jun 2023 22:37:43 +0300 Subject: [PATCH 028/206] Add non concurrency to all workflows --- .github/workflows/build-linux.yml | 6 ++++++ .github/workflows/build-macos.yml | 6 ++++++ .github/workflows/build-python.yml | 6 ++++++ .github/workflows/build-special.yml | 6 ++++++ .github/workflows/build-windows.yml | 2 ++ 5 files changed, 26 insertions(+) diff --git a/.github/workflows/build-linux.yml b/.github/workflows/build-linux.yml index 129b5aaaf..e4937ce06 100644 --- a/.github/workflows/build-linux.yml +++ b/.github/workflows/build-linux.yml @@ -2,6 +2,12 @@ name: Linux CI on: [pull_request] +# Every time you make a push to your PR, it cancel immediately the previous checks, +# and start a new one. The other runner will be available more quickly to your PR. +concurrency: + group: ${{ github.workflow }}-${{ github.head_ref || github.run_id }} + cancel-in-progress: true + jobs: build: name: ${{ matrix.name }} ${{ matrix.build_type }} diff --git a/.github/workflows/build-macos.yml b/.github/workflows/build-macos.yml index 3fa3c15dd..541701836 100644 --- a/.github/workflows/build-macos.yml +++ b/.github/workflows/build-macos.yml @@ -2,6 +2,12 @@ name: macOS CI on: [pull_request] +# Every time you make a push to your PR, it cancel immediately the previous checks, +# and start a new one. The other runner will be available more quickly to your PR. +concurrency: + group: ${{ github.workflow }}-${{ github.head_ref || github.run_id }} + cancel-in-progress: true + jobs: build: name: ${{ matrix.name }} ${{ matrix.build_type }} diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index f09d589dc..ca4645a77 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -2,6 +2,12 @@ name: Python CI on: [pull_request] +# Every time you make a push to your PR, it cancel immediately the previous checks, +# and start a new one. The other runner will be available more quickly to your PR. +concurrency: + group: ${{ github.workflow }}-${{ github.head_ref || github.run_id }} + cancel-in-progress: true + jobs: build: name: ${{ matrix.name }} ${{ matrix.build_type }} Python ${{ matrix.python_version }} diff --git a/.github/workflows/build-special.yml b/.github/workflows/build-special.yml index f72dadbae..72466ffd6 100644 --- a/.github/workflows/build-special.yml +++ b/.github/workflows/build-special.yml @@ -2,6 +2,12 @@ name: Special Cases CI on: [pull_request] +# Every time you make a push to your PR, it cancel immediately the previous checks, +# and start a new one. The other runner will be available more quickly to your PR. +concurrency: + group: ${{ github.workflow }}-${{ github.head_ref || github.run_id }} + cancel-in-progress: true + jobs: build: name: ${{ matrix.name }} ${{ matrix.build_type }} diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index 99b654b3a..5d5342f3f 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -2,6 +2,8 @@ name: Windows CI on: [pull_request] +# Every time you make a push to your PR, it cancel immediately the previous checks, +# and start a new one. The other runner will be available more quickly to your PR. concurrency: group: ${{ github.workflow }}-${{ github.head_ref || github.run_id }} cancel-in-progress: true From c95bcae93a6353fd241b55d6c885c1e552bce148 Mon Sep 17 00:00:00 2001 From: Tal Regev Date: Sat, 1 Jul 2023 01:40:55 +0300 Subject: [PATCH 029/206] Add non concurrency to all workflows --- .github/workflows/build-linux.yml | 6 ++++++ .github/workflows/build-macos.yml | 6 ++++++ .github/workflows/build-python.yml | 6 ++++++ .github/workflows/build-special.yml | 6 ++++++ .github/workflows/build-windows.yml | 6 ++++++ 5 files changed, 30 insertions(+) diff --git a/.github/workflows/build-linux.yml b/.github/workflows/build-linux.yml index 129b5aaaf..e4937ce06 100644 --- a/.github/workflows/build-linux.yml +++ b/.github/workflows/build-linux.yml @@ -2,6 +2,12 @@ name: Linux CI on: [pull_request] +# Every time you make a push to your PR, it cancel immediately the previous checks, +# and start a new one. The other runner will be available more quickly to your PR. +concurrency: + group: ${{ github.workflow }}-${{ github.head_ref || github.run_id }} + cancel-in-progress: true + jobs: build: name: ${{ matrix.name }} ${{ matrix.build_type }} diff --git a/.github/workflows/build-macos.yml b/.github/workflows/build-macos.yml index 3fa3c15dd..541701836 100644 --- a/.github/workflows/build-macos.yml +++ b/.github/workflows/build-macos.yml @@ -2,6 +2,12 @@ name: macOS CI on: [pull_request] +# Every time you make a push to your PR, it cancel immediately the previous checks, +# and start a new one. The other runner will be available more quickly to your PR. +concurrency: + group: ${{ github.workflow }}-${{ github.head_ref || github.run_id }} + cancel-in-progress: true + jobs: build: name: ${{ matrix.name }} ${{ matrix.build_type }} diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index f09d589dc..ca4645a77 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -2,6 +2,12 @@ name: Python CI on: [pull_request] +# Every time you make a push to your PR, it cancel immediately the previous checks, +# and start a new one. The other runner will be available more quickly to your PR. +concurrency: + group: ${{ github.workflow }}-${{ github.head_ref || github.run_id }} + cancel-in-progress: true + jobs: build: name: ${{ matrix.name }} ${{ matrix.build_type }} Python ${{ matrix.python_version }} diff --git a/.github/workflows/build-special.yml b/.github/workflows/build-special.yml index f72dadbae..72466ffd6 100644 --- a/.github/workflows/build-special.yml +++ b/.github/workflows/build-special.yml @@ -2,6 +2,12 @@ name: Special Cases CI on: [pull_request] +# Every time you make a push to your PR, it cancel immediately the previous checks, +# and start a new one. The other runner will be available more quickly to your PR. +concurrency: + group: ${{ github.workflow }}-${{ github.head_ref || github.run_id }} + cancel-in-progress: true + jobs: build: name: ${{ matrix.name }} ${{ matrix.build_type }} diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index 0434577c1..cbe0c10f1 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -2,6 +2,12 @@ name: Windows CI on: [pull_request] +# Every time you make a push to your PR, it cancel immediately the previous checks, +# and start a new one. The other runner will be available more quickly to your PR. +concurrency: + group: ${{ github.workflow }}-${{ github.head_ref || github.run_id }} + cancel-in-progress: true + jobs: build: name: ${{ matrix.name }} ${{ matrix.build_type }} From d1bd76a0aadb06da0ff258e8091b1e8ba9c7b10d Mon Sep 17 00:00:00 2001 From: Tal Regev Date: Sat, 1 Jul 2023 17:02:03 +0300 Subject: [PATCH 030/206] Check more tests --- .github/workflows/build-windows.yml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index 5d5342f3f..7f8c56558 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -118,9 +118,9 @@ jobs: cmake --build build -j4 --config ${{ matrix.build_type }} --target check.sam cmake --build build -j4 --config ${{ matrix.build_type }} --target check.sfm cmake --build build -j4 --config ${{ matrix.build_type }} --target check.symbolic - # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.hybrid - # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.nonlinear - # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.slam + cmake --build build -j4 --config ${{ matrix.build_type }} --target check.hybrid + cmake --build build -j4 --config ${{ matrix.build_type }} --target check.nonlinear + cmake --build build -j4 --config ${{ matrix.build_type }} --target check.slam # Run GTSAM_UNSTABLE tests cmake --build build -j4 --config ${{ matrix.build_type }} --target check.base_unstable From d87db0a2f139384921ecea435d24bbdd308bedf2 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sat, 1 Jul 2023 13:57:11 -0700 Subject: [PATCH 031/206] Fix the CI under normal circumstances Signed-off-by: Fan Jiang --- cmake/HandleGeneralOptions.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/HandleGeneralOptions.cmake b/cmake/HandleGeneralOptions.cmake index eab40d16b..4474aa13f 100644 --- a/cmake/HandleGeneralOptions.cmake +++ b/cmake/HandleGeneralOptions.cmake @@ -30,7 +30,7 @@ option(GTSAM_ALLOW_DEPRECATED_SINCE_V43 "Allow use of methods/functions depr option(GTSAM_SUPPORT_NESTED_DISSECTION "Support Metis-based nested dissection" ON) option(GTSAM_TANGENT_PREINTEGRATION "Use new ImuFactor with integration on tangent space" ON) option(GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR "Use the slower but correct version of BetweenFactor" OFF) -option(GTSAM_SLOW_BUT_CORRECT_EXPMAP "Use slower but correct expmap" ON) +option(GTSAM_SLOW_BUT_CORRECT_EXPMAP "Use slower but correct expmap for Pose2" OFF) if (GTSAM_FORCE_SHARED_LIB) message(STATUS "GTSAM is a shared library due to GTSAM_FORCE_SHARED_LIB") From ac86415032ff54a3b1b488e4dc7060796675baf3 Mon Sep 17 00:00:00 2001 From: Tal Regev Date: Sat, 1 Jul 2023 21:20:56 +0300 Subject: [PATCH 032/206] Fix tests --- .github/workflows/build-windows.yml | 14 +++++++------- gtsam/hybrid/HybridFactorGraph.h | 2 +- gtsam/hybrid/HybridSmoother.h | 2 +- .../hybrid/tests/testHybridGaussianFactorGraph.cpp | 1 + 4 files changed, 10 insertions(+), 9 deletions(-) diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index 7f8c56558..e0f56aad2 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -123,11 +123,11 @@ jobs: cmake --build build -j4 --config ${{ matrix.build_type }} --target check.slam # Run GTSAM_UNSTABLE tests + cmake --build build -j4 --config ${{ matrix.build_type }} --target check.geometry_unstable + cmake --build build -j4 --config ${{ matrix.build_type }} --target check.linear_unstable + cmake --build build -j4 --config ${{ matrix.build_type }} --target check.discrete_unstable + cmake --build build -j4 --config ${{ matrix.build_type }} --target check.dynamics_unstable + cmake --build build -j4 --config ${{ matrix.build_type }} --target check.nonlinear_unstable + cmake --build build -j4 --config ${{ matrix.build_type }} --target check.slam_unstable + cmake --build build -j4 --config ${{ matrix.build_type }} --target check.partition cmake --build build -j4 --config ${{ matrix.build_type }} --target check.base_unstable - # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.geometry_unstable - # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.linear_unstable - # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.discrete_unstable - # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.dynamics_unstable - # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.nonlinear_unstable - # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.slam_unstable - # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.partition diff --git a/gtsam/hybrid/HybridFactorGraph.h b/gtsam/hybrid/HybridFactorGraph.h index b8e7ff588..8b59fd4f9 100644 --- a/gtsam/hybrid/HybridFactorGraph.h +++ b/gtsam/hybrid/HybridFactorGraph.h @@ -35,7 +35,7 @@ using SharedFactor = std::shared_ptr; * Hybrid Factor Graph * Factor graph with utilities for hybrid factors. */ -class HybridFactorGraph : public FactorGraph { +class GTSAM_EXPORT HybridFactorGraph : public FactorGraph { public: using Base = FactorGraph; using This = HybridFactorGraph; ///< this class diff --git a/gtsam/hybrid/HybridSmoother.h b/gtsam/hybrid/HybridSmoother.h index 0767da12f..5c2e4f546 100644 --- a/gtsam/hybrid/HybridSmoother.h +++ b/gtsam/hybrid/HybridSmoother.h @@ -24,7 +24,7 @@ namespace gtsam { -class HybridSmoother { +class GTSAM_EXPORT HybridSmoother { private: HybridBayesNet hybridBayesNet_; HybridGaussianFactorGraph remainingFactorGraph_; diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp index 8276264ae..073f49c04 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp @@ -43,6 +43,7 @@ #include #include #include +#include #include "Switching.h" #include "TinyHybridExample.h" From e00e1236b5799c63d9c8a756d3e742a3c47caa67 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 3 Jul 2023 08:56:08 -0400 Subject: [PATCH 033/206] reorganize CI workflow file --- .github/workflows/build-windows.yml | 23 ++++++++++++++--------- 1 file changed, 14 insertions(+), 9 deletions(-) diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index e0f56aad2..7123d6eea 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -22,15 +22,20 @@ jobs: BOOST_EXE: boost_1_72_0-msvc-14.2 strategy: - fail-fast: false + fail-fast: true matrix: - build_type: - - Debug - - Release - - build_unstable: - - ON - # - OFF + # Github Actions requires a single row to be added to the build matrix. + # See https://help.github.com/en/articles/workflow-syntax-for-github-actions. + name: [ + windows-2019-cl, + ] + + build_type: [ + Debug, + Release + ] + + build_unstable: [ON] include: - name: windows-2019-cl os: windows-2019 @@ -123,6 +128,7 @@ jobs: cmake --build build -j4 --config ${{ matrix.build_type }} --target check.slam # Run GTSAM_UNSTABLE tests + cmake --build build -j4 --config ${{ matrix.build_type }} --target check.base_unstable cmake --build build -j4 --config ${{ matrix.build_type }} --target check.geometry_unstable cmake --build build -j4 --config ${{ matrix.build_type }} --target check.linear_unstable cmake --build build -j4 --config ${{ matrix.build_type }} --target check.discrete_unstable @@ -130,4 +136,3 @@ jobs: cmake --build build -j4 --config ${{ matrix.build_type }} --target check.nonlinear_unstable cmake --build build -j4 --config ${{ matrix.build_type }} --target check.slam_unstable cmake --build build -j4 --config ${{ matrix.build_type }} --target check.partition - cmake --build build -j4 --config ${{ matrix.build_type }} --target check.base_unstable From 6fc2e36137caed3392e7af6b63bf09556770bcb5 Mon Sep 17 00:00:00 2001 From: Tal Regev Date: Mon, 3 Jul 2023 22:23:00 +0300 Subject: [PATCH 034/206] Handle latest ttb --- cmake/FindTBB.cmake | 323 ------------------------------------------ cmake/HandleTBB.cmake | 2 +- 2 files changed, 1 insertion(+), 324 deletions(-) delete mode 100644 cmake/FindTBB.cmake diff --git a/cmake/FindTBB.cmake b/cmake/FindTBB.cmake deleted file mode 100644 index 0ecd4ca0e..000000000 --- a/cmake/FindTBB.cmake +++ /dev/null @@ -1,323 +0,0 @@ -# The MIT License (MIT) -# -# Copyright (c) 2015 Justus Calvin -# -# Permission is hereby granted, free of charge, to any person obtaining a copy -# of this software and associated documentation files (the "Software"), to deal -# in the Software without restriction, including without limitation the rights -# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -# copies of the Software, and to permit persons to whom the Software is -# furnished to do so, subject to the following conditions: -# -# The above copyright notice and this permission notice shall be included in all -# copies or substantial portions of the Software. -# -# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -# SOFTWARE. - -# -# FindTBB -# ------- -# -# Find TBB include directories and libraries. -# -# Usage: -# -# find_package(TBB [major[.minor]] [EXACT] -# [QUIET] [REQUIRED] -# [[COMPONENTS] [components...]] -# [OPTIONAL_COMPONENTS components...]) -# -# where the allowed components are tbbmalloc and tbb_preview. Users may modify -# the behavior of this module with the following variables: -# -# * TBB_ROOT_DIR - The base directory the of TBB installation. -# * TBB_INCLUDE_DIR - The directory that contains the TBB headers files. -# * TBB_LIBRARY - The directory that contains the TBB library files. -# * TBB__LIBRARY - The path of the TBB the corresponding TBB library. -# These libraries, if specified, override the -# corresponding library search results, where -# may be tbb, tbb_debug, tbbmalloc, tbbmalloc_debug, -# tbb_preview, or tbb_preview_debug. -# * TBB_USE_DEBUG_BUILD - The debug version of tbb libraries, if present, will -# be used instead of the release version. -# -# Users may modify the behavior of this module with the following environment -# variables: -# -# * TBB_INSTALL_DIR -# * TBBROOT -# * LIBRARY_PATH -# -# This module will set the following variables: -# -# * TBB_FOUND - Set to false, or undefined, if we haven’t found, or -# don’t want to use TBB. -# * TBB__FOUND - If False, optional part of TBB sytem is -# not available. -# * TBB_VERSION - The full version string -# * TBB_VERSION_MAJOR - The major version -# * TBB_VERSION_MINOR - The minor version -# * TBB_INTERFACE_VERSION - The interface version number defined in -# tbb/tbb_stddef.h. -# * TBB__LIBRARY_RELEASE - The path of the TBB release version of -# , where may be tbb, tbb_debug, -# tbbmalloc, tbbmalloc_debug, tbb_preview, or -# tbb_preview_debug. -# * TBB__LIBRARY_DEGUG - The path of the TBB release version of -# , where may be tbb, tbb_debug, -# tbbmalloc, tbbmalloc_debug, tbb_preview, or -# tbb_preview_debug. -# -# The following varibles should be used to build and link with TBB: -# -# * TBB_INCLUDE_DIRS - The include directory for TBB. -# * TBB_LIBRARIES - The libraries to link against to use TBB. -# * TBB_LIBRARIES_RELEASE - The release libraries to link against to use TBB. -# * TBB_LIBRARIES_DEBUG - The debug libraries to link against to use TBB. -# * TBB_DEFINITIONS - Definitions to use when compiling code that uses -# TBB. -# * TBB_DEFINITIONS_RELEASE - Definitions to use when compiling release code that -# uses TBB. -# * TBB_DEFINITIONS_DEBUG - Definitions to use when compiling debug code that -# uses TBB. -# -# This module will also create the "tbb" target that may be used when building -# executables and libraries. - -include(FindPackageHandleStandardArgs) - -if(NOT TBB_FOUND) - - ################################## - # Check the build type - ################################## - - if(NOT DEFINED TBB_USE_DEBUG_BUILD) - # Set build type to RELEASE by default for optimization. - set(TBB_BUILD_TYPE RELEASE) - elseif(TBB_USE_DEBUG_BUILD) - set(TBB_BUILD_TYPE DEBUG) - else() - set(TBB_BUILD_TYPE RELEASE) - endif() - - ################################## - # Set the TBB search directories - ################################## - - # Define search paths based on user input and environment variables - set(TBB_SEARCH_DIR ${TBB_ROOT_DIR} $ENV{TBB_INSTALL_DIR} $ENV{TBBROOT}) - - # Define the search directories based on the current platform - if(CMAKE_SYSTEM_NAME STREQUAL "Windows") - set(TBB_DEFAULT_SEARCH_DIR "C:/Program Files/Intel/TBB" - "C:/Program Files (x86)/Intel/TBB") - - # Set the target architecture - if(CMAKE_SIZEOF_VOID_P EQUAL 8) - set(TBB_ARCHITECTURE "intel64") - else() - set(TBB_ARCHITECTURE "ia32") - endif() - - # Set the TBB search library path search suffix based on the version of VC - if(WINDOWS_STORE) - set(TBB_LIB_PATH_SUFFIX "lib/${TBB_ARCHITECTURE}/vc11_ui") - elseif(MSVC14) - set(TBB_LIB_PATH_SUFFIX "lib/${TBB_ARCHITECTURE}/vc14") - elseif(MSVC12) - set(TBB_LIB_PATH_SUFFIX "lib/${TBB_ARCHITECTURE}/vc12") - elseif(MSVC11) - set(TBB_LIB_PATH_SUFFIX "lib/${TBB_ARCHITECTURE}/vc11") - elseif(MSVC10) - set(TBB_LIB_PATH_SUFFIX "lib/${TBB_ARCHITECTURE}/vc10") - endif() - - # Add the library path search suffix for the VC independent version of TBB - list(APPEND TBB_LIB_PATH_SUFFIX "lib/${TBB_ARCHITECTURE}/vc_mt") - - elseif(CMAKE_SYSTEM_NAME STREQUAL "Darwin") - # OS X - set(TBB_DEFAULT_SEARCH_DIR "/opt/intel/tbb" - "/usr/local/opt/tbb") - - # TODO: Check to see which C++ library is being used by the compiler. - if(NOT ${CMAKE_SYSTEM_VERSION} VERSION_LESS 13.0) - # The default C++ library on OS X 10.9 and later is libc++ - set(TBB_LIB_PATH_SUFFIX "lib/libc++" "lib") - else() - set(TBB_LIB_PATH_SUFFIX "lib") - endif() - elseif(CMAKE_SYSTEM_NAME STREQUAL "Linux") - # Linux - set(TBB_DEFAULT_SEARCH_DIR "/opt/intel/tbb") - - # TODO: Check compiler version to see the suffix should be /gcc4.1 or - # /gcc4.1. For now, assume that the compiler is more recent than - # gcc 4.4.x or later. - if(CMAKE_SYSTEM_PROCESSOR STREQUAL "x86_64") - set(TBB_LIB_PATH_SUFFIX "lib/intel64/gcc4.4") - elseif(CMAKE_SYSTEM_PROCESSOR MATCHES "^i.86$") - set(TBB_LIB_PATH_SUFFIX "lib/ia32/gcc4.4") - endif() - endif() - - ################################## - # Find the TBB include dir - ################################## - - find_path(TBB_INCLUDE_DIRS tbb/tbb.h - HINTS ${TBB_INCLUDE_DIR} ${TBB_SEARCH_DIR} - PATHS ${TBB_DEFAULT_SEARCH_DIR} - PATH_SUFFIXES include) - - ################################## - # Set version strings - ################################## - - if(TBB_INCLUDE_DIRS) - set(_tbb_version_file_prior_to_tbb_2021_1 "${TBB_INCLUDE_DIRS}/tbb/tbb_stddef.h") - set(_tbb_version_file_after_tbb_2021_1 "${TBB_INCLUDE_DIRS}/oneapi/tbb/version.h") - - if (EXISTS "${_tbb_version_file_prior_to_tbb_2021_1}") - file(READ "${_tbb_version_file_prior_to_tbb_2021_1}" _tbb_version_file ) - elseif (EXISTS "${_tbb_version_file_after_tbb_2021_1}") - file(READ "${_tbb_version_file_after_tbb_2021_1}" _tbb_version_file ) - else() - message(FATAL_ERROR "Found TBB installation: ${TBB_INCLUDE_DIRS} " - "missing version header.") - endif() - - string(REGEX REPLACE ".*#define TBB_VERSION_MAJOR ([0-9]+).*" "\\1" - TBB_VERSION_MAJOR "${_tbb_version_file}") - string(REGEX REPLACE ".*#define TBB_VERSION_MINOR ([0-9]+).*" "\\1" - TBB_VERSION_MINOR "${_tbb_version_file}") - string(REGEX REPLACE ".*#define TBB_INTERFACE_VERSION ([0-9]+).*" "\\1" - TBB_INTERFACE_VERSION "${_tbb_version_file}") - set(TBB_VERSION "${TBB_VERSION_MAJOR}.${TBB_VERSION_MINOR}") - endif() - - ################################## - # Find TBB components - ################################## - - if(TBB_VERSION VERSION_LESS 4.3) - set(TBB_SEARCH_COMPOMPONENTS tbb_preview tbbmalloc tbb) - else() - set(TBB_SEARCH_COMPOMPONENTS tbb_preview tbbmalloc_proxy tbbmalloc tbb) - endif() - - # Find each component - foreach(_comp ${TBB_SEARCH_COMPOMPONENTS}) - if(";${TBB_FIND_COMPONENTS};tbb;" MATCHES ";${_comp};") - - # Search for the libraries - find_library(TBB_${_comp}_LIBRARY_RELEASE ${_comp} - HINTS ${TBB_LIBRARY} ${TBB_SEARCH_DIR} - PATHS ${TBB_DEFAULT_SEARCH_DIR} ENV LIBRARY_PATH - PATH_SUFFIXES ${TBB_LIB_PATH_SUFFIX}) - - find_library(TBB_${_comp}_LIBRARY_DEBUG ${_comp}_debug - HINTS ${TBB_LIBRARY} ${TBB_SEARCH_DIR} - PATHS ${TBB_DEFAULT_SEARCH_DIR} ENV LIBRARY_PATH - PATH_SUFFIXES ${TBB_LIB_PATH_SUFFIX}) - - if(TBB_${_comp}_LIBRARY_DEBUG) - list(APPEND TBB_LIBRARIES_DEBUG "${TBB_${_comp}_LIBRARY_DEBUG}") - endif() - if(TBB_${_comp}_LIBRARY_RELEASE) - list(APPEND TBB_LIBRARIES_RELEASE "${TBB_${_comp}_LIBRARY_RELEASE}") - endif() - if(TBB_${_comp}_LIBRARY_${TBB_BUILD_TYPE} AND NOT TBB_${_comp}_LIBRARY) - set(TBB_${_comp}_LIBRARY "${TBB_${_comp}_LIBRARY_${TBB_BUILD_TYPE}}") - endif() - - if(TBB_${_comp}_LIBRARY AND EXISTS "${TBB_${_comp}_LIBRARY}") - set(TBB_${_comp}_FOUND TRUE) - else() - set(TBB_${_comp}_FOUND FALSE) - endif() - - # Mark internal variables as advanced - mark_as_advanced(TBB_${_comp}_LIBRARY_RELEASE) - mark_as_advanced(TBB_${_comp}_LIBRARY_DEBUG) - mark_as_advanced(TBB_${_comp}_LIBRARY) - - endif() - endforeach() - - ################################## - # Set compile flags and libraries - ################################## - - set(TBB_DEFINITIONS_RELEASE "") - set(TBB_DEFINITIONS_DEBUG "-DTBB_USE_DEBUG=1") - - if(TBB_LIBRARIES_${TBB_BUILD_TYPE}) - set(TBB_DEFINITIONS "${TBB_DEFINITIONS_${TBB_BUILD_TYPE}}") - set(TBB_LIBRARIES "${TBB_LIBRARIES_${TBB_BUILD_TYPE}}") - elseif(TBB_LIBRARIES_RELEASE) - set(TBB_DEFINITIONS "${TBB_DEFINITIONS_RELEASE}") - set(TBB_LIBRARIES "${TBB_LIBRARIES_RELEASE}") - elseif(TBB_LIBRARIES_DEBUG) - set(TBB_DEFINITIONS "${TBB_DEFINITIONS_DEBUG}") - set(TBB_LIBRARIES "${TBB_LIBRARIES_DEBUG}") - endif() - - find_package_handle_standard_args(TBB - REQUIRED_VARS TBB_INCLUDE_DIRS TBB_LIBRARIES - HANDLE_COMPONENTS - VERSION_VAR TBB_VERSION) - - ################################## - # Create targets - ################################## - - if(NOT CMAKE_VERSION VERSION_LESS 3.0 AND TBB_FOUND) - # Start fix to support different targets for tbb, tbbmalloc, etc. - # (Jose Luis Blanco, Jan 2019) - # Iterate over tbb, tbbmalloc, etc. - foreach(libname ${TBB_SEARCH_COMPOMPONENTS}) - if ((NOT TBB_${libname}_LIBRARY_RELEASE) AND (NOT TBB_${libname}_LIBRARY_DEBUG)) - continue() - endif() - - add_library(${libname} SHARED IMPORTED) - - set_target_properties(${libname} PROPERTIES - INTERFACE_INCLUDE_DIRECTORIES ${TBB_INCLUDE_DIRS} - IMPORTED_LOCATION ${TBB_${libname}_LIBRARY_RELEASE}) - if(TBB_${libname}_LIBRARY_RELEASE AND TBB_${libname}_LIBRARY_DEBUG) - set_target_properties(${libname} PROPERTIES - INTERFACE_COMPILE_DEFINITIONS "$<$,$>:TBB_USE_DEBUG=1>" - IMPORTED_LOCATION_DEBUG ${TBB_${libname}_LIBRARY_DEBUG} - IMPORTED_LOCATION_RELWITHDEBINFO ${TBB_${libname}_LIBRARY_DEBUG} - IMPORTED_LOCATION_RELEASE ${TBB_${libname}_LIBRARY_RELEASE} - IMPORTED_LOCATION_MINSIZEREL ${TBB_${libname}_LIBRARY_RELEASE} - ) - elseif(TBB_${libname}_LIBRARY_RELEASE) - set_target_properties(${libname} PROPERTIES IMPORTED_LOCATION ${TBB_${libname}_LIBRARY_RELEASE}) - else() - set_target_properties(${libname} PROPERTIES - INTERFACE_COMPILE_DEFINITIONS "${TBB_DEFINITIONS_DEBUG}" - IMPORTED_LOCATION ${TBB_${libname}_LIBRARY_DEBUG} - ) - endif() - endforeach() - # End of fix to support different targets - endif() - - mark_as_advanced(TBB_INCLUDE_DIRS TBB_LIBRARIES) - - unset(TBB_ARCHITECTURE) - unset(TBB_BUILD_TYPE) - unset(TBB_LIB_PATH_SUFFIX) - unset(TBB_DEFAULT_SEARCH_DIR) - -endif() diff --git a/cmake/HandleTBB.cmake b/cmake/HandleTBB.cmake index fb944ba5b..393aeb345 100644 --- a/cmake/HandleTBB.cmake +++ b/cmake/HandleTBB.cmake @@ -14,7 +14,7 @@ if (GTSAM_WITH_TBB) endif() # all definitions and link requisites will go via imported targets: # tbb & tbbmalloc - list(APPEND GTSAM_ADDITIONAL_LIBRARIES tbb tbbmalloc) + list(APPEND GTSAM_ADDITIONAL_LIBRARIES TBB::tbb TBB::tbbmalloc) else() set(GTSAM_USE_TBB 0) # This will go into config.h endif() From d3bcfddfb5269cff43e5fcbc5ad988e8e3c8758c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 30 Jan 2020 15:17:04 -0500 Subject: [PATCH 035/206] ISAM2: wrap full update method and error method --- gtsam/nonlinear/nonlinear.i | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 6d101af98..a24bcbb75 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -507,18 +507,18 @@ class ISAM2 { gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices, - gtsam::KeyGroupMap& constrainedKeys, + const gtsam::KeyGroupMap& constrainedKeys, const gtsam::KeyList& noRelinKeys); gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices, - gtsam::KeyGroupMap& constrainedKeys, + const gtsam::KeyGroupMap& constrainedKeys, const gtsam::KeyList& noRelinKeys, const gtsam::KeyList& extraReelimKeys); gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices, - gtsam::KeyGroupMap& constrainedKeys, + const gtsam::KeyGroupMap& constrainedKeys, const gtsam::KeyList& noRelinKeys, const gtsam::KeyList& extraReelimKeys, bool force_relinearize); @@ -527,6 +527,8 @@ class ISAM2 { const gtsam::Values& newTheta, const gtsam::ISAM2UpdateParams& updateParams); + double error(const gtsam::VectorValues& values) const; + gtsam::Values getLinearizationPoint() const; bool valueExists(gtsam::Key key) const; gtsam::Values calculateEstimate() const; From 6992f0d64cafbedea3ec12580bf0c9094ffa55f4 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 3 Feb 2020 16:50:32 -0500 Subject: [PATCH 036/206] added test for full ISAM2 update method --- python/gtsam/tests/test_VisualISAMExample.py | 52 +++++++++++++++++++- python/gtsam/utils/visual_isam.py | 4 +- 2 files changed, 52 insertions(+), 4 deletions(-) diff --git a/python/gtsam/tests/test_VisualISAMExample.py b/python/gtsam/tests/test_VisualISAMExample.py index ebc77e2e3..a25019213 100644 --- a/python/gtsam/tests/test_VisualISAMExample.py +++ b/python/gtsam/tests/test_VisualISAMExample.py @@ -6,15 +6,18 @@ All Rights Reserved See LICENSE for the license information visual_isam unit tests. -Author: Frank Dellaert & Duy Nguyen Ta (Python) +Author: Frank Dellaert & Duy Nguyen Ta & Varun Agrawal (Python) """ +# pylint: disable=maybe-no-member,invalid-name + import unittest import gtsam.utils.visual_data_generator as generator import gtsam.utils.visual_isam as visual_isam -from gtsam import symbol from gtsam.utils.test_case import GtsamTestCase +from gtsam import symbol + class TestVisualISAMExample(GtsamTestCase): """Test class for ISAM2 with visual landmarks.""" @@ -53,6 +56,51 @@ class TestVisualISAMExample(GtsamTestCase): point_j = result.atPoint3(symbol('l', j)) self.gtsamAssertEquals(point_j, truth.points[j], 1e-5) + def test_isam2_update(self): + """ + Test for full version of ISAM2::update method + """ + # Data Options + options = generator.Options() + options.triangle = False + options.nrCameras = 20 + + # iSAM Options + isamOptions = visual_isam.Options() + isamOptions.hardConstraint = False + isamOptions.pointPriors = False + isamOptions.batchInitialization = True + isamOptions.reorderInterval = 10 + isamOptions.alwaysRelinearize = False + + # Generate data + data, truth = generator.generate_data(options) + + # Initialize iSAM with the first pose and points + isam, result, nextPose = visual_isam.initialize(data, truth, isamOptions) + + remove_factor_indices = gtsam.FactorIndices() + constrained_keys = gtsam.KeyGroupMap() + no_relin_keys = gtsam.KeyList() + extra_reelim_keys = gtsam.KeyList() + isamArgs = (remove_factor_indices, + constrained_keys, + no_relin_keys, + extra_reelim_keys, + False) + + # Main loop for iSAM: stepping through all poses + for currentPose in range(nextPose, options.nrCameras): + isam, result = visual_isam.step(data, isam, result, truth, currentPose, isamArgs) + + for i in range(len(truth.cameras)): + pose_i = result.atPose3(symbol(ord('x'), i)) + self.gtsamAssertEquals(pose_i, truth.cameras[i].pose(), 1e-5) + + for j in range(len(truth.points)): + point_j = result.atPoint3(symbol(ord('l'), j)) + self.gtsamAssertEquals(point_j, truth.points[j], 1e-5) + if __name__ == "__main__": unittest.main() diff --git a/python/gtsam/utils/visual_isam.py b/python/gtsam/utils/visual_isam.py index 4ebd8accd..52ec8e32f 100644 --- a/python/gtsam/utils/visual_isam.py +++ b/python/gtsam/utils/visual_isam.py @@ -79,7 +79,7 @@ def initialize(data, truth, options): return isam, result, nextPoseIndex -def step(data, isam, result, truth, currPoseIndex): +def step(data, isam, result, truth, currPoseIndex, isamArgs=()): ''' Do one step isam update @param[in] data: measurement data (odometry and visual measurements and their noiseModels) @@ -123,7 +123,7 @@ def step(data, isam, result, truth, currPoseIndex): # Update ISAM # figure(1)tic - isam.update(newFactors, initialEstimates) + isam.update(newFactors, initialEstimates, *isamArgs) # t=toc plot(frame_i,t,'r.') tic newResult = isam.calculateEstimate() # t=toc plot(frame_i,t,'g.') From 42e8f498e7adaac369e5f9ad21305f37d123094d Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 4 Feb 2020 17:57:44 -0500 Subject: [PATCH 037/206] added tests for newly wrapped isam2 methods --- python/gtsam/tests/test_VisualISAMExample.py | 95 ++++++++++++-------- 1 file changed, 56 insertions(+), 39 deletions(-) diff --git a/python/gtsam/tests/test_VisualISAMExample.py b/python/gtsam/tests/test_VisualISAMExample.py index a25019213..5f30e83ea 100644 --- a/python/gtsam/tests/test_VisualISAMExample.py +++ b/python/gtsam/tests/test_VisualISAMExample.py @@ -21,12 +21,13 @@ from gtsam import symbol class TestVisualISAMExample(GtsamTestCase): """Test class for ISAM2 with visual landmarks.""" - def test_VisualISAMExample(self): - """Test to see if ISAM works as expected for a simple visual SLAM example.""" + + def setUp(self): # Data Options options = generator.Options() options.triangle = False options.nrCameras = 20 + self.options = options # iSAM Options isamOptions = visual_isam.Options() @@ -35,71 +36,87 @@ class TestVisualISAMExample(GtsamTestCase): isamOptions.batchInitialization = True isamOptions.reorderInterval = 10 isamOptions.alwaysRelinearize = False + self.isamOptions = isamOptions # Generate data - data, truth = generator.generate_data(options) + self.data, self.truth = generator.generate_data(options) + def test_VisualISAMExample(self): + """Test to see if ISAM works as expected for a simple visual SLAM example.""" # Initialize iSAM with the first pose and points isam, result, nextPose = visual_isam.initialize( - data, truth, isamOptions) + self.data, self.truth, self.isamOptions) # Main loop for iSAM: stepping through all poses - for currentPose in range(nextPose, options.nrCameras): - isam, result = visual_isam.step(data, isam, result, truth, - currentPose) + for currentPose in range(nextPose, self.options.nrCameras): + isam, result = visual_isam.step(self.data, isam, result, + self.truth, currentPose) - for i, _ in enumerate(truth.cameras): + for i, true_camera in enumerate(self.truth.cameras): pose_i = result.atPose3(symbol('x', i)) - self.gtsamAssertEquals(pose_i, truth.cameras[i].pose(), 1e-5) + self.gtsamAssertEquals(pose_i, true_camera.pose(), 1e-5) - for j, _ in enumerate(truth.points): + for j, expected_point in enumerate(self.truth.points): point_j = result.atPoint3(symbol('l', j)) - self.gtsamAssertEquals(point_j, truth.points[j], 1e-5) + self.gtsamAssertEquals(point_j, expected_point, 1e-5) + + @unittest.skip( + "Need to understand how to calculate error using VectorValues correctly" + ) + def test_isam2_error(self): + #TODO(Varun) fix + # Initialize iSAM with the first pose and points + isam, result, nextPose = visual_isam.initialize( + self.data, self.truth, self.isamOptions) + + # Main loop for iSAM: stepping through all poses + for currentPose in range(nextPose, self.options.nrCameras): + isam, result = visual_isam.step(self.data, isam, result, + self.truth, currentPose) + + values = gtsam.VectorValues() + + estimate = isam.calculateBestEstimate() + + keys = estimate.keys() + + for k in range(keys.size()): + key = keys.at(k) + try: + v = estimate.atPose3(key).matrix() + + except RuntimeError: + v = estimate.atPoint3(key).vector() + values.insert(key, v) + # print(isam.error(values)) def test_isam2_update(self): """ Test for full version of ISAM2::update method """ - # Data Options - options = generator.Options() - options.triangle = False - options.nrCameras = 20 - - # iSAM Options - isamOptions = visual_isam.Options() - isamOptions.hardConstraint = False - isamOptions.pointPriors = False - isamOptions.batchInitialization = True - isamOptions.reorderInterval = 10 - isamOptions.alwaysRelinearize = False - - # Generate data - data, truth = generator.generate_data(options) - # Initialize iSAM with the first pose and points - isam, result, nextPose = visual_isam.initialize(data, truth, isamOptions) + isam, result, nextPose = visual_isam.initialize( + self.data, self.truth, self.isamOptions) remove_factor_indices = gtsam.FactorIndices() constrained_keys = gtsam.KeyGroupMap() no_relin_keys = gtsam.KeyList() extra_reelim_keys = gtsam.KeyList() - isamArgs = (remove_factor_indices, - constrained_keys, - no_relin_keys, - extra_reelim_keys, - False) + isamArgs = (remove_factor_indices, constrained_keys, no_relin_keys, + extra_reelim_keys, False) # Main loop for iSAM: stepping through all poses - for currentPose in range(nextPose, options.nrCameras): - isam, result = visual_isam.step(data, isam, result, truth, currentPose, isamArgs) + for currentPose in range(nextPose, self.options.nrCameras): + isam, result = visual_isam.step(self.data, isam, result, + self.truth, currentPose, isamArgs) - for i in range(len(truth.cameras)): + for i in range(len(self.truth.cameras)): pose_i = result.atPose3(symbol(ord('x'), i)) - self.gtsamAssertEquals(pose_i, truth.cameras[i].pose(), 1e-5) + self.gtsamAssertEquals(pose_i, self.truth.cameras[i].pose(), 1e-5) - for j in range(len(truth.points)): + for j in range(len(self.truth.points)): point_j = result.atPoint3(symbol(ord('l'), j)) - self.gtsamAssertEquals(point_j, truth.points[j], 1e-5) + self.gtsamAssertEquals(point_j, self.truth.points[j], 1e-5) if __name__ == "__main__": From 93ed850c6c01b7c5f66e6db291f00769b46ad95b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 5 Jul 2023 13:14:16 -0400 Subject: [PATCH 038/206] get tests working --- gtsam/nonlinear/nonlinear.i | 10 ++----- python/gtsam/tests/test_VisualISAMExample.py | 29 ++++++++++---------- 2 files changed, 17 insertions(+), 22 deletions(-) diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index a24bcbb75..3721e27e8 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -512,16 +512,10 @@ class ISAM2 { gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices, - const gtsam::KeyGroupMap& constrainedKeys, - const gtsam::KeyList& noRelinKeys, - const gtsam::KeyList& extraReelimKeys); - gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, - const gtsam::Values& newTheta, - const gtsam::FactorIndices& removeFactorIndices, - const gtsam::KeyGroupMap& constrainedKeys, + gtsam::KeyGroupMap& constrainedKeys, const gtsam::KeyList& noRelinKeys, const gtsam::KeyList& extraReelimKeys, - bool force_relinearize); + bool force_relinearize=false); gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, diff --git a/python/gtsam/tests/test_VisualISAMExample.py b/python/gtsam/tests/test_VisualISAMExample.py index 5f30e83ea..c7e72ff8b 100644 --- a/python/gtsam/tests/test_VisualISAMExample.py +++ b/python/gtsam/tests/test_VisualISAMExample.py @@ -16,6 +16,7 @@ import gtsam.utils.visual_data_generator as generator import gtsam.utils.visual_isam as visual_isam from gtsam.utils.test_case import GtsamTestCase +import gtsam from gtsam import symbol @@ -60,10 +61,8 @@ class TestVisualISAMExample(GtsamTestCase): point_j = result.atPoint3(symbol('l', j)) self.gtsamAssertEquals(point_j, expected_point, 1e-5) - @unittest.skip( - "Need to understand how to calculate error using VectorValues correctly" - ) def test_isam2_error(self): + """Test for isam2 error() method.""" #TODO(Varun) fix # Initialize iSAM with the first pose and points isam, result, nextPose = visual_isam.initialize( @@ -78,17 +77,19 @@ class TestVisualISAMExample(GtsamTestCase): estimate = isam.calculateBestEstimate() - keys = estimate.keys() - - for k in range(keys.size()): - key = keys.at(k) + for key in estimate.keys(): try: - v = estimate.atPose3(key).matrix() - + v = gtsam.Pose3.Logmap(estimate.atPose3(key)) except RuntimeError: - v = estimate.atPoint3(key).vector() + v = estimate.atPoint3(key) + + print(key) + print(type(v)) + print(v) values.insert(key, v) - # print(isam.error(values)) + print(isam.error(values)) + + self.assertEqual(isam.error(values), 34212421.14731998) def test_isam2_update(self): """ @@ -98,7 +99,7 @@ class TestVisualISAMExample(GtsamTestCase): isam, result, nextPose = visual_isam.initialize( self.data, self.truth, self.isamOptions) - remove_factor_indices = gtsam.FactorIndices() + remove_factor_indices = [] constrained_keys = gtsam.KeyGroupMap() no_relin_keys = gtsam.KeyList() extra_reelim_keys = gtsam.KeyList() @@ -111,11 +112,11 @@ class TestVisualISAMExample(GtsamTestCase): self.truth, currentPose, isamArgs) for i in range(len(self.truth.cameras)): - pose_i = result.atPose3(symbol(ord('x'), i)) + pose_i = result.atPose3(symbol('x', i)) self.gtsamAssertEquals(pose_i, self.truth.cameras[i].pose(), 1e-5) for j in range(len(self.truth.points)): - point_j = result.atPoint3(symbol(ord('l'), j)) + point_j = result.atPoint3(symbol('l', j)) self.gtsamAssertEquals(point_j, self.truth.points[j], 1e-5) From a14fb8db7d55da85e3d5f860572cae7bab7ee20f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 6 Jul 2023 04:12:39 -0400 Subject: [PATCH 039/206] formatting and fix --- gtsam/nonlinear/nonlinear.i | 7 +++---- python/gtsam/tests/test_VisualISAMExample.py | 7 +------ 2 files changed, 4 insertions(+), 10 deletions(-) diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 3721e27e8..4d0d847c1 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -515,7 +515,7 @@ class ISAM2 { gtsam::KeyGroupMap& constrainedKeys, const gtsam::KeyList& noRelinKeys, const gtsam::KeyList& extraReelimKeys, - bool force_relinearize=false); + bool force_relinearize = false); gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, @@ -548,9 +548,8 @@ class ISAM2 { string dot(const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; - void saveGraph(string s, - const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; + void saveGraph(string s, const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; }; #include diff --git a/python/gtsam/tests/test_VisualISAMExample.py b/python/gtsam/tests/test_VisualISAMExample.py index c7e72ff8b..39f563a35 100644 --- a/python/gtsam/tests/test_VisualISAMExample.py +++ b/python/gtsam/tests/test_VisualISAMExample.py @@ -63,7 +63,6 @@ class TestVisualISAMExample(GtsamTestCase): def test_isam2_error(self): """Test for isam2 error() method.""" - #TODO(Varun) fix # Initialize iSAM with the first pose and points isam, result, nextPose = visual_isam.initialize( self.data, self.truth, self.isamOptions) @@ -83,13 +82,9 @@ class TestVisualISAMExample(GtsamTestCase): except RuntimeError: v = estimate.atPoint3(key) - print(key) - print(type(v)) - print(v) values.insert(key, v) - print(isam.error(values)) - self.assertEqual(isam.error(values), 34212421.14731998) + self.assertAlmostEqual(isam.error(values), 34212421.14731998) def test_isam2_update(self): """ From 5ce7c812a99f2a36aacd5c3352820ae8511060f9 Mon Sep 17 00:00:00 2001 From: Adam Rutkowski Date: Fri, 7 Jul 2023 16:42:58 -0500 Subject: [PATCH 040/206] Added evaluation of expressions when inserting in Values --- gtsam/nonlinear/Values-inl.h | 34 +++++++++++++ gtsam/nonlinear/Values.h | 56 ++++++++++++++++++--- gtsam/nonlinear/tests/testValues.cpp | 74 ++++++++++++++++++++++++++++ 3 files changed, 158 insertions(+), 6 deletions(-) diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index 61ea6684a..a93f9570e 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -243,16 +243,50 @@ namespace gtsam { insert(j, static_cast(GenericValue(val))); } + // partial specialization to insert an expression involving unary operators + template + void Values::insert(Key j, const Eigen::CwiseUnaryOp& val) { + insert(j, val.eval()); + } + + // partial specialization to insert an expression involving binary operators + template + void Values::insert(Key j, const Eigen::CwiseBinaryOp& val) { + insert(j, val.eval()); + } + // update with templated value template void Values::update(Key j, const ValueType& val) { update(j, static_cast(GenericValue(val))); } + // partial specialization to update with an expression involving unary operators + template + void Values::update(Key j, const Eigen::CwiseUnaryOp& val) { + update(j, val.eval()); + } + + // partial specialization to update with an expression involving binary operators + template + void Values::update(Key j, const Eigen::CwiseBinaryOp& val) { + update(j, val.eval()); + } + // insert_or_assign with templated value template void Values::insert_or_assign(Key j, const ValueType& val) { insert_or_assign(j, static_cast(GenericValue(val))); } + template + void Values::insert_or_assign(Key j, const Eigen::CwiseUnaryOp& val) { + insert_or_assign(j, val.eval()); + } + + template + void Values::insert_or_assign(Key j, const Eigen::CwiseBinaryOp& val) { + insert_or_assign(j, val.eval()); + } + } diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index 75cfc24e0..7e95852a5 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -29,11 +29,6 @@ #include #include -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION -#include -#endif - - #include #include #include @@ -245,6 +240,31 @@ namespace gtsam { template void insert(Key j, const ValueType& val); + /** Partial specialization that allows passing a unary Eigen expression for val. + * + * A unary expression is an expression such as 2*a or -a, where a is a valid Vector or Matrix type. + * The typical usage is for types Point2 (i.e. Eigen::Vector2d) or Point3 (i.e. Eigen::Vector3d). + * For example, together with the partial specialization for binary operators, a user may call insert(j, 2*a + M*b - c), + * where M is an appropriately sized matrix (such as a rotation matrix). + * Thus, it isn't necessary to explicitly evaluate the Eigen expression, as in insert(j, (2*a + M*b - c).eval()), + * nor is it necessary to first assign the expression to a separate variable. + */ + template + void insert(Key j, const Eigen::CwiseUnaryOp& val); + + /** Partial specialization that allows passing a binary Eigen expression for val. + * + * A binary expression is an expression such as a + b, where a and b are valid Vector or Matrix + * types of compatible size. + * The typical usage is for types Point2 (i.e. Eigen::Vector2d) or Point3 (i.e. Eigen::Vector3d). + * For example, together with the partial specialization for binary operators, a user may call insert(j, 2*a + M*b - c), + * where M is an appropriately sized matrix (such as a rotation matrix). + * Thus, it isn't necessary to explicitly evaluate the Eigen expression, as in insert(j, (2*a + M*b - c).eval()), + * nor is it necessary to first assign the expression to a separate variable. + */ + template + void insert(Key j, const Eigen::CwiseBinaryOp& val); + /// version for double void insertDouble(Key j, double c) { insert(j,c); } @@ -258,6 +278,18 @@ namespace gtsam { template void update(Key j, const T& val); + /** Partial specialization that allows passing a unary Eigen expression for val, + * similar to the partial specialization for insert. + */ + template + void update(Key j, const Eigen::CwiseUnaryOp& val); + + /** Partial specialization that allows passing a binary Eigen expression for val, + * similar to the partial specialization for insert. + */ + template + void update(Key j, const Eigen::CwiseBinaryOp& val); + /** update the current available values without adding new ones */ void update(const Values& values); @@ -266,7 +298,7 @@ namespace gtsam { /** * Update a set of variables. - * If any variable key doe not exist, then perform an insert. + * If any variable key does not exist, then perform an insert. */ void insert_or_assign(const Values& values); @@ -274,6 +306,18 @@ namespace gtsam { template void insert_or_assign(Key j, const ValueType& val); + /** Partial specialization that allows passing a unary Eigen expression for val, + * similar to the partial specialization for insert. + */ + template + void insert_or_assign(Key j, const Eigen::CwiseUnaryOp& val); + + /** Partial specialization that allows passing a binary Eigen expression for val, + * similar to the partial specialization for insert. + */ + template + void insert_or_assign(Key j, const Eigen::CwiseBinaryOp& val); + /** Remove a variable from the config, throws KeyDoesNotExist if j is not present */ void erase(Key j); diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index 1af18ef95..2a7bede30 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -134,6 +134,44 @@ TEST( Values, insert_good ) CHECK(assert_equal(expected, cfg1)); } +/* ************************************************************************* */ +TEST( Values, insert_expression ) +{ + Point2 p1(0.1, 0.2); + Point2 p2(0.3, 0.4); + Point2 p3(0.5, 0.6); + Point2 p4(p1 + p2 + p3); + Point2 p5(-p1); + Point2 p6(2.0*p1); + + Values cfg1, cfg2; + cfg1.insert(key1, p1 + p2 + p3); + cfg1.insert(key2, -p1); + cfg1.insert(key3, 2.0*p1); + cfg2.insert(key1, p4); + cfg2.insert(key2, p5); + cfg2.insert(key3, p6); + + CHECK(assert_equal(cfg1, cfg2)); + + Point3 p7(0.1, 0.2, 0.3); + Point3 p8(0.4, 0.5, 0.6); + Point3 p9(0.7, 0.8, 0.9); + Point3 p10(p7 + p8 + p9); + Point3 p11(-p7); + Point3 p12(2.0*p7); + + Values cfg3, cfg4; + cfg3.insert(key1, p7 + p8 + p9); + cfg3.insert(key2, -p7); + cfg3.insert(key3, 2.0*p7); + cfg4.insert(key1, p10); + cfg4.insert(key2, p11); + cfg4.insert(key3, p12); + + CHECK(assert_equal(cfg3, cfg4)); +} + /* ************************************************************************* */ TEST( Values, insert_bad ) { @@ -167,6 +205,23 @@ TEST( Values, update_element ) CHECK(assert_equal((Vector)v2, cfg.at(key1))); } +/* ************************************************************************* */ +TEST(Values, update_element_with_expression) +{ + Values cfg; + Vector3 v1(5.0, 6.0, 7.0); + Vector3 v2(8.0, 9.0, 1.0); + + cfg.insert(key1, v1); + CHECK(cfg.size() == 1); + CHECK(assert_equal((Vector)v1, cfg.at(key1))); + + cfg.update(key1, 2.0*v1 + v2); + CHECK(cfg.size() == 1); + CHECK(assert_equal((2.0*v1 + v2).eval(), cfg.at(key1))); +} + +/* ************************************************************************* */ TEST(Values, InsertOrAssign) { Values values; Key X(0); @@ -183,6 +238,25 @@ TEST(Values, InsertOrAssign) { EXPECT(assert_equal(values.at(X), y)); } +/* ************************************************************************* */ +TEST(Values, InsertOrAssignWithExpression) { + Values values,expected; + Key X(0); + Vector3 x{1.0, 2.0, 3.0}; + Vector3 y{4.0, 5.0, 6.0}; + + CHECK(values.size() == 0); + // This should perform an insert. + Vector3 z = x + y; + values.insert_or_assign(X, x + y); + EXPECT(assert_equal(values.at(X), z)); + + // This should perform an update. + z = 2.0*x - 3.0*y; + values.insert_or_assign(X, 2.0*x - 3.0*y); + EXPECT(assert_equal(values.at(X), z)); +} + /* ************************************************************************* */ TEST(Values, basic_functions) { From 609351e25b1ccc0ed8e24a187e907467e1c2bf87 Mon Sep 17 00:00:00 2001 From: Adam Rutkowski Date: Thu, 13 Jul 2023 12:45:32 -0500 Subject: [PATCH 041/206] added boost unique_ptr header that was removed in previous commit --- gtsam/nonlinear/Values.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index 7e95852a5..21e8e281f 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -29,6 +29,11 @@ #include #include +#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#include +#endif + + #include #include #include From 64cd43636235bfba3cb42ba5a1c61ee89b20c2b0 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 15 Jul 2023 16:53:17 -0400 Subject: [PATCH 042/206] update InconsistentEliminationRequested to show leftover keys --- .../inference/EliminateableFactorGraph-inst.h | 10 +-- gtsam/inference/inferenceExceptions.h | 63 ++++++++++++++----- .../linear/tests/testGaussianFactorGraph.cpp | 52 ++++++++++++--- 3 files changed, 95 insertions(+), 30 deletions(-) diff --git a/gtsam/inference/EliminateableFactorGraph-inst.h b/gtsam/inference/EliminateableFactorGraph-inst.h index 8a524e353..f6b11f785 100644 --- a/gtsam/inference/EliminateableFactorGraph-inst.h +++ b/gtsam/inference/EliminateableFactorGraph-inst.h @@ -74,8 +74,9 @@ namespace gtsam { EliminationTreeType etree(asDerived(), (*variableIndex).get(), ordering); const auto [bayesNet, factorGraph] = etree.eliminate(function); // If any factors are remaining, the ordering was incomplete - if(!factorGraph->empty()) - throw InconsistentEliminationRequested(); + if(!factorGraph->empty()) { + throw InconsistentEliminationRequested(factorGraph->keys()); + } // Return the Bayes net return bayesNet; } @@ -136,8 +137,9 @@ namespace gtsam { JunctionTreeType junctionTree(etree); const auto [bayesTree, factorGraph] = junctionTree.eliminate(function); // If any factors are remaining, the ordering was incomplete - if(!factorGraph->empty()) - throw InconsistentEliminationRequested(); + if(!factorGraph->empty()) { + throw InconsistentEliminationRequested(factorGraph->keys()); + } // Return the Bayes tree return bayesTree; } diff --git a/gtsam/inference/inferenceExceptions.h b/gtsam/inference/inferenceExceptions.h index fa3e3cb25..061abeb85 100644 --- a/gtsam/inference/inferenceExceptions.h +++ b/gtsam/inference/inferenceExceptions.h @@ -12,30 +12,59 @@ /** * @file inferenceExceptions.h * @brief Exceptions that may be thrown by inference algorithms - * @author Richard Roberts + * @author Richard Roberts, Varun Agrawal * @date Apr 25, 2013 */ #pragma once #include + #include +#include namespace gtsam { - /** An inference algorithm was called with inconsistent arguments. The factor graph, ordering, or - * variable index were inconsistent with each other, or a full elimination routine was called - * with an ordering that does not include all of the variables. */ - class InconsistentEliminationRequested : public std::exception { - public: - InconsistentEliminationRequested() noexcept {} - ~InconsistentEliminationRequested() noexcept override {} - const char* what() const noexcept override { - return - "An inference algorithm was called with inconsistent arguments. The\n" - "factor graph, ordering, or variable index were inconsistent with each\n" - "other, or a full elimination routine was called with an ordering that\n" - "does not include all of the variables."; - } - }; +/** An inference algorithm was called with inconsistent arguments. The factor + * graph, ordering, or variable index were inconsistent with each other, or a + * full elimination routine was called with an ordering that does not include + * all of the variables. */ +class InconsistentEliminationRequested : public std::exception { + KeySet keys_; + const KeyFormatter& keyFormatter = DefaultKeyFormatter; -} + public: + InconsistentEliminationRequested() noexcept {} + + InconsistentEliminationRequested( + const KeySet& keys, + const KeyFormatter& key_formatter = DefaultKeyFormatter) + : keys_(keys), keyFormatter(key_formatter) {} + + ~InconsistentEliminationRequested() noexcept override {} + const char* what() const noexcept override { + // Format keys for printing + std::stringstream sstr; + for (auto key : keys_) { + sstr << keyFormatter(key) << ", "; + } + std::string keys = sstr.str(); + // remove final comma and space. + keys.pop_back(); + keys.pop_back(); + + static std::string msg = + "An inference algorithm was called with inconsistent " + "arguments. " + "The\n" + "factor graph, ordering, or variable index were " + "inconsistent with " + "each\n" + "other, or a full elimination routine was called with " + "an ordering " + "that\n" + "does not include all of the variables.\n"; + msg += ("Leftover keys after elimination: " + keys); + return msg.c_str(); + } +}; +} // namespace gtsam diff --git a/gtsam/linear/tests/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp index e9e626296..b140c3d85 100644 --- a/gtsam/linear/tests/testGaussianFactorGraph.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraph.cpp @@ -18,16 +18,16 @@ * @author Richard Roberts **/ -#include -#include -#include -#include -#include -#include -#include - -#include #include +#include +#include +#include +#include +#include +#include +#include +#include +#include using namespace std; using namespace gtsam; @@ -435,6 +435,40 @@ TEST(GaussianFactorGraph, ProbPrime) { EXPECT_DOUBLES_EQUAL(expected, gfg.probPrime(values), 1e-12); } +TEST(GaussianFactorGraph, InconsistentEliminationMessage) { + // Create empty graph + GaussianFactorGraph fg; + SharedDiagonal unit2 = noiseModel::Unit::Create(2); + + using gtsam::symbol_shorthand::X; + fg.emplace_shared(0, 10 * I_2x2, -1.0 * Vector::Ones(2), + unit2); + fg.emplace_shared(0, -10 * I_2x2, 1, 10 * I_2x2, + Vector2(2.0, -1.0), unit2); + fg.emplace_shared(1, -5 * I_2x2, 2, 5 * I_2x2, + Vector2(-1.0, 1.5), unit2); + fg.emplace_shared(2, -5 * I_2x2, X(3), 5 * I_2x2, + Vector2(-1.0, 1.5), unit2); + + Ordering ordering{0, 1}; + + try { + fg.eliminateSequential(ordering); + } catch (const exception& exc) { + std::string expected_exception_message = "An inference algorithm was called with inconsistent " + "arguments. " + "The\n" + "factor graph, ordering, or variable index were " + "inconsistent with " + "each\n" + "other, or a full elimination routine was called with " + "an ordering " + "that\n" + "does not include all of the variables.\n" + "Leftover keys after elimination: 2, x3"; + EXPECT(expected_exception_message == exc.what()); + } +} /* ************************************************************************* */ int main() { TestResult tr; From 953150649208b1a8f7b3bb3f4dae097dfcb2600a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 28 Jun 2023 16:13:54 -0400 Subject: [PATCH 043/206] rename variables to be agnostic to underlying data structure --- gtsam/hybrid/GaussianMixture.cpp | 28 ++++++------- gtsam/hybrid/GaussianMixture.h | 11 +++-- gtsam/hybrid/HybridBayesNet.cpp | 49 ++++++++++++---------- gtsam/hybrid/HybridBayesNet.h | 4 +- gtsam/hybrid/HybridBayesTree.cpp | 17 ++++---- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 7 +--- gtsam/hybrid/HybridGaussianFactorGraph.h | 7 ++-- 7 files changed, 61 insertions(+), 62 deletions(-) diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp index 546d0200b..659d44423 100644 --- a/gtsam/hybrid/GaussianMixture.cpp +++ b/gtsam/hybrid/GaussianMixture.cpp @@ -228,19 +228,19 @@ std::set DiscreteKeysAsSet(const DiscreteKeys &discreteKeys) { /** * @brief Helper function to get the pruner functional. * - * @param decisionTree The probability decision tree of only discrete keys. + * @param discreteProbs The probabilities of only discrete keys. * @return std::function &, const GaussianConditional::shared_ptr &)> */ std::function &, const GaussianConditional::shared_ptr &)> -GaussianMixture::prunerFunc(const DecisionTreeFactor &decisionTree) { +GaussianMixture::prunerFunc(const DecisionTreeFactor &discreteProbs) { // Get the discrete keys as sets for the decision tree // and the gaussian mixture. - auto decisionTreeKeySet = DiscreteKeysAsSet(decisionTree.discreteKeys()); + auto discreteProbsKeySet = DiscreteKeysAsSet(discreteProbs.discreteKeys()); auto gaussianMixtureKeySet = DiscreteKeysAsSet(this->discreteKeys()); - auto pruner = [decisionTree, decisionTreeKeySet, gaussianMixtureKeySet]( + auto pruner = [discreteProbs, discreteProbsKeySet, gaussianMixtureKeySet]( const Assignment &choices, const GaussianConditional::shared_ptr &conditional) -> GaussianConditional::shared_ptr { @@ -249,8 +249,8 @@ GaussianMixture::prunerFunc(const DecisionTreeFactor &decisionTree) { // Case where the gaussian mixture has the same // discrete keys as the decision tree. - if (gaussianMixtureKeySet == decisionTreeKeySet) { - if (decisionTree(values) == 0.0) { + if (gaussianMixtureKeySet == discreteProbsKeySet) { + if (discreteProbs(values) == 0.0) { // empty aka null pointer std::shared_ptr null; return null; @@ -259,10 +259,10 @@ GaussianMixture::prunerFunc(const DecisionTreeFactor &decisionTree) { } } else { std::vector set_diff; - std::set_difference(decisionTreeKeySet.begin(), decisionTreeKeySet.end(), - gaussianMixtureKeySet.begin(), - gaussianMixtureKeySet.end(), - std::back_inserter(set_diff)); + std::set_difference( + discreteProbsKeySet.begin(), discreteProbsKeySet.end(), + gaussianMixtureKeySet.begin(), gaussianMixtureKeySet.end(), + std::back_inserter(set_diff)); const std::vector assignments = DiscreteValues::CartesianProduct(set_diff); @@ -272,7 +272,7 @@ GaussianMixture::prunerFunc(const DecisionTreeFactor &decisionTree) { // If any one of the sub-branches are non-zero, // we need this conditional. - if (decisionTree(augmented_values) > 0.0) { + if (discreteProbs(augmented_values) > 0.0) { return conditional; } } @@ -285,12 +285,12 @@ GaussianMixture::prunerFunc(const DecisionTreeFactor &decisionTree) { } /* *******************************************************************************/ -void GaussianMixture::prune(const DecisionTreeFactor &decisionTree) { - auto decisionTreeKeySet = DiscreteKeysAsSet(decisionTree.discreteKeys()); +void GaussianMixture::prune(const DecisionTreeFactor &discreteProbs) { + auto discreteProbsKeySet = DiscreteKeysAsSet(discreteProbs.discreteKeys()); auto gmKeySet = DiscreteKeysAsSet(this->discreteKeys()); // Functional which loops over all assignments and create a set of // GaussianConditionals - auto pruner = prunerFunc(decisionTree); + auto pruner = prunerFunc(discreteProbs); auto pruned_conditionals = conditionals_.apply(pruner); conditionals_.root_ = pruned_conditionals.root_; diff --git a/gtsam/hybrid/GaussianMixture.h b/gtsam/hybrid/GaussianMixture.h index 2d715c6e3..0b68fcfd0 100644 --- a/gtsam/hybrid/GaussianMixture.h +++ b/gtsam/hybrid/GaussianMixture.h @@ -74,13 +74,13 @@ class GTSAM_EXPORT GaussianMixture /** * @brief Helper function to get the pruner functor. * - * @param decisionTree The pruned discrete probability decision tree. + * @param discreteProbs The pruned discrete probabilities. * @return std::function &, const GaussianConditional::shared_ptr &)> */ std::function &, const GaussianConditional::shared_ptr &)> - prunerFunc(const DecisionTreeFactor &decisionTree); + prunerFunc(const DecisionTreeFactor &discreteProbs); public: /// @name Constructors @@ -234,12 +234,11 @@ class GTSAM_EXPORT GaussianMixture /** * @brief Prune the decision tree of Gaussian factors as per the discrete - * `decisionTree`. + * `discreteProbs`. * - * @param decisionTree A pruned decision tree of discrete keys where the - * leaves are probabilities. + * @param discreteProbs A pruned set of probabilities for the discrete keys. */ - void prune(const DecisionTreeFactor &decisionTree); + void prune(const DecisionTreeFactor &discreteProbs); /** * @brief Merge the Gaussian Factor Graphs in `this` and `sum` while diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index 68129bc27..ab68e170f 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -39,41 +39,41 @@ bool HybridBayesNet::equals(const This &bn, double tol) const { /* ************************************************************************* */ DecisionTreeFactor::shared_ptr HybridBayesNet::discreteConditionals() const { - AlgebraicDecisionTree decisionTree; + AlgebraicDecisionTree discreteProbs; // The canonical decision tree factor which will get // the discrete conditionals added to it. - DecisionTreeFactor dtFactor; + DecisionTreeFactor discreteProbsFactor; for (auto &&conditional : *this) { if (conditional->isDiscrete()) { // Convert to a DecisionTreeFactor and add it to the main factor. DecisionTreeFactor f(*conditional->asDiscrete()); - dtFactor = dtFactor * f; + discreteProbsFactor = discreteProbsFactor * f; } } - return std::make_shared(dtFactor); + return std::make_shared(discreteProbsFactor); } /* ************************************************************************* */ /** * @brief Helper function to get the pruner functional. * - * @param prunedDecisionTree The prob. decision tree of only discrete keys. + * @param prunedDiscreteProbs The prob. decision tree of only discrete keys. * @param conditional Conditional to prune. Used to get full assignment. * @return std::function &, double)> */ std::function &, double)> prunerFunc( - const DecisionTreeFactor &prunedDecisionTree, + const DecisionTreeFactor &prunedDiscreteProbs, const HybridConditional &conditional) { // Get the discrete keys as sets for the decision tree // and the Gaussian mixture. - std::set decisionTreeKeySet = - DiscreteKeysAsSet(prunedDecisionTree.discreteKeys()); + std::set discreteProbsKeySet = + DiscreteKeysAsSet(prunedDiscreteProbs.discreteKeys()); std::set conditionalKeySet = DiscreteKeysAsSet(conditional.discreteKeys()); - auto pruner = [prunedDecisionTree, decisionTreeKeySet, conditionalKeySet]( + auto pruner = [prunedDiscreteProbs, discreteProbsKeySet, conditionalKeySet]( const Assignment &choices, double probability) -> double { // This corresponds to 0 probability @@ -83,8 +83,8 @@ std::function &, double)> prunerFunc( DiscreteValues values(choices); // Case where the Gaussian mixture has the same // discrete keys as the decision tree. - if (conditionalKeySet == decisionTreeKeySet) { - if (prunedDecisionTree(values) == 0) { + if (conditionalKeySet == discreteProbsKeySet) { + if (prunedDiscreteProbs(values) == 0) { return pruned_prob; } else { return probability; @@ -114,11 +114,12 @@ std::function &, double)> prunerFunc( } // Now we generate the full assignment by enumerating - // over all keys in the prunedDecisionTree. + // over all keys in the prunedDiscreteProbs. // First we find the differing keys std::vector set_diff; - std::set_difference(decisionTreeKeySet.begin(), decisionTreeKeySet.end(), - conditionalKeySet.begin(), conditionalKeySet.end(), + std::set_difference(discreteProbsKeySet.begin(), + discreteProbsKeySet.end(), conditionalKeySet.begin(), + conditionalKeySet.end(), std::back_inserter(set_diff)); // Now enumerate over all assignments of the differing keys @@ -130,7 +131,7 @@ std::function &, double)> prunerFunc( // If any one of the sub-branches are non-zero, // we need this probability. - if (prunedDecisionTree(augmented_values) > 0.0) { + if (prunedDiscreteProbs(augmented_values) > 0.0) { return probability; } } @@ -144,8 +145,8 @@ std::function &, double)> prunerFunc( /* ************************************************************************* */ void HybridBayesNet::updateDiscreteConditionals( - const DecisionTreeFactor &prunedDecisionTree) { - KeyVector prunedTreeKeys = prunedDecisionTree.keys(); + const DecisionTreeFactor &prunedDiscreteProbs) { + KeyVector prunedTreeKeys = prunedDiscreteProbs.keys(); // Loop with index since we need it later. for (size_t i = 0; i < this->size(); i++) { @@ -157,7 +158,7 @@ void HybridBayesNet::updateDiscreteConditionals( auto discreteTree = std::dynamic_pointer_cast(discrete); DecisionTreeFactor::ADT prunedDiscreteTree = - discreteTree->apply(prunerFunc(prunedDecisionTree, *conditional)); + discreteTree->apply(prunerFunc(prunedDiscreteProbs, *conditional)); // Create the new (hybrid) conditional KeyVector frontals(discrete->frontals().begin(), @@ -175,10 +176,12 @@ void HybridBayesNet::updateDiscreteConditionals( /* ************************************************************************* */ HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) { // Get the decision tree of only the discrete keys - auto discreteConditionals = this->discreteConditionals(); - const auto decisionTree = discreteConditionals->prune(maxNrLeaves); + DecisionTreeFactor::shared_ptr discreteConditionals = + this->discreteConditionals(); + const DecisionTreeFactor prunedDiscreteProbs = + discreteConditionals->prune(maxNrLeaves); - this->updateDiscreteConditionals(decisionTree); + this->updateDiscreteConditionals(prunedDiscreteProbs); /* To Prune, we visitWith every leaf in the GaussianMixture. * For each leaf, using the assignment we can check the discrete decision tree @@ -190,12 +193,12 @@ HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) { HybridBayesNet prunedBayesNetFragment; // Go through all the conditionals in the - // Bayes Net and prune them as per decisionTree. + // Bayes Net and prune them as per prunedDiscreteProbs. for (auto &&conditional : *this) { if (auto gm = conditional->asMixture()) { // Make a copy of the Gaussian mixture and prune it! auto prunedGaussianMixture = std::make_shared(*gm); - prunedGaussianMixture->prune(decisionTree); // imperative :-( + prunedGaussianMixture->prune(prunedDiscreteProbs); // imperative :-( // Type-erase and add to the pruned Bayes Net fragment. prunedBayesNetFragment.push_back(prunedGaussianMixture); diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h index 2b0042b8d..23fc4d5d3 100644 --- a/gtsam/hybrid/HybridBayesNet.h +++ b/gtsam/hybrid/HybridBayesNet.h @@ -224,9 +224,9 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { /** * @brief Update the discrete conditionals with the pruned versions. * - * @param prunedDecisionTree + * @param prunedDiscreteProbs */ - void updateDiscreteConditionals(const DecisionTreeFactor &prunedDecisionTree); + void updateDiscreteConditionals(const DecisionTreeFactor &prunedDiscreteProbs); #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ diff --git a/gtsam/hybrid/HybridBayesTree.cpp b/gtsam/hybrid/HybridBayesTree.cpp index b252e613e..ae8fa0378 100644 --- a/gtsam/hybrid/HybridBayesTree.cpp +++ b/gtsam/hybrid/HybridBayesTree.cpp @@ -173,19 +173,18 @@ VectorValues HybridBayesTree::optimize(const DiscreteValues& assignment) const { /* ************************************************************************* */ void HybridBayesTree::prune(const size_t maxNrLeaves) { - auto decisionTree = - this->roots_.at(0)->conditional()->asDiscrete(); + auto discreteProbs = this->roots_.at(0)->conditional()->asDiscrete(); - DecisionTreeFactor prunedDecisionTree = decisionTree->prune(maxNrLeaves); - decisionTree->root_ = prunedDecisionTree.root_; + DecisionTreeFactor prunedDiscreteProbs = discreteProbs->prune(maxNrLeaves); + discreteProbs->root_ = prunedDiscreteProbs.root_; /// Helper struct for pruning the hybrid bayes tree. struct HybridPrunerData { /// The discrete decision tree after pruning. - DecisionTreeFactor prunedDecisionTree; - HybridPrunerData(const DecisionTreeFactor& prunedDecisionTree, + DecisionTreeFactor prunedDiscreteProbs; + HybridPrunerData(const DecisionTreeFactor& prunedDiscreteProbs, const HybridBayesTree::sharedNode& parentClique) - : prunedDecisionTree(prunedDecisionTree) {} + : prunedDiscreteProbs(prunedDiscreteProbs) {} /** * @brief A function used during tree traversal that operates on each node @@ -205,13 +204,13 @@ void HybridBayesTree::prune(const size_t maxNrLeaves) { if (conditional->isHybrid()) { auto gaussianMixture = conditional->asMixture(); - gaussianMixture->prune(parentData.prunedDecisionTree); + gaussianMixture->prune(parentData.prunedDiscreteProbs); } return parentData; } }; - HybridPrunerData rootData(prunedDecisionTree, 0); + HybridPrunerData rootData(prunedDiscreteProbs, 0); { treeTraversal::no_op visitorPost; // Limits OpenMP threads since we're mixing TBB and OpenMP diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index f0d28e9f5..8b9d62822 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -190,7 +190,8 @@ discreteElimination(const HybridGaussianFactorGraph &factors, /* ************************************************************************ */ // If any GaussianFactorGraph in the decision tree contains a nullptr, convert // that leaf to an empty GaussianFactorGraph. Needed since the DecisionTree will -// otherwise create a GFG with a single (null) factor, which doesn't register as null. +// otherwise create a GFG with a single (null) factor, +// which doesn't register as null. GaussianFactorGraphTree removeEmpty(const GaussianFactorGraphTree &sum) { auto emptyGaussian = [](const GaussianFactorGraph &graph) { bool hasNull = @@ -246,10 +247,6 @@ hybridElimination(const HybridGaussianFactorGraph &factors, // Perform elimination! DecisionTree eliminationResults(factorGraphTree, eliminate); -#ifdef HYBRID_TIMING - tictoc_print_(); -#endif - // Separate out decision tree into conditionals and remaining factors. const auto [conditionals, newFactors] = unzip(eliminationResults); diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h index f911b135b..421e69aa0 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.h +++ b/gtsam/hybrid/HybridGaussianFactorGraph.h @@ -112,8 +112,8 @@ class GTSAM_EXPORT HybridGaussianFactorGraph public: using Base = HybridFactorGraph; using This = HybridGaussianFactorGraph; ///< this class - using BaseEliminateable = - EliminateableFactorGraph; ///< for elimination + ///< for elimination + using BaseEliminateable = EliminateableFactorGraph; using shared_ptr = std::shared_ptr; ///< shared_ptr to This using Values = gtsam::Values; ///< backwards compatibility @@ -148,7 +148,8 @@ class GTSAM_EXPORT HybridGaussianFactorGraph /// @name Standard Interface /// @{ - using Base::error; // Expose error(const HybridValues&) method.. + /// Expose error(const HybridValues&) method. + using Base::error; /** * @brief Compute error for each discrete assignment, From 0515329c389cb8c13a65ddc7c629b684cef74f32 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 28 Jun 2023 18:24:12 -0400 Subject: [PATCH 044/206] DiscreteFactor serialization --- gtsam/discrete/DiscreteFactor.h | 27 ++++++++++++++++++--------- 1 file changed, 18 insertions(+), 9 deletions(-) diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h index f00ebc499..6da0e98ae 100644 --- a/gtsam/discrete/DiscreteFactor.h +++ b/gtsam/discrete/DiscreteFactor.h @@ -36,18 +36,16 @@ class HybridValues; * @ingroup discrete */ class GTSAM_EXPORT DiscreteFactor: public Factor { - -public: - + public: // typedefs needed to play nice with gtsam - typedef DiscreteFactor This; ///< This class - typedef std::shared_ptr shared_ptr; ///< shared_ptr to this class - typedef Factor Base; ///< Our base class + typedef DiscreteFactor This; ///< This class + typedef std::shared_ptr + shared_ptr; ///< shared_ptr to this class + typedef Factor Base; ///< Our base class - using Values = DiscreteValues; ///< backwards compatibility - -public: + using Values = DiscreteValues; ///< backwards compatibility + public: /// @name Standard Constructors /// @{ @@ -124,6 +122,17 @@ public: const Names& names = {}) const = 0; /// @} + + private: +#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(ADT); + } +#endif }; // DiscreteFactor From 3ee46ec2802dd4cfc396307e938483c05bff82b8 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 28 Jun 2023 18:24:39 -0400 Subject: [PATCH 045/206] test to construct DecisionTreeFactor from DiscreteConditional --- gtsam/discrete/tests/testDecisionTreeFactor.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/gtsam/discrete/tests/testDecisionTreeFactor.cpp b/gtsam/discrete/tests/testDecisionTreeFactor.cpp index 3dbb3e64f..57584a03b 100644 --- a/gtsam/discrete/tests/testDecisionTreeFactor.cpp +++ b/gtsam/discrete/tests/testDecisionTreeFactor.cpp @@ -51,6 +51,11 @@ TEST( DecisionTreeFactor, constructors) // Assert that error = -log(value) EXPECT_DOUBLES_EQUAL(-log(f1(values)), f1.error(values), 1e-9); + + // Construct from DiscreteConditional + DiscreteConditional conditional(X | Y = "1/1 2/3 1/4"); + DecisionTreeFactor f4(conditional); + EXPECT_DOUBLES_EQUAL(0.8, f4(values), 1e-9); } /* ************************************************************************* */ From 2994c55b5176b0c20d8df445b12ac2f691ee08c3 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 28 Jun 2023 18:25:23 -0400 Subject: [PATCH 046/206] fix DiscreteFactor serialization --- gtsam/discrete/DiscreteFactor.h | 1 - 1 file changed, 1 deletion(-) diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h index 6da0e98ae..2132e1cc8 100644 --- a/gtsam/discrete/DiscreteFactor.h +++ b/gtsam/discrete/DiscreteFactor.h @@ -130,7 +130,6 @@ class GTSAM_EXPORT DiscreteFactor: public Factor { template void serialize(ARCHIVE& ar, const unsigned int /*version*/) { ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(ADT); } #endif }; From c06861e7da13e8fa67c89415797773a90611b316 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 28 Jun 2023 18:25:53 -0400 Subject: [PATCH 047/206] DecisionTreeFactor cardinalities getter --- gtsam/discrete/DecisionTreeFactor.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h index 95054bcdb..0cfda6b7d 100644 --- a/gtsam/discrete/DecisionTreeFactor.h +++ b/gtsam/discrete/DecisionTreeFactor.h @@ -119,6 +119,8 @@ namespace gtsam { static double safe_div(const double& a, const double& b); + std::map cardinalities() const { return cardinalities_; } + size_t cardinality(Key j) const { return cardinalities_.at(j); } /// divide by factor f (safely) From fceb290e8058fcb930f71fa10e3122628b0a28db Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 28 Jun 2023 19:09:08 -0400 Subject: [PATCH 048/206] add TableFactor constructor from DiscreteConditional --- gtsam/discrete/DecisionTreeFactor.cpp | 18 +++++++++++++----- gtsam/discrete/DecisionTreeFactor.h | 3 +++ gtsam/discrete/TableFactor.cpp | 7 ++++++- gtsam/discrete/TableFactor.h | 18 +++++++++++------- gtsam/discrete/tests/testTableFactor.cpp | 13 +++++++++++-- 5 files changed, 44 insertions(+), 15 deletions(-) diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp index 5fb5ae2e6..04e29024c 100644 --- a/gtsam/discrete/DecisionTreeFactor.cpp +++ b/gtsam/discrete/DecisionTreeFactor.cpp @@ -181,6 +181,15 @@ namespace gtsam { return result; } + /* ************************************************************************ */ + std::vector DecisionTreeFactor::probabilities() const { + std::vector probs; + for (auto&& [key, value] : enumerate()) { + probs.push_back(value); + } + return probs; + } + /* ************************************************************************ */ DiscreteKeys DecisionTreeFactor::discreteKeys() const { DiscreteKeys result; @@ -306,11 +315,10 @@ namespace gtsam { // Get the probabilities in the decision tree so we can threshold. std::vector probabilities; - this->visitLeaf([&](const Leaf& leaf) { - size_t nrAssignments = leaf.nrAssignments(); - double prob = leaf.constant(); - probabilities.insert(probabilities.end(), nrAssignments, prob); - }); + // NOTE(Varun) this is potentially slow due to the cartesian product + for (auto&& [assignment, prob] : this->enumerate()) { + probabilities.push_back(prob); + } // The number of probabilities can be lower than max_leaves if (probabilities.size() <= N) { diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h index 0cfda6b7d..42639095f 100644 --- a/gtsam/discrete/DecisionTreeFactor.h +++ b/gtsam/discrete/DecisionTreeFactor.h @@ -181,6 +181,9 @@ namespace gtsam { /// Enumerate all values into a map from values to double. std::vector> enumerate() const; + /// Get all the probabilities in order of assignment values + std::vector probabilities() const; + /// Return all the discrete keys associated with this factor. DiscreteKeys discreteKeys() const; diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp index 5fe3cd9d1..c59b7b72c 100644 --- a/gtsam/discrete/TableFactor.cpp +++ b/gtsam/discrete/TableFactor.cpp @@ -13,11 +13,12 @@ * @file TableFactor.cpp * @brief discrete factor * @date May 4, 2023 - * @author Yoonwoo Kim + * @author Yoonwoo Kim, Varun Agrawal */ #include #include +#include #include #include @@ -56,6 +57,10 @@ TableFactor::TableFactor(const DiscreteKeys& dkeys, sort(sorted_dkeys_.begin(), sorted_dkeys_.end()); } +/* ************************************************************************ */ +TableFactor::TableFactor(const DiscreteConditional& c) + : TableFactor(c.discreteKeys(), c.probabilities()) {} + /* ************************************************************************ */ Eigen::SparseVector TableFactor::Convert( const std::vector& table) { diff --git a/gtsam/discrete/TableFactor.h b/gtsam/discrete/TableFactor.h index 1462180e0..08c675b67 100644 --- a/gtsam/discrete/TableFactor.h +++ b/gtsam/discrete/TableFactor.h @@ -12,7 +12,7 @@ /** * @file TableFactor.h * @date May 4, 2023 - * @author Yoonwoo Kim + * @author Yoonwoo Kim, Varun Agrawal */ #pragma once @@ -32,6 +32,7 @@ namespace gtsam { +class DiscreteConditional; class HybridValues; /** @@ -57,10 +58,10 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor { /** * @brief Uses lazy cartesian product to find nth entry in the cartesian - * product of arrays in O(1) - * Example) - * v0 | v1 | val - * 0 | 0 | 10 + * product of arrays in O(1) + * Example) + * v0 | v1 | val + * 0 | 0 | 10 * 0 | 1 | 21 * 1 | 0 | 32 * 1 | 1 | 43 @@ -75,13 +76,13 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor { * @brief Return ith key in keys_ as a DiscreteKey * @param i ith key in keys_ * @return DiscreteKey - * */ + */ DiscreteKey discreteKey(size_t i) const { 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} + /// 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 string to SparseVector. @@ -142,6 +143,9 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor { TableFactor(const DiscreteKey& key, const std::vector& row) : TableFactor(DiscreteKeys{key}, row) {} + /** Construct from a DiscreteConditional type */ + explicit TableFactor(const DiscreteConditional& c); + /// @} /// @name Testable /// @{ diff --git a/gtsam/discrete/tests/testTableFactor.cpp b/gtsam/discrete/tests/testTableFactor.cpp index 3ad757347..b307d78f6 100644 --- a/gtsam/discrete/tests/testTableFactor.cpp +++ b/gtsam/discrete/tests/testTableFactor.cpp @@ -93,7 +93,8 @@ void printTime(map> for (auto&& kv : measured_time) { cout << "dropout: " << kv.first << " | TableFactor time: " << kv.second.first.count() - << " | DecisionTreeFactor time: " << kv.second.second.count() << endl; + << " | DecisionTreeFactor time: " << kv.second.second.count() << + endl; } } @@ -124,6 +125,13 @@ TEST(TableFactor, constructors) { // Assert that error = -log(value) EXPECT_DOUBLES_EQUAL(-log(f1(values)), f1.error(values), 1e-9); + + // Construct from DiscreteConditional + DiscreteConditional conditional(X | Y = "1/1 2/3 1/4"); + TableFactor f4(conditional); + // Manually constructed via inspection and comparison to DecisionTreeFactor + TableFactor expected(X & Y, "0.5 0.4 0.2 0.5 0.6 0.8"); + EXPECT(assert_equal(expected, f4)); } /* ************************************************************************* */ @@ -156,7 +164,8 @@ TEST(TableFactor, multiplication) { /* ************************************************************************* */ // Benchmark which compares runtime of multiplication of two TableFactors // and two DecisionTreeFactors given sparsity from dense to 90% sparsity. -TEST(TableFactor, benchmark) { +// NOTE: Enable to run. +TEST_DISABLED(TableFactor, benchmark) { DiscreteKey A(0, 5), B(1, 2), C(2, 5), D(3, 2), E(4, 5), F(5, 2), G(6, 3), H(7, 2), I(8, 5), J(9, 7), K(10, 2), L(11, 3); From 64ecb8581e6b6b92199cf0370be0bea2f0d6cc0c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 28 Jun 2023 19:09:43 -0400 Subject: [PATCH 049/206] improve comments --- gtsam/hybrid/HybridBayesNet.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index ab68e170f..13280f1c2 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -154,9 +154,10 @@ void HybridBayesNet::updateDiscreteConditionals( if (conditional->isDiscrete()) { auto discrete = conditional->asDiscrete(); - // Apply prunerFunc to the underlying AlgebraicDecisionTree + // Convert pointer from conditional to factor auto discreteTree = std::dynamic_pointer_cast(discrete); + // Apply prunerFunc to the underlying AlgebraicDecisionTree DecisionTreeFactor::ADT prunedDiscreteTree = discreteTree->apply(prunerFunc(prunedDiscreteProbs, *conditional)); From 50d24ab38ebf356ca6e9671d61dc9a1369240fd0 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 8 Jul 2023 11:33:01 -0400 Subject: [PATCH 050/206] move common functions to DiscreteFactor class --- gtsam/discrete/DecisionTreeFactor.cpp | 37 ++++++++------------------- gtsam/discrete/DecisionTreeFactor.h | 12 --------- gtsam/discrete/DiscreteFactor.cpp | 12 +++++++++ gtsam/discrete/DiscreteFactor.h | 25 +++++++++++++++--- gtsam/discrete/TableFactor.cpp | 19 +++----------- gtsam/discrete/TableFactor.h | 7 ----- 6 files changed, 46 insertions(+), 66 deletions(-) diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp index 04e29024c..ff18268b1 100644 --- a/gtsam/discrete/DecisionTreeFactor.cpp +++ b/gtsam/discrete/DecisionTreeFactor.cpp @@ -33,16 +33,13 @@ namespace gtsam { /* ************************************************************************ */ DecisionTreeFactor::DecisionTreeFactor(const DiscreteKeys& keys, - const ADT& potentials) - : DiscreteFactor(keys.indices()), - ADT(potentials), - cardinalities_(keys.cardinalities()) {} + const ADT& potentials) + : DiscreteFactor(keys.indices(), keys.cardinalities()), ADT(potentials) {} /* ************************************************************************ */ DecisionTreeFactor::DecisionTreeFactor(const DiscreteConditional& c) - : DiscreteFactor(c.keys()), - AlgebraicDecisionTree(c), - cardinalities_(c.cardinalities_) {} + : DiscreteFactor(c.keys(), c.cardinalities()), + AlgebraicDecisionTree(c) {} /* ************************************************************************ */ bool DecisionTreeFactor::equals(const DiscreteFactor& other, @@ -190,18 +187,6 @@ namespace gtsam { return probs; } - /* ************************************************************************ */ - DiscreteKeys DecisionTreeFactor::discreteKeys() const { - DiscreteKeys result; - for (auto&& key : keys()) { - DiscreteKey dkey(key, cardinality(key)); - if (std::find(result.begin(), result.end(), dkey) == result.end()) { - result.push_back(dkey); - } - } - return result; - } - /* ************************************************************************ */ static std::string valueFormatter(const double& v) { std::stringstream ss; @@ -297,17 +282,15 @@ namespace gtsam { /* ************************************************************************ */ DecisionTreeFactor::DecisionTreeFactor(const DiscreteKeys& keys, - const vector& table) - : DiscreteFactor(keys.indices()), - AlgebraicDecisionTree(keys, table), - cardinalities_(keys.cardinalities()) {} + const vector& table) + : DiscreteFactor(keys.indices(), keys.cardinalities()), + AlgebraicDecisionTree(keys, table) {} /* ************************************************************************ */ DecisionTreeFactor::DecisionTreeFactor(const DiscreteKeys& keys, - const string& table) - : DiscreteFactor(keys.indices()), - AlgebraicDecisionTree(keys, table), - cardinalities_(keys.cardinalities()) {} + const string& table) + : DiscreteFactor(keys.indices(), keys.cardinalities()), + AlgebraicDecisionTree(keys, table) {} /* ************************************************************************ */ DecisionTreeFactor DecisionTreeFactor::prune(size_t maxNrAssignments) const { diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h index 42639095f..6cce6e5d4 100644 --- a/gtsam/discrete/DecisionTreeFactor.h +++ b/gtsam/discrete/DecisionTreeFactor.h @@ -50,10 +50,6 @@ namespace gtsam { typedef std::shared_ptr shared_ptr; typedef AlgebraicDecisionTree ADT; - protected: - std::map cardinalities_; - - public: /// @name Standard Constructors /// @{ @@ -119,10 +115,6 @@ namespace gtsam { static double safe_div(const double& a, const double& b); - std::map cardinalities() const { return cardinalities_; } - - size_t cardinality(Key j) const { return cardinalities_.at(j); } - /// divide by factor f (safely) DecisionTreeFactor operator/(const DecisionTreeFactor& f) const { return apply(f, safe_div); @@ -184,9 +176,6 @@ namespace gtsam { /// Get all the probabilities in order of assignment values std::vector probabilities() const; - /// Return all the discrete keys associated with this factor. - DiscreteKeys discreteKeys() const; - /** * @brief Prune the decision tree of discrete variables. * @@ -265,7 +254,6 @@ namespace gtsam { void serialize(ARCHIVE& ar, const unsigned int /*version*/) { ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(ADT); - ar& BOOST_SERIALIZATION_NVP(cardinalities_); } #endif }; diff --git a/gtsam/discrete/DiscreteFactor.cpp b/gtsam/discrete/DiscreteFactor.cpp index 2b1bc36a3..b44d4fce2 100644 --- a/gtsam/discrete/DiscreteFactor.cpp +++ b/gtsam/discrete/DiscreteFactor.cpp @@ -28,6 +28,18 @@ using namespace std; namespace gtsam { +/* ************************************************************************ */ +DiscreteKeys DiscreteFactor::discreteKeys() const { + DiscreteKeys result; + for (auto&& key : keys()) { + DiscreteKey dkey(key, cardinality(key)); + if (std::find(result.begin(), result.end(), dkey) == result.end()) { + result.push_back(dkey); + } + } + return result; +} + /* ************************************************************************* */ double DiscreteFactor::error(const DiscreteValues& values) const { return -std::log((*this)(values)); diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h index 2132e1cc8..24b2b55e4 100644 --- a/gtsam/discrete/DiscreteFactor.h +++ b/gtsam/discrete/DiscreteFactor.h @@ -45,6 +45,10 @@ class GTSAM_EXPORT DiscreteFactor: public Factor { using Values = DiscreteValues; ///< backwards compatibility + protected: + /// Map of Keys and their cardinalities. + std::map cardinalities_; + public: /// @name Standard Constructors /// @{ @@ -52,10 +56,15 @@ class GTSAM_EXPORT DiscreteFactor: public Factor { /** Default constructor creates empty factor */ DiscreteFactor() {} - /** Construct from container of keys. This constructor is used internally from derived factor - * constructors, either from a container of keys or from a boost::assign::list_of. */ - template - DiscreteFactor(const CONTAINER& keys) : Base(keys) {} + /** + * Construct from container of keys and map of cardinalities. + * This constructor is used internally from derived factor constructors, + * either from a container of keys or from a boost::assign::list_of. + */ + template + DiscreteFactor(const CONTAINER& keys, + const std::map cardinalities = {}) + : Base(keys), cardinalities_(cardinalities) {} /// @} /// @name Testable @@ -75,6 +84,13 @@ class GTSAM_EXPORT DiscreteFactor: public Factor { /// @name Standard Interface /// @{ + /// Return all the discrete keys associated with this factor. + DiscreteKeys discreteKeys() const; + + std::map cardinalities() const { return cardinalities_; } + + size_t cardinality(Key j) const { return cardinalities_.at(j); } + /// Find value for given assignment of values to variables virtual double operator()(const DiscreteValues&) const = 0; @@ -130,6 +146,7 @@ class GTSAM_EXPORT DiscreteFactor: public Factor { template void serialize(ARCHIVE& ar, const unsigned int /*version*/) { ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar& BOOST_SERIALIZATION_NVP(cardinalities_); } #endif }; diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp index c59b7b72c..74eb3ddb3 100644 --- a/gtsam/discrete/TableFactor.cpp +++ b/gtsam/discrete/TableFactor.cpp @@ -34,8 +34,7 @@ TableFactor::TableFactor() {} /* ************************************************************************ */ TableFactor::TableFactor(const DiscreteKeys& dkeys, const TableFactor& potentials) - : DiscreteFactor(dkeys.indices()), - cardinalities_(potentials.cardinalities_) { + : DiscreteFactor(dkeys.indices(), dkeys.cardinalities()) { sparse_table_ = potentials.sparse_table_; denominators_ = potentials.denominators_; sorted_dkeys_ = discreteKeys(); @@ -45,11 +44,11 @@ TableFactor::TableFactor(const DiscreteKeys& dkeys, /* ************************************************************************ */ TableFactor::TableFactor(const DiscreteKeys& dkeys, const Eigen::SparseVector& table) - : DiscreteFactor(dkeys.indices()), sparse_table_(table.size()) { + : DiscreteFactor(dkeys.indices(), dkeys.cardinalities()), + sparse_table_(table.size()) { sparse_table_ = table; double denom = table.size(); for (const DiscreteKey& dkey : dkeys) { - cardinalities_.insert(dkey); denom /= dkey.second; denominators_.insert(std::pair(dkey.first, denom)); } @@ -440,18 +439,6 @@ std::vector> TableFactor::enumerate() const { return result; } -/* ************************************************************************ */ -DiscreteKeys TableFactor::discreteKeys() const { - DiscreteKeys result; - for (auto&& key : keys()) { - DiscreteKey dkey(key, cardinality(key)); - if (std::find(result.begin(), result.end(), dkey) == result.end()) { - result.push_back(dkey); - } - } - return result; -} - // Print out header. /* ************************************************************************ */ string TableFactor::markdown(const KeyFormatter& keyFormatter, diff --git a/gtsam/discrete/TableFactor.h b/gtsam/discrete/TableFactor.h index 08c675b67..bd637bb7d 100644 --- a/gtsam/discrete/TableFactor.h +++ b/gtsam/discrete/TableFactor.h @@ -45,8 +45,6 @@ class HybridValues; */ class GTSAM_EXPORT TableFactor : public DiscreteFactor { protected: - /// Map of Keys and their cardinalities. - std::map cardinalities_; /// SparseVector of nonzero probabilities. Eigen::SparseVector sparse_table_; @@ -184,8 +182,6 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor { static double safe_div(const double& a, const double& b); - size_t cardinality(Key j) const { return cardinalities_.at(j); } - /// divide by factor f (safely) TableFactor operator/(const TableFactor& f) const { return apply(f, safe_div); @@ -278,9 +274,6 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor { /// Enumerate all values into a map from values to double. std::vector> enumerate() const; - /// Return all the discrete keys associated with this factor. - DiscreteKeys discreteKeys() const; - /** * @brief Prune the decision tree of discrete variables. * From 2b72f75a0775a9b2a972c8ac0d317567c83e58d3 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 13 Jul 2023 23:47:56 -0400 Subject: [PATCH 051/206] add timing checkpoints --- gtsam/hybrid/HybridBayesNet.cpp | 8 ++++++++ gtsam/hybrid/HybridGaussianFactorGraph.cpp | 6 ++++-- 2 files changed, 12 insertions(+), 2 deletions(-) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index 13280f1c2..266e02b0d 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -161,12 +161,14 @@ void HybridBayesNet::updateDiscreteConditionals( DecisionTreeFactor::ADT prunedDiscreteTree = discreteTree->apply(prunerFunc(prunedDiscreteProbs, *conditional)); + gttic_(HybridBayesNet_MakeConditional); // Create the new (hybrid) conditional KeyVector frontals(discrete->frontals().begin(), discrete->frontals().end()); auto prunedDiscrete = std::make_shared( frontals.size(), conditional->discreteKeys(), prunedDiscreteTree); conditional = std::make_shared(prunedDiscrete); + gttoc_(HybridBayesNet_MakeConditional); // Add it back to the BayesNet this->at(i) = conditional; @@ -177,12 +179,16 @@ void HybridBayesNet::updateDiscreteConditionals( /* ************************************************************************* */ HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) { // Get the decision tree of only the discrete keys + gttic_(HybridBayesNet_PruneDiscreteConditionals); DecisionTreeFactor::shared_ptr discreteConditionals = this->discreteConditionals(); const DecisionTreeFactor prunedDiscreteProbs = discreteConditionals->prune(maxNrLeaves); + gttoc_(HybridBayesNet_PruneDiscreteConditionals); + gttic_(HybridBayesNet_UpdateDiscreteConditionals); this->updateDiscreteConditionals(prunedDiscreteProbs); + gttoc_(HybridBayesNet_UpdateDiscreteConditionals); /* To Prune, we visitWith every leaf in the GaussianMixture. * For each leaf, using the assignment we can check the discrete decision tree @@ -193,6 +199,7 @@ HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) { HybridBayesNet prunedBayesNetFragment; + gttic_(HybridBayesNet_PruneMixtures); // Go through all the conditionals in the // Bayes Net and prune them as per prunedDiscreteProbs. for (auto &&conditional : *this) { @@ -209,6 +216,7 @@ HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves) { prunedBayesNetFragment.push_back(conditional); } } + gttoc_(HybridBayesNet_PruneMixtures); return prunedBayesNetFragment; } diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 8b9d62822..e14023edd 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -98,7 +98,7 @@ static GaussianFactorGraphTree addGaussian( // TODO(dellaert): it's probably more efficient to first collect the discrete // keys, and then loop over all assignments to populate a vector. GaussianFactorGraphTree HybridGaussianFactorGraph::assembleGraphTree() const { - gttic(assembleGraphTree); + gttic_(assembleGraphTree); GaussianFactorGraphTree result; @@ -131,7 +131,7 @@ GaussianFactorGraphTree HybridGaussianFactorGraph::assembleGraphTree() const { } } - gttoc(assembleGraphTree); + gttoc_(assembleGraphTree); return result; } @@ -235,7 +235,9 @@ hybridElimination(const HybridGaussianFactorGraph &factors, gttic_(hybrid_eliminate); #endif + gttic_(hybrid_continuous_eliminate); auto result = EliminatePreferCholesky(graph, frontalKeys); + gttoc_(hybrid_continuous_eliminate); #ifdef HYBRID_TIMING gttoc_(hybrid_eliminate); From 59e26ee79dbcef13ab7c8501653d2b1169eb570e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 17 Jul 2023 10:51:33 -0400 Subject: [PATCH 052/206] remove timers --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index e14023edd..2b23ed4db 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -231,17 +231,7 @@ hybridElimination(const HybridGaussianFactorGraph &factors, return {nullptr, nullptr}; } -#ifdef HYBRID_TIMING - gttic_(hybrid_eliminate); -#endif - - gttic_(hybrid_continuous_eliminate); auto result = EliminatePreferCholesky(graph, frontalKeys); - gttoc_(hybrid_continuous_eliminate); - -#ifdef HYBRID_TIMING - gttoc_(hybrid_eliminate); -#endif return result; }; From fbbf673f7bdbede4faca53bd309ac41c9ab12777 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 4 Jun 2023 15:40:02 +0100 Subject: [PATCH 053/206] Improved documentation and tests (cherry picked from commit febeacd68686ed0b7ced72458eb0b31a196bdab7) --- gtsam/discrete/AlgebraicDecisionTree.h | 64 +++++++++++++++++++--- gtsam/discrete/DecisionTree-inl.h | 2 +- gtsam/discrete/DecisionTree.h | 31 +++++++++-- gtsam/discrete/tests/testDecisionTree.cpp | 66 ++++++++++++++++++----- 4 files changed, 137 insertions(+), 26 deletions(-) diff --git a/gtsam/discrete/AlgebraicDecisionTree.h b/gtsam/discrete/AlgebraicDecisionTree.h index 425ad15f9..20ab4bd68 100644 --- a/gtsam/discrete/AlgebraicDecisionTree.h +++ b/gtsam/discrete/AlgebraicDecisionTree.h @@ -29,9 +29,9 @@ namespace gtsam { /** - * Algebraic Decision Trees fix the range to double - * Just has some nice constructors and some syntactic sugar - * TODO: consider eliminating this class altogether? + * An algebraic decision tree fixes the range of a DecisionTree to double. + * Just has some nice constructors and some syntactic sugar. + * TODO(dellaert): consider eliminating this class altogether? * * @ingroup discrete */ @@ -81,20 +81,62 @@ namespace gtsam { AlgebraicDecisionTree(const L& label, double y1, double y2) : Base(label, y1, y2) {} - /** Create a new leaf function splitting on a variable */ + /** + * @brief Create a new leaf function splitting on a variable + * + * @param labelC: The label with cardinality 2 + * @param y1: The value for the first key + * @param y2: The value for the second key + * + * Example: + * @code{.cpp} + * std::pair A {"a", 2}; + * AlgebraicDecisionTree a(A, 0.6, 0.4); + * @endcode + */ AlgebraicDecisionTree(const typename Base::LabelC& labelC, double y1, double y2) : Base(labelC, y1, y2) {} - /** Create from keys and vector table */ + /** + * @brief Create from keys with cardinalities and a vector table + * + * @param labelCs: The keys, with cardinalities, given as pairs + * @param ys: The vector table + * + * Example with three keys, A, B, and C, with cardinalities 2, 3, and 2, + * respectively, and a vector table of size 12: + * @code{.cpp} + * DiscreteKey A(0, 2), B(1, 3), C(2, 2); + * const vector cpt{ + * 1.0 / 3, 2.0 / 3, 3.0 / 7, 4.0 / 7, 5.0 / 11, 6.0 / 11, // + * 1.0 / 9, 8.0 / 9, 3.0 / 6, 3.0 / 6, 5.0 / 10, 5.0 / 10}; + * AlgebraicDecisionTree expected(A & B & C, cpt); + * @endcode + * The table is given in the following order: + * A=0, B=0, C=0 + * A=0, B=0, C=1 + * ... + * A=1, B=1, C=1 + * Hence, the first line in the table is for A==0, and the second for A==1. + * In each line, the first two entries are for B==0, the next two for B==1, + * and the last two for B==2. Each pair is for a C value of 0 and 1. + */ AlgebraicDecisionTree // (const std::vector& labelCs, - const std::vector& ys) { + const std::vector& ys) { this->root_ = Base::create(labelCs.begin(), labelCs.end(), ys.begin(), ys.end()); } - /** Create from keys and string table */ + /** + * @brief Create from keys and string table + * + * @param labelCs: The keys, with cardinalities, given as pairs + * @param table: The string table, given as a string of doubles. + * + * @note Table needs to be in same order as the vector table in the other constructor. + */ AlgebraicDecisionTree // (const std::vector& labelCs, const std::string& table) { @@ -109,7 +151,13 @@ namespace gtsam { Base::create(labelCs.begin(), labelCs.end(), ys.begin(), ys.end()); } - /** Create a new function splitting on a variable */ + /** + * @brief Create a range of decision trees, splitting on a single variable. + * + * @param begin: Iterator to beginning of a range of decision trees + * @param end: Iterator to end of a range of decision trees + * @param label: The label to split on + */ template AlgebraicDecisionTree(Iterator begin, Iterator end, const L& label) : Base(nullptr) { diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h index 9d618dea0..c99a51501 100644 --- a/gtsam/discrete/DecisionTree-inl.h +++ b/gtsam/discrete/DecisionTree-inl.h @@ -626,7 +626,7 @@ namespace gtsam { // B=1 // A=0: 3 // A=1: 4 - // Note, through the magic of "compose", create([A B],[1 2 3 4]) will produce + // Note, through the magic of "compose", create([A B],[1 3 2 4]) will produce // exactly the same tree as above: the highest label is always the root. // However, it will be *way* faster if labels are given highest to lowest. template diff --git a/gtsam/discrete/DecisionTree.h b/gtsam/discrete/DecisionTree.h index ed1908485..68f3ee73f 100644 --- a/gtsam/discrete/DecisionTree.h +++ b/gtsam/discrete/DecisionTree.h @@ -39,9 +39,23 @@ namespace gtsam { /** - * Decision Tree - * L = label for variables - * Y = function range (any algebra), e.g., bool, int, double + * @brief a decision tree is a function from assignments to values. + * @tparam L label for variables + * @tparam Y function range (any algebra), e.g., bool, int, double + * + * After creating a decision tree on some variables, the tree can be evaluated + * on an assignment to those variables. Example: + * + * @code{.cpp} + * // Create a decision stump one one variable 'a' with values 10 and 20. + * DecisionTree tree('a', 10, 20); + * + * // Evaluate the tree on an assignment to the variable. + * int value0 = tree({{'a', 0}}); // value0 = 10 + * int value1 = tree({{'a', 1}}); // value1 = 20 + * @endcode + * + * More examples can be found in testDecisionTree.cpp * * @ingroup discrete */ @@ -136,7 +150,8 @@ namespace gtsam { NodePtr root_; protected: - /** Internal recursive function to create from keys, cardinalities, + /** + * Internal recursive function to create from keys, cardinalities, * and Y values */ template @@ -167,7 +182,13 @@ namespace gtsam { /** Create a constant */ explicit DecisionTree(const Y& y); - /// Create tree with 2 assignments `y1`, `y2`, splitting on variable `label` + /** + * @brief Create tree with 2 assignments `y1`, `y2`, splitting on variable `label` + * + * @param label The variable to split on. + * @param y1 The value for the first assignment. + * @param y2 The value for the second assignment. + */ DecisionTree(const L& label, const Y& y1, const Y& y2); /** Allow Label+Cardinality for convenience */ diff --git a/gtsam/discrete/tests/testDecisionTree.cpp b/gtsam/discrete/tests/testDecisionTree.cpp index d2a94ddc3..6ca8d843e 100644 --- a/gtsam/discrete/tests/testDecisionTree.cpp +++ b/gtsam/discrete/tests/testDecisionTree.cpp @@ -75,6 +75,19 @@ struct traits : public Testable {}; GTSAM_CONCEPT_TESTABLE_INST(CrazyDecisionTree) +/* ************************************************************************** */ +// Test char labels and int range +/* ************************************************************************** */ + +// Create a decision stump one one variable 'a' with values 10 and 20. +TEST(DecisionTree, constructor) { + DecisionTree tree('a', 10, 20); + + // Evaluate the tree on an assignment to the variable. + EXPECT_LONGS_EQUAL(10, tree({{'a', 0}})); + EXPECT_LONGS_EQUAL(20, tree({{'a', 1}})); +} + /* ************************************************************************** */ // Test string labels and int range /* ************************************************************************** */ @@ -118,18 +131,47 @@ struct Ring { static inline int mul(const int& a, const int& b) { return a * b; } }; +/* ************************************************************************** */ +// Check that creating decision trees respects key order. +TEST(DecisionTree, constructor_order) { + // Create labels + string A("A"), B("B"); + + const std::vector ys1 = {1, 2, 3, 4}; + DT tree1({{B, 2}, {A, 2}}, ys1); // faster version, as B is "higher" than A! + + const std::vector ys2 = {1, 3, 2, 4}; + DT tree2({{A, 2}, {B, 2}}, ys2); // slower version ! + + // Both trees will be the same, tree is order from high to low labels. + // Choice(B) + // 0 Choice(A) + // 0 0 Leaf 1 + // 0 1 Leaf 2 + // 1 Choice(A) + // 1 0 Leaf 3 + // 1 1 Leaf 4 + + EXPECT(tree2.equals(tree1)); + + // Check the values are as expected by calling the () operator: + EXPECT_LONGS_EQUAL(1, tree1({{A, 0}, {B, 0}})); + EXPECT_LONGS_EQUAL(3, tree1({{A, 0}, {B, 1}})); + EXPECT_LONGS_EQUAL(2, tree1({{A, 1}, {B, 0}})); + EXPECT_LONGS_EQUAL(4, tree1({{A, 1}, {B, 1}})); +} + /* ************************************************************************** */ // test DT TEST(DecisionTree, example) { // Create labels string A("A"), B("B"), C("C"); - // create a value - Assignment x00, x01, x10, x11; - x00[A] = 0, x00[B] = 0; - x01[A] = 0, x01[B] = 1; - x10[A] = 1, x10[B] = 0; - x11[A] = 1, x11[B] = 1; + // Create assignments using brace initialization: + Assignment x00{{A, 0}, {B, 0}}; + Assignment x01{{A, 0}, {B, 1}}; + Assignment x10{{A, 1}, {B, 0}}; + Assignment x11{{A, 1}, {B, 1}}; // empty DT empty; @@ -241,8 +283,7 @@ TEST(DecisionTree, ConvertValuesOnly) { StringBoolTree f2(f1, bool_of_int); // Check a value - Assignment x00; - x00["A"] = 0, x00["B"] = 0; + Assignment x00 {{A, 0}, {B, 0}}; EXPECT(!f2(x00)); } @@ -266,10 +307,11 @@ TEST(DecisionTree, ConvertBoth) { // Check some values Assignment