From bd4ab1598e307d85c0953fc2e1a086040b08cf6d Mon Sep 17 00:00:00 2001 From: Mike Sheffler Date: Thu, 27 Sep 2018 18:19:44 -0700 Subject: [PATCH 01/51] Expanded shortcut_overlapping_separator test in testGaussianBayesTreeB to allow for sign reversal in a row of the augmented Jacobian. The joint density does not appear to be affected in the case where the augmented Jacobian row sign is changed, so we should not fail the test if that happens. --- tests/testGaussianBayesTreeB.cpp | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/tests/testGaussianBayesTreeB.cpp b/tests/testGaussianBayesTreeB.cpp index 8b2aa3ae7..88b1aede2 100644 --- a/tests/testGaussianBayesTreeB.cpp +++ b/tests/testGaussianBayesTreeB.cpp @@ -317,8 +317,25 @@ TEST(GaussianBayesTree, shortcut_overlapping_separator) 5, 0, 6, 0, -11, -12 ).finished(); + Matrix actualJointJ = joint.augmentedJacobian(); + bool Row0RhsSignsEqual = + signbit(expectedJointJ(0, 2)) == signbit(actualJointJ(0, 2)); + + if (!Row0RhsSignsEqual) + { + expectedJointJ.row(0) = -expectedJointJ.row(0); + } + + bool Row1RhsSignsEqual = + signbit(expectedJointJ(1, 2)) == signbit(actualJointJ(1, 2)); + + if (!Row1RhsSignsEqual) + { + expectedJointJ.row(1) = -expectedJointJ.row(1); + } + EXPECT(assert_equal(expectedJointJ, actualJointJ)); } From 1ec15a734505495d3158a91b98699e4e16cc4351 Mon Sep 17 00:00:00 2001 From: Mike Sheffler Date: Fri, 28 Sep 2018 00:47:17 -0700 Subject: [PATCH 02/51] Incorporate Frank's suggestions for PR #315 --- tests/testGaussianBayesTreeB.cpp | 15 +++------------ 1 file changed, 3 insertions(+), 12 deletions(-) diff --git a/tests/testGaussianBayesTreeB.cpp b/tests/testGaussianBayesTreeB.cpp index 88b1aede2..984728ebf 100644 --- a/tests/testGaussianBayesTreeB.cpp +++ b/tests/testGaussianBayesTreeB.cpp @@ -320,21 +320,12 @@ TEST(GaussianBayesTree, shortcut_overlapping_separator) Matrix actualJointJ = joint.augmentedJacobian(); - bool Row0RhsSignsEqual = - signbit(expectedJointJ(0, 2)) == signbit(actualJointJ(0, 2)); - - if (!Row0RhsSignsEqual) - { + // PR 315: sign of rows in joint are immaterial + if (signbit(expectedJointJ(0, 2)) != signbit(actualJointJ(0, 2))) expectedJointJ.row(0) = -expectedJointJ.row(0); - } - bool Row1RhsSignsEqual = - signbit(expectedJointJ(1, 2)) == signbit(actualJointJ(1, 2)); - - if (!Row1RhsSignsEqual) - { + if (signbit(expectedJointJ(1, 2)) != signbit(actualJointJ(1, 2))) expectedJointJ.row(1) = -expectedJointJ.row(1); - } EXPECT(assert_equal(expectedJointJ, actualJointJ)); } From 4a8c2f666c619427ffc180abf17bddc916133176 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 28 Sep 2018 11:20:26 -0400 Subject: [PATCH 03/51] Fixed warning --- gtsam/3rdparty/metis/libmetis/balance.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/3rdparty/metis/libmetis/balance.c b/gtsam/3rdparty/metis/libmetis/balance.c index 3fb0e6e56..f71df9c0e 100644 --- a/gtsam/3rdparty/metis/libmetis/balance.c +++ b/gtsam/3rdparty/metis/libmetis/balance.c @@ -20,7 +20,7 @@ void Balance2Way(ctrl_t *ctrl, graph_t *graph, real_t *ntpwgts) if (graph->ncon == 1) { /* return right away if the balance is OK */ - if (iabs(ntpwgts[0]*graph->tvwgt[0]-graph->pwgts[0]) < 3*graph->tvwgt[0]/graph->nvtxs) + if (fabs(ntpwgts[0]*graph->tvwgt[0]-graph->pwgts[0]) < 3*graph->tvwgt[0]/graph->nvtxs) return; if (graph->nbnd > 0) From ce65b5d0439af811846360195ad712aac93408f3 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 28 Sep 2018 11:20:38 -0400 Subject: [PATCH 04/51] virtual destructors --- gtsam/navigation/ImuFactor.h | 4 ++++ gtsam/navigation/TangentPreintegration.h | 6 +++++- 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 55f043dd3..ccf5db5c3 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -106,6 +106,10 @@ public: preintMeasCov_(preintMeasCov) { } + /// Virtual destructor + virtual ~PreintegratedImuMeasurements() { + } + /// print void print(const std::string& s = "Preintegrated Measurements:") const override; diff --git a/gtsam/navigation/TangentPreintegration.h b/gtsam/navigation/TangentPreintegration.h index 34c38e22b..da49e4ddd 100644 --- a/gtsam/navigation/TangentPreintegration.h +++ b/gtsam/navigation/TangentPreintegration.h @@ -42,7 +42,7 @@ class TangentPreintegration : public PreintegrationBase { } public: - /// @name Constructors + /// @name Constructors/destructors /// @{ /** @@ -53,6 +53,10 @@ public: TangentPreintegration(const boost::shared_ptr& p, const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()); + /// Virtual destructor + virtual ~TangentPreintegration() { + } + /// @} /// @name Basic utilities From a87ce9bac423adf45e7d69fb08cbc14ed188773e Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 28 Sep 2018 11:20:55 -0400 Subject: [PATCH 05/51] Fixed consistency checks --- gtsam/linear/JacobianFactor.cpp | 2 +- gtsam/nonlinear/ISAM2-impl.cpp | 7 ++++--- gtsam/nonlinear/ISAM2-inl.h | 4 ++-- 3 files changed, 7 insertions(+), 6 deletions(-) diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 4597759e3..56a5dc085 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -188,7 +188,7 @@ boost::tuple, DenseIndex, DenseIndex> _countDims( m += factor->rows(); } -#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS +#if !defined(NDEBUG) && defined(GTSAM_EXTRA_CONSISTENCY_CHECKS) for(DenseIndex d: varDims) { assert(d != numeric_limits::max()); } diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index a0fc117d9..3b5156535 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -291,9 +291,10 @@ size_t ISAM2::Impl::UpdateGaussNewtonDelta(const FastVector lastBacksubVariableCount += optimizeWildfireNonRecursive( root, wildfireThreshold, replacedKeys, delta); // modifies delta -#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS - for(size_t j=0; j)).all()); +#if !defined(NDEBUG) && defined(GTSAM_EXTRA_CONSISTENCY_CHECKS) + for (VectorValues::const_iterator key_delta = delta.begin(); key_delta != delta.end(); ++key_delta) { + assert(delta[key_delta->first].allFinite()); + } #endif } diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h index f04e16b7d..fecefd2a5 100644 --- a/gtsam/nonlinear/ISAM2-inl.h +++ b/gtsam/nonlinear/ISAM2-inl.h @@ -44,7 +44,7 @@ void optimizeWildfire(const boost::shared_ptr& clique, double threshold, // Are any clique variables part of the tree that has been redone? bool cliqueReplaced = replaced.exists((*clique)->frontals().front()); -#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS +#if !defined(NDEBUG) && defined(GTSAM_EXTRA_CONSISTENCY_CHECKS) for(Key frontal: clique->conditional()->frontals()) { assert(cliqueReplaced == replaced.exists(frontal)); } @@ -121,7 +121,7 @@ bool optimizeWildfireNode(const boost::shared_ptr& clique, double thresh // Are any clique variables part of the tree that has been redone? bool cliqueReplaced = replaced.exists(clique->conditional()->frontals().front()); -#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS +#if !defined(NDEBUG) && defined(GTSAM_EXTRA_CONSISTENCY_CHECKS) for(Key frontal: clique->conditional()->frontals()) { assert(cliqueReplaced == replaced.exists(frontal)); } From 67ffd65838b774ad7acd89bf18711423af6a2bcb Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 29 Sep 2018 00:04:13 -0400 Subject: [PATCH 06/51] Added Alexander's example file --- examples/ISAM2Example_SmartFactor.cpp | 106 ++++++++++++++++++++++++++ 1 file changed, 106 insertions(+) create mode 100644 examples/ISAM2Example_SmartFactor.cpp diff --git a/examples/ISAM2Example_SmartFactor.cpp b/examples/ISAM2Example_SmartFactor.cpp new file mode 100644 index 000000000..8435cc86e --- /dev/null +++ b/examples/ISAM2Example_SmartFactor.cpp @@ -0,0 +1,106 @@ +#include +#include + +#include +#include +#include + +#include + +using namespace gtsam; + +// Make the typename short so it looks much cleaner +typedef SmartProjectionPoseFactor SmartFactor; + +int main(int argc, char* argv[]) { + Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)); + + noiseModel::Isotropic::shared_ptr measurement_noise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v + + noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas( + (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); + noiseModel::Isotropic::shared_ptr large_noise = noiseModel::Isotropic::Sigma(6, 100); + + ISAM2Params parameters; + parameters.relinearizeThreshold = 0.01; + parameters.relinearizeSkip = 1; + parameters.cacheLinearizedFactors = false; + parameters.enableDetailedResults = true; + parameters.print(); + ISAM2 isam(parameters); + + // Create a factor graph + NonlinearFactorGraph graph; + Values initial_estimate; + + Point3 point(0.0, 0.0, 1.0); + + // Intentionally initialize the variables off from the ground truth + Pose3 delta(Rot3::Rodrigues(0.0, 0.0, 0.0), Point3(0.05, -0.10, 0.20)); + + Pose3 pose1(Rot3(), Point3(0.0, 0.0, 0.0)); + Pose3 pose2(Rot3(), Point3(0.0, 0.2, 0.0)); + Pose3 pose3(Rot3(), Point3(0.0, 0.4, 0.0)); + Pose3 pose4(Rot3(), Point3(0.0, 0.5, 0.0)); + Pose3 pose5(Rot3(), Point3(0.0, 0.6, 0.0)); + + std::vector pose_list = { pose1, pose2, pose3, pose4, pose5 }; + + SmartFactor::shared_ptr smart_factor(new SmartFactor(measurement_noise, K)); + graph.push_back(smart_factor); + + graph.emplace_shared>(0, pose_list[0], noise); + initial_estimate.insert(0, pose_list[0].compose(delta)); + smart_factor->add(PinholePose(pose_list[0], K).project(point), 0); + + for (int i = 1; i < 5; i++) { + PinholePose camera(pose_list[i], K); + Point2 measurement = camera.project(point); + + std::cout << "****************************************************" << std::endl; + std::cout << "i = " << i << std::endl; + std::cout << "Measurement " << i << " is " << measurement << std::endl; + + graph.emplace_shared>(i, pose_list[i], noise); + //graph.emplace_shared>(i - 1, i, Pose3(), large_noise); + initial_estimate.insert(i, pose_list[i].compose(delta)); + smart_factor->add(measurement, i); + + ISAM2Result result = isam.update(graph, initial_estimate); + graph.resize(0); + initial_estimate.clear(); + + result.print(); + + const auto& var_map = (*(result.detail)).variableStatus; + + std::cout << "Detailed results:" << std::endl; + for (int j = 0; j < 3; j++) { + if (var_map.exists(j)) { + std::cout << j << " is reeliminated: " << var_map.at(j).isReeliminated << std::endl; + std::cout << j << " is relinearized above thresh: " << var_map.at(j).isAboveRelinThreshold << std::endl; + std::cout << j << " is relinearized involved: " << var_map.at(j).isRelinearizeInvolved << std::endl; + std::cout << j << " is relinearized: " << var_map.at(j).isRelinearized << std::endl; + std::cout << j << " is observed: " << var_map.at(j).isObserved << std::endl; + std::cout << j << " is new: " << var_map.at(j).isNew << std::endl; + std::cout << j << " is in the root clique: " << var_map.at(j).inRootClique << std::endl; + } + else { + std::cout << j << " does not exist in the detailed results map." << std::endl; + } + } + + Values current_estimate = isam.calculateEstimate(); + current_estimate.print("Current estimate: "); + + boost::optional point_res = smart_factor->point(current_estimate); + if (point_res) { + std::cout << *point_res << std::endl; + } + else { + std::cout << "Point is degenerate." << std::endl; + } + } + + return 0; +} From 224299ccb90d0205d4926cb5ea2b92395e497e2f Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 29 Sep 2018 00:05:39 -0400 Subject: [PATCH 07/51] Cleaned up/naming conventions/docs --- examples/ISAM2Example_SmartFactor.cpp | 111 ++++++++++++++------------ 1 file changed, 60 insertions(+), 51 deletions(-) diff --git a/examples/ISAM2Example_SmartFactor.cpp b/examples/ISAM2Example_SmartFactor.cpp index 8435cc86e..77e29b8ff 100644 --- a/examples/ISAM2Example_SmartFactor.cpp +++ b/examples/ISAM2Example_SmartFactor.cpp @@ -1,5 +1,8 @@ -#include -#include +/** + * @file ISAM2Example_SmartFactor.cpp + * @brief test of iSAM with smart factors, led to bitbucket issue #367 + * @author Alexander (pumaking on BitBucket) + */ #include #include @@ -7,7 +10,13 @@ #include +#include +#include + +using namespace std; using namespace gtsam; +using symbol_shorthand::P; +using symbol_shorthand::X; // Make the typename short so it looks much cleaner typedef SmartProjectionPoseFactor SmartFactor; @@ -15,11 +24,12 @@ typedef SmartProjectionPoseFactor SmartFactor; int main(int argc, char* argv[]) { Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)); - noiseModel::Isotropic::shared_ptr measurement_noise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v + auto measurementNoise = + noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v - noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas( - (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); - noiseModel::Isotropic::shared_ptr large_noise = noiseModel::Isotropic::Sigma(6, 100); + Vector6 sigmas; + sigmas << Vector3::Constant(0.3), Vector3::Constant(0.1); + auto noise = noiseModel::Diagonal::Sigmas(sigmas); ISAM2Params parameters; parameters.relinearizeThreshold = 0.01; @@ -31,7 +41,7 @@ int main(int argc, char* argv[]) { // Create a factor graph NonlinearFactorGraph graph; - Values initial_estimate; + Values initialEstimate; Point3 point(0.0, 0.0, 1.0); @@ -44,62 +54,61 @@ int main(int argc, char* argv[]) { Pose3 pose4(Rot3(), Point3(0.0, 0.5, 0.0)); Pose3 pose5(Rot3(), Point3(0.0, 0.6, 0.0)); - std::vector pose_list = { pose1, pose2, pose3, pose4, pose5 }; + vector poses = {pose1, pose2, pose3, pose4, pose5}; - SmartFactor::shared_ptr smart_factor(new SmartFactor(measurement_noise, K)); - graph.push_back(smart_factor); + SmartFactor::shared_ptr smartFactor(new SmartFactor(measurementNoise, K)); + graph.push_back(smartFactor); - graph.emplace_shared>(0, pose_list[0], noise); - initial_estimate.insert(0, pose_list[0].compose(delta)); - smart_factor->add(PinholePose(pose_list[0], K).project(point), 0); + graph.emplace_shared>(X(0), poses[0], noise); + initialEstimate.insert(X(0), poses[0].compose(delta)); + smartFactor->add(PinholePose(poses[0], K).project(point), X(0)); - for (int i = 1; i < 5; i++) { - PinholePose camera(pose_list[i], K); + for (size_t i = 1; i < 5; i++) { + cout << "****************************************************" << endl; + cout << "i = " << i << endl; + + // Add measurement to smart factor + PinholePose camera(poses[i], K); Point2 measurement = camera.project(point); + smartFactor->add(measurement, X(i)); + cout << "Measurement " << i << "" << measurement << endl; - std::cout << "****************************************************" << std::endl; - std::cout << "i = " << i << std::endl; - std::cout << "Measurement " << i << " is " << measurement << std::endl; - - graph.emplace_shared>(i, pose_list[i], noise); - //graph.emplace_shared>(i - 1, i, Pose3(), large_noise); - initial_estimate.insert(i, pose_list[i].compose(delta)); - smart_factor->add(measurement, i); - - ISAM2Result result = isam.update(graph, initial_estimate); - graph.resize(0); - initial_estimate.clear(); + // Add prior on new pose + graph.emplace_shared>(X(i), poses[i], noise); + initialEstimate.insert(X(i), poses[i].compose(delta)); + // Update iSAM2 + ISAM2Result result = isam.update(graph, initialEstimate); result.print(); - const auto& var_map = (*(result.detail)).variableStatus; - - std::cout << "Detailed results:" << std::endl; - for (int j = 0; j < 3; j++) { - if (var_map.exists(j)) { - std::cout << j << " is reeliminated: " << var_map.at(j).isReeliminated << std::endl; - std::cout << j << " is relinearized above thresh: " << var_map.at(j).isAboveRelinThreshold << std::endl; - std::cout << j << " is relinearized involved: " << var_map.at(j).isRelinearizeInvolved << std::endl; - std::cout << j << " is relinearized: " << var_map.at(j).isRelinearized << std::endl; - std::cout << j << " is observed: " << var_map.at(j).isObserved << std::endl; - std::cout << j << " is new: " << var_map.at(j).isNew << std::endl; - std::cout << j << " is in the root clique: " << var_map.at(j).inRootClique << std::endl; - } - else { - std::cout << j << " does not exist in the detailed results map." << std::endl; - } + cout << "Detailed results:" << endl; + for (auto keyedStatus : result.detail->variableStatus) { + const auto& status = keyedStatus.second; + cout << keyedStatus.first << " {" << endl; + cout << "reeliminated: " << status.isReeliminated << endl; + cout << "relinearized above thresh: " << status.isAboveRelinThreshold + << endl; + cout << "relinearized involved: " << status.isRelinearizeInvolved << endl; + cout << "relinearized: " << status.isRelinearized << endl; + cout << "observed: " << status.isObserved << endl; + cout << "new: " << status.isNew << endl; + cout << "in the root clique: " << status.inRootClique << endl; + cout << "}" << endl; } - Values current_estimate = isam.calculateEstimate(); - current_estimate.print("Current estimate: "); + Values currentEstimate = isam.calculateEstimate(); + currentEstimate.print("Current estimate: "); - boost::optional point_res = smart_factor->point(current_estimate); - if (point_res) { - std::cout << *point_res << std::endl; - } - else { - std::cout << "Point is degenerate." << std::endl; + boost::optional pointEstimate = smartFactor->point(currentEstimate); + if (pointEstimate) { + cout << *pointEstimate << endl; + } else { + cout << "Point degenerate." << endl; } + + // Reset graph and initial estimate for next iteration + graph.resize(0); + initialEstimate.clear(); } return 0; From e6c0d7334fba6eac1ef6208fb7f949fe171ed0ec Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 29 Sep 2018 00:06:19 -0400 Subject: [PATCH 08/51] Cleaned up cpplint errors - no functionality change --- gtsam/nonlinear/ISAM2.cpp | 2 +- gtsam/nonlinear/ISAM2.h | 642 ++++++++++++++++++++++++-------------- 2 files changed, 401 insertions(+), 243 deletions(-) diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 122bf8aeb..d4df01a68 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -247,7 +247,7 @@ ISAM2::relinearizeAffectedFactors(const FastList& affectedKeys, const KeySe /* ************************************************************************* */ // find intermediate (linearized) factors from cache that are passed into the affected area -GaussianFactorGraph ISAM2::getCachedBoundaryFactors(Cliques& orphans) { +GaussianFactorGraph ISAM2::getCachedBoundaryFactors(const Cliques& orphans) { GaussianFactorGraph cachedBoundary; for(sharedClique orphan: orphans) { diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 114c04018..235cb65a5 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -11,7 +11,8 @@ /** * @file ISAM2.h - * @brief Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization. + * @brief Incremental update functionality (ISAM2) for BayesTree, with fluid + * relinearization. * @author Michael Kaess, Richard Roberts */ @@ -19,9 +20,12 @@ #pragma once -#include -#include +#include +#include + #include +#include +#include #include @@ -34,21 +38,29 @@ namespace gtsam { * ISAM2Params, which should in turn be passed to ISAM2(const ISAM2Params&). */ struct GTSAM_EXPORT ISAM2GaussNewtonParams { - double wildfireThreshold; ///< Continue updating the linear delta only when changes are above this threshold (default: 0.001) + double + wildfireThreshold; ///< Continue updating the linear delta only when + ///< changes are above this threshold (default: 0.001) /** Specify parameters as constructor arguments */ ISAM2GaussNewtonParams( - double _wildfireThreshold = 0.001 ///< see ISAM2GaussNewtonParams public variables, ISAM2GaussNewtonParams::wildfireThreshold - ) : wildfireThreshold(_wildfireThreshold) {} + double _wildfireThreshold = + 0.001 ///< see ISAM2GaussNewtonParams public variables, + ///< ISAM2GaussNewtonParams::wildfireThreshold + ) + : wildfireThreshold(_wildfireThreshold) {} void print(const std::string str = "") const { - std::cout << str << "type: ISAM2GaussNewtonParams\n"; - std::cout << str << "wildfireThreshold: " << wildfireThreshold << "\n"; - std::cout.flush(); + using std::cout; + cout << str << "type: ISAM2GaussNewtonParams\n"; + cout << str << "wildfireThreshold: " << wildfireThreshold << "\n"; + cout.flush(); } double getWildfireThreshold() const { return wildfireThreshold; } - void setWildfireThreshold(double wildfireThreshold) { this->wildfireThreshold = wildfireThreshold; } + void setWildfireThreshold(double wildfireThreshold) { + this->wildfireThreshold = wildfireThreshold; + } }; /** @@ -58,51 +70,81 @@ struct GTSAM_EXPORT ISAM2GaussNewtonParams { * ISAM2Params, which should in turn be passed to ISAM2(const ISAM2Params&). */ struct GTSAM_EXPORT ISAM2DoglegParams { - double initialDelta; ///< The initial trust region radius for Dogleg - double wildfireThreshold; ///< Continue updating the linear delta only when changes are above this threshold (default: 1e-5) - DoglegOptimizerImpl::TrustRegionAdaptationMode adaptationMode; ///< See description in DoglegOptimizerImpl::TrustRegionAdaptationMode - bool verbose; ///< Whether Dogleg prints iteration and convergence information + double initialDelta; ///< The initial trust region radius for Dogleg + double + wildfireThreshold; ///< Continue updating the linear delta only when + ///< changes are above this threshold (default: 1e-5) + DoglegOptimizerImpl::TrustRegionAdaptationMode + adaptationMode; ///< See description in + ///< DoglegOptimizerImpl::TrustRegionAdaptationMode + bool + verbose; ///< Whether Dogleg prints iteration and convergence information /** Specify parameters as constructor arguments */ ISAM2DoglegParams( - double _initialDelta = 1.0, ///< see ISAM2DoglegParams::initialDelta - double _wildfireThreshold = 1e-5, ///< see ISAM2DoglegParams::wildfireThreshold - DoglegOptimizerImpl::TrustRegionAdaptationMode _adaptationMode = DoglegOptimizerImpl::SEARCH_EACH_ITERATION, ///< see ISAM2DoglegParams::adaptationMode - bool _verbose = false ///< see ISAM2DoglegParams::verbose - ) : initialDelta(_initialDelta), wildfireThreshold(_wildfireThreshold), - adaptationMode(_adaptationMode), verbose(_verbose) {} + double _initialDelta = 1.0, ///< see ISAM2DoglegParams::initialDelta + double _wildfireThreshold = + 1e-5, ///< see ISAM2DoglegParams::wildfireThreshold + DoglegOptimizerImpl::TrustRegionAdaptationMode _adaptationMode = + DoglegOptimizerImpl:: + SEARCH_EACH_ITERATION, ///< see ISAM2DoglegParams::adaptationMode + bool _verbose = false ///< see ISAM2DoglegParams::verbose + ) + : initialDelta(_initialDelta), + wildfireThreshold(_wildfireThreshold), + adaptationMode(_adaptationMode), + verbose(_verbose) {} void print(const std::string str = "") const { - std::cout << str << "type: ISAM2DoglegParams\n"; - std::cout << str << "initialDelta: " << initialDelta << "\n"; - std::cout << str << "wildfireThreshold: " << wildfireThreshold << "\n"; - std::cout << str << "adaptationMode: " << adaptationModeTranslator(adaptationMode) << "\n"; - std::cout.flush(); + using std::cout; + cout << str << "type: ISAM2DoglegParams\n"; + cout << str << "initialDelta: " << initialDelta << "\n"; + cout << str << "wildfireThreshold: " << wildfireThreshold << "\n"; + cout << str + << "adaptationMode: " << adaptationModeTranslator(adaptationMode) + << "\n"; + cout.flush(); } double getInitialDelta() const { return initialDelta; } double getWildfireThreshold() const { return wildfireThreshold; } - std::string getAdaptationMode() const { return adaptationModeTranslator(adaptationMode); }; - bool isVerbose() const { return verbose; }; + std::string getAdaptationMode() const { + return adaptationModeTranslator(adaptationMode); + } + bool isVerbose() const { return verbose; } + void setInitialDelta(double initialDelta) { + this->initialDelta = initialDelta; + } + void setWildfireThreshold(double wildfireThreshold) { + this->wildfireThreshold = wildfireThreshold; + } + void setAdaptationMode(const std::string& adaptationMode) { + this->adaptationMode = adaptationModeTranslator(adaptationMode); + } + void setVerbose(bool verbose) { this->verbose = verbose; } - void setInitialDelta(double initialDelta) { this->initialDelta = initialDelta; } - void setWildfireThreshold(double wildfireThreshold) { this->wildfireThreshold = wildfireThreshold; } - void setAdaptationMode(const std::string& adaptationMode) { this->adaptationMode = adaptationModeTranslator(adaptationMode); } - void setVerbose(bool verbose) { this->verbose = verbose; }; - - std::string adaptationModeTranslator(const DoglegOptimizerImpl::TrustRegionAdaptationMode& adaptationMode) const; - DoglegOptimizerImpl::TrustRegionAdaptationMode adaptationModeTranslator(const std::string& adaptationMode) const; + std::string adaptationModeTranslator( + const DoglegOptimizerImpl::TrustRegionAdaptationMode& adaptationMode) + const; + DoglegOptimizerImpl::TrustRegionAdaptationMode adaptationModeTranslator( + const std::string& adaptationMode) const; }; /** * @addtogroup ISAM2 - * Parameters for the ISAM2 algorithm. Default parameter values are listed below. + * Parameters for the ISAM2 algorithm. Default parameter values are listed + * below. */ -typedef FastMap ISAM2ThresholdMap; +typedef FastMap ISAM2ThresholdMap; typedef ISAM2ThresholdMap::value_type ISAM2ThresholdMapValue; struct GTSAM_EXPORT ISAM2Params { - typedef boost::variant OptimizationParams; ///< Either ISAM2GaussNewtonParams or ISAM2DoglegParams - typedef boost::variant > RelinearizationThreshold; ///< Either a constant relinearization threshold or a per-variable-type set of thresholds + typedef boost::variant + OptimizationParams; ///< Either ISAM2GaussNewtonParams or + ///< ISAM2DoglegParams + typedef boost::variant > + RelinearizationThreshold; ///< Either a constant relinearization + ///< threshold or a per-variable-type set of + ///< thresholds /** Optimization parameters, this both selects the nonlinear optimization * method and specifies its parameters, either ISAM2GaussNewtonParams or @@ -122,27 +164,36 @@ struct GTSAM_EXPORT ISAM2Params { * entries would be added with: * \code FastMap thresholds; - thresholds['x'] = (Vector(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5).finished(); // 0.1 rad rotation threshold, 0.5 m translation threshold - thresholds['l'] = Vector3(1.0, 1.0, 1.0); // 1.0 m landmark position threshold + thresholds['x'] = (Vector(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5).finished(); + // 0.1 rad rotation threshold, 0.5 m translation threshold thresholds['l'] = + Vector3(1.0, 1.0, 1.0); // 1.0 m landmark position threshold params.relinearizeThreshold = thresholds; \endcode */ RelinearizationThreshold relinearizeThreshold; - int relinearizeSkip; ///< Only relinearize any variables every relinearizeSkip calls to ISAM2::update (default: 10) + int relinearizeSkip; ///< Only relinearize any variables every + ///< relinearizeSkip calls to ISAM2::update (default: + ///< 10) - bool enableRelinearization; ///< Controls whether ISAM2 will ever relinearize any variables (default: true) + bool enableRelinearization; ///< Controls whether ISAM2 will ever relinearize + ///< any variables (default: true) - bool evaluateNonlinearError; ///< Whether to evaluate the nonlinear error before and after the update, to return in ISAM2Result from update() + bool evaluateNonlinearError; ///< Whether to evaluate the nonlinear error + ///< before and after the update, to return in + ///< ISAM2Result from update() enum Factorization { CHOLESKY, QR }; - /** Specifies whether to use QR or CHOESKY numerical factorization (default: CHOLESKY). - * Cholesky is faster but potentially numerically unstable for poorly-conditioned problems, which can occur when - * uncertainty is very low in some variables (or dimensions of variables) and very high in others. QR is - * slower but more numerically stable in poorly-conditioned problems. We suggest using the default of Cholesky - * unless gtsam sometimes throws IndefiniteLinearSystemException when your problem's Hessian is actually positive - * definite. For positive definite problems, numerical error accumulation can cause the problem to become - * numerically negative or indefinite as solving proceeds, especially when using Cholesky. + /** Specifies whether to use QR or CHOESKY numerical factorization (default: + * CHOLESKY). Cholesky is faster but potentially numerically unstable for + * poorly-conditioned problems, which can occur when uncertainty is very low + * in some variables (or dimensions of variables) and very high in others. QR + * is slower but more numerically stable in poorly-conditioned problems. We + * suggest using the default of Cholesky unless gtsam sometimes throws + * IndefiniteLinearSystemException when your problem's Hessian is actually + * positive definite. For positive definite problems, numerical error + * accumulation can cause the problem to become numerically negative or + * indefinite as solving proceeds, especially when using Cholesky. */ Factorization factorization; @@ -153,97 +204,156 @@ struct GTSAM_EXPORT ISAM2Params { */ bool cacheLinearizedFactors; - KeyFormatter keyFormatter; ///< A KeyFormatter for when keys are printed during debugging (default: DefaultKeyFormatter) + KeyFormatter + keyFormatter; ///< A KeyFormatter for when keys are printed during + ///< debugging (default: DefaultKeyFormatter) - bool enableDetailedResults; ///< Whether to compute and return ISAM2Result::detailedResults, this can increase running time (default: false) + bool enableDetailedResults; ///< Whether to compute and return + ///< ISAM2Result::detailedResults, this can + ///< increase running time (default: false) - /** Check variables for relinearization in tree-order, stopping the check once a variable does not need to be relinearized (default: false). - * This can improve speed by only checking a small part of the top of the tree. However, variables below the check cut-off can accumulate - * significant deltas without triggering relinearization. This is particularly useful in exploration scenarios where real-time performance - * is desired over correctness. Use with caution. + /** Check variables for relinearization in tree-order, stopping the check once + * a variable does not need to be relinearized (default: false). This can + * improve speed by only checking a small part of the top of the tree. + * However, variables below the check cut-off can accumulate significant + * deltas without triggering relinearization. This is particularly useful in + * exploration scenarios where real-time performance is desired over + * correctness. Use with caution. */ bool enablePartialRelinearizationCheck; - /// When you will be removing many factors, e.g. when using ISAM2 as a fixed-lag smoother, enable this option to - /// add factors in the first available factor slots, to avoid accumulating NULL factor slots, at the cost of - /// having to search for slots every time a factor is added. + /// When you will be removing many factors, e.g. when using ISAM2 as a + /// fixed-lag smoother, enable this option to add factors in the first + /// available factor slots, to avoid accumulating NULL factor slots, at the + /// cost of having to search for slots every time a factor is added. bool findUnusedFactorSlots; - /** Specify parameters as constructor arguments */ - ISAM2Params( - OptimizationParams _optimizationParams = ISAM2GaussNewtonParams(), ///< see ISAM2Params::optimizationParams - RelinearizationThreshold _relinearizeThreshold = 0.1, ///< see ISAM2Params::relinearizeThreshold - int _relinearizeSkip = 10, ///< see ISAM2Params::relinearizeSkip - bool _enableRelinearization = true, ///< see ISAM2Params::enableRelinearization - bool _evaluateNonlinearError = false, ///< see ISAM2Params::evaluateNonlinearError - Factorization _factorization = ISAM2Params::CHOLESKY, ///< see ISAM2Params::factorization - bool _cacheLinearizedFactors = true, ///< see ISAM2Params::cacheLinearizedFactors - const KeyFormatter& _keyFormatter = DefaultKeyFormatter ///< see ISAM2::Params::keyFormatter - ) : optimizationParams(_optimizationParams), relinearizeThreshold(_relinearizeThreshold), - relinearizeSkip(_relinearizeSkip), enableRelinearization(_enableRelinearization), - evaluateNonlinearError(_evaluateNonlinearError), factorization(_factorization), - cacheLinearizedFactors(_cacheLinearizedFactors), keyFormatter(_keyFormatter), - enableDetailedResults(false), enablePartialRelinearizationCheck(false), - findUnusedFactorSlots(false) {} + /** + * Specify parameters as constructor arguments + * See the documentation of member variables above. + */ + ISAM2Params(OptimizationParams _optimizationParams = ISAM2GaussNewtonParams(), + RelinearizationThreshold _relinearizeThreshold = 0.1, + int _relinearizeSkip = 10, bool _enableRelinearization = true, + bool _evaluateNonlinearError = false, + Factorization _factorization = ISAM2Params::CHOLESKY, + bool _cacheLinearizedFactors = true, + const KeyFormatter& _keyFormatter = + DefaultKeyFormatter ///< see ISAM2::Params::keyFormatter + ) + : optimizationParams(_optimizationParams), + relinearizeThreshold(_relinearizeThreshold), + relinearizeSkip(_relinearizeSkip), + enableRelinearization(_enableRelinearization), + evaluateNonlinearError(_evaluateNonlinearError), + factorization(_factorization), + cacheLinearizedFactors(_cacheLinearizedFactors), + keyFormatter(_keyFormatter), + enableDetailedResults(false), + enablePartialRelinearizationCheck(false), + findUnusedFactorSlots(false) {} /// print iSAM2 parameters void print(const std::string& str = "") const { - std::cout << str << "\n"; - if(optimizationParams.type() == typeid(ISAM2GaussNewtonParams)) - boost::get(optimizationParams).print("optimizationParams: "); - else if(optimizationParams.type() == typeid(ISAM2DoglegParams)) - boost::get(optimizationParams).print("optimizationParams: "); + using std::cout; + cout << str << "\n"; + + static const std::string kStr("optimizationParams: "); + if (optimizationParams.type() == typeid(ISAM2GaussNewtonParams)) + boost::get(optimizationParams).print(); + else if (optimizationParams.type() == typeid(ISAM2DoglegParams)) + boost::get(optimizationParams).print(kStr); else - std::cout << "optimizationParams: " << "{unknown type}" << "\n"; - if(relinearizeThreshold.type() == typeid(double)) - std::cout << "relinearizeThreshold: " << boost::get(relinearizeThreshold) << "\n"; - else - { - std::cout << "relinearizeThreshold: " << "{mapped}" << "\n"; - for(const ISAM2ThresholdMapValue& value: boost::get(relinearizeThreshold)) { - std::cout << " '" << value.first << "' -> [" << value.second.transpose() << " ]\n"; + cout << kStr << "{unknown type}\n"; + + cout << "relinearizeThreshold: "; + if (relinearizeThreshold.type() == typeid(double)) { + cout << boost::get(relinearizeThreshold) << "\n"; + } else { + cout << "{mapped}\n"; + for (const ISAM2ThresholdMapValue& value : + boost::get(relinearizeThreshold)) { + cout << " '" << value.first + << "' -> [" << value.second.transpose() << " ]\n"; } } - std::cout << "relinearizeSkip: " << relinearizeSkip << "\n"; - std::cout << "enableRelinearization: " << enableRelinearization << "\n"; - std::cout << "evaluateNonlinearError: " << evaluateNonlinearError << "\n"; - std::cout << "factorization: " << factorizationTranslator(factorization) << "\n"; - std::cout << "cacheLinearizedFactors: " << cacheLinearizedFactors << "\n"; - std::cout << "enableDetailedResults: " << enableDetailedResults << "\n"; - std::cout << "enablePartialRelinearizationCheck: " << enablePartialRelinearizationCheck << "\n"; - std::cout << "findUnusedFactorSlots: " << findUnusedFactorSlots << "\n"; - std::cout.flush(); + + cout << "relinearizeSkip: " << relinearizeSkip << "\n"; + cout << "enableRelinearization: " << enableRelinearization + << "\n"; + cout << "evaluateNonlinearError: " << evaluateNonlinearError + << "\n"; + cout << "factorization: " + << factorizationTranslator(factorization) << "\n"; + cout << "cacheLinearizedFactors: " << cacheLinearizedFactors + << "\n"; + cout << "enableDetailedResults: " << enableDetailedResults + << "\n"; + cout << "enablePartialRelinearizationCheck: " + << enablePartialRelinearizationCheck << "\n"; + cout << "findUnusedFactorSlots: " << findUnusedFactorSlots + << "\n"; + cout.flush(); } /// @name Getters and Setters for all properties /// @{ - OptimizationParams getOptimizationParams() const { return this->optimizationParams; } - RelinearizationThreshold getRelinearizeThreshold() const { return relinearizeThreshold; } + OptimizationParams getOptimizationParams() const { + return this->optimizationParams; + } + RelinearizationThreshold getRelinearizeThreshold() const { + return relinearizeThreshold; + } int getRelinearizeSkip() const { return relinearizeSkip; } bool isEnableRelinearization() const { return enableRelinearization; } bool isEvaluateNonlinearError() const { return evaluateNonlinearError; } - std::string getFactorization() const { return factorizationTranslator(factorization); } + std::string getFactorization() const { + return factorizationTranslator(factorization); + } bool isCacheLinearizedFactors() const { return cacheLinearizedFactors; } KeyFormatter getKeyFormatter() const { return keyFormatter; } bool isEnableDetailedResults() const { return enableDetailedResults; } - bool isEnablePartialRelinearizationCheck() const { return enablePartialRelinearizationCheck; } + bool isEnablePartialRelinearizationCheck() const { + return enablePartialRelinearizationCheck; + } - void setOptimizationParams(OptimizationParams optimizationParams) { this->optimizationParams = optimizationParams; } - void setRelinearizeThreshold(RelinearizationThreshold relinearizeThreshold) { this->relinearizeThreshold = relinearizeThreshold; } - void setRelinearizeSkip(int relinearizeSkip) { this->relinearizeSkip = relinearizeSkip; } - void setEnableRelinearization(bool enableRelinearization) { this->enableRelinearization = enableRelinearization; } - void setEvaluateNonlinearError(bool evaluateNonlinearError) { this->evaluateNonlinearError = evaluateNonlinearError; } - void setFactorization(const std::string& factorization) { this->factorization = factorizationTranslator(factorization); } - void setCacheLinearizedFactors(bool cacheLinearizedFactors) { this->cacheLinearizedFactors = cacheLinearizedFactors; } - void setKeyFormatter(KeyFormatter keyFormatter) { this->keyFormatter = keyFormatter; } - void setEnableDetailedResults(bool enableDetailedResults) { this->enableDetailedResults = enableDetailedResults; } - void setEnablePartialRelinearizationCheck(bool enablePartialRelinearizationCheck) { this->enablePartialRelinearizationCheck = enablePartialRelinearizationCheck; } + void setOptimizationParams(OptimizationParams optimizationParams) { + this->optimizationParams = optimizationParams; + } + void setRelinearizeThreshold(RelinearizationThreshold relinearizeThreshold) { + this->relinearizeThreshold = relinearizeThreshold; + } + void setRelinearizeSkip(int relinearizeSkip) { + this->relinearizeSkip = relinearizeSkip; + } + void setEnableRelinearization(bool enableRelinearization) { + this->enableRelinearization = enableRelinearization; + } + void setEvaluateNonlinearError(bool evaluateNonlinearError) { + this->evaluateNonlinearError = evaluateNonlinearError; + } + void setFactorization(const std::string& factorization) { + this->factorization = factorizationTranslator(factorization); + } + void setCacheLinearizedFactors(bool cacheLinearizedFactors) { + this->cacheLinearizedFactors = cacheLinearizedFactors; + } + void setKeyFormatter(KeyFormatter keyFormatter) { + this->keyFormatter = keyFormatter; + } + void setEnableDetailedResults(bool enableDetailedResults) { + this->enableDetailedResults = enableDetailedResults; + } + void setEnablePartialRelinearizationCheck( + bool enablePartialRelinearizationCheck) { + this->enablePartialRelinearizationCheck = enablePartialRelinearizationCheck; + } GaussianFactorGraph::Eliminate getEliminationFunction() const { return factorization == CHOLESKY - ? (GaussianFactorGraph::Eliminate)EliminatePreferCholesky - : (GaussianFactorGraph::Eliminate)EliminateQR; + ? (GaussianFactorGraph::Eliminate)EliminatePreferCholesky + : (GaussianFactorGraph::Eliminate)EliminateQR; } /// @} @@ -275,8 +385,9 @@ struct GTSAM_EXPORT ISAM2Result { * delta, as computed by ISAM2::calculateEstimate(). * \li New variables will be evaluated at their initialization points passed * into the current call to update. - * \par Note: This will only be computed if ISAM2Params::evaluateNonlinearError - * is set to \c true, because there is some cost to this computation. + * \par Note: This will only be computed if + * ISAM2Params::evaluateNonlinearError is set to \c true, because there is + * some cost to this computation. */ boost::optional errorBefore; @@ -286,8 +397,9 @@ struct GTSAM_EXPORT ISAM2Result { * variables have undergone one linear update. Variable values are * again computed by combining their linearization points with their * partial linear deltas, by ISAM2::calculateEstimate(). - * \par Note: This will only be computed if ISAM2Params::evaluateNonlinearError - * is set to \c true, because there is some cost to this computation. + * \par Note: This will only be computed if + * ISAM2Params::evaluateNonlinearError is set to \c true, because there is + * some cost to this computation. */ boost::optional errorAfter; @@ -309,7 +421,8 @@ struct GTSAM_EXPORT ISAM2Result { */ size_t variablesReeliminated; - /** The number of factors that were included in reelimination of the Bayes' tree. */ + /** The number of factors that were included in reelimination of the Bayes' + * tree. */ size_t factorsRecalculated; /** The number of cliques in the Bayes' Tree */ @@ -332,14 +445,29 @@ struct GTSAM_EXPORT ISAM2Result { * observed, new, or on the path up to the root clique from another * reeliminated variable. */ bool isReeliminated; - bool isAboveRelinThreshold; ///< Whether the variable was just relinearized due to being above the relinearization threshold - bool isRelinearizeInvolved; ///< Whether the variable was below the relinearization threshold but was relinearized by being involved in a factor with a variable above the relinearization threshold - bool isRelinearized; /// Whether the variable was relinearized, either by being above the relinearization threshold or by involvement. - bool isObserved; ///< Whether the variable was just involved in new factors - bool isNew; ///< Whether the variable itself was just added - bool inRootClique; ///< Whether the variable is in the root clique - VariableStatus(): isReeliminated(false), isAboveRelinThreshold(false), isRelinearizeInvolved(false), - isRelinearized(false), isObserved(false), isNew(false), inRootClique(false) {} + bool isAboveRelinThreshold; ///< Whether the variable was just + ///< relinearized due to being above the + ///< relinearization threshold + bool isRelinearizeInvolved; ///< Whether the variable was below the + ///< relinearization threshold but was + ///< relinearized by being involved in a + ///< factor with a variable above the + ///< relinearization threshold + bool isRelinearized; /// Whether the variable was relinearized, either by + /// being above the relinearization threshold or by + /// involvement. + bool isObserved; ///< Whether the variable was just involved in new + ///< factors + bool isNew; ///< Whether the variable itself was just added + bool inRootClique; ///< Whether the variable is in the root clique + VariableStatus() + : isReeliminated(false), + isAboveRelinThreshold(false), + isRelinearizeInvolved(false), + isRelinearized(false), + isObserved(false), + isNew(false), + inRootClique(false) {} }; /** The status of each variable during this update, see VariableStatus. @@ -351,24 +479,27 @@ struct GTSAM_EXPORT ISAM2Result { * Detail for information about the results data stored here. */ boost::optional detail; - void print(const std::string str = "") const { - std::cout << str << " Reelimintated: " << variablesReeliminated << " Relinearized: " << variablesRelinearized << " Cliques: " << cliques << std::endl; + using std::cout; + cout << str << " Reelimintated: " << variablesReeliminated + << " Relinearized: " << variablesRelinearized + << " Cliques: " << cliques << std::endl; } /** Getters and Setters */ - size_t getVariablesRelinearized() const { return variablesRelinearized; }; - size_t getVariablesReeliminated() const { return variablesReeliminated; }; - size_t getCliques() const { return cliques; }; + size_t getVariablesRelinearized() const { return variablesRelinearized; } + size_t getVariablesReeliminated() const { return variablesReeliminated; } + size_t getCliques() const { return cliques; } }; /** - * Specialized Clique structure for ISAM2, incorporating caching and gradient contribution + * Specialized Clique structure for ISAM2, incorporating caching and gradient + * contribution * TODO: more documentation */ -class GTSAM_EXPORT ISAM2Clique : public BayesTreeCliqueBase -{ -public: +class GTSAM_EXPORT ISAM2Clique + : public BayesTreeCliqueBase { + public: typedef ISAM2Clique This; typedef BayesTreeCliqueBase Base; typedef boost::shared_ptr shared_ptr; @@ -383,13 +514,16 @@ public: /// Default constructor ISAM2Clique() : Base() {} - /// Copy constructor, does *not* copy solution pointers as these are invalid in different trees. - ISAM2Clique(const ISAM2Clique& other) : - Base(other), cachedFactor_(other.cachedFactor_), gradientContribution_(other.gradientContribution_) {} + /// Copy constructor, does *not* copy solution pointers as these are invalid + /// in different trees. + ISAM2Clique(const ISAM2Clique& other) + : Base(other), + cachedFactor_(other.cachedFactor_), + gradientContribution_(other.gradientContribution_) {} - /// Assignment operator, does *not* copy solution pointers as these are invalid in different trees. - ISAM2Clique& operator=(const ISAM2Clique& other) - { + /// Assignment operator, does *not* copy solution pointers as these are + /// invalid in different trees. + ISAM2Clique& operator=(const ISAM2Clique& other) { Base::operator=(other); cachedFactor_ = other.cachedFactor_; gradientContribution_ = other.gradientContribution_; @@ -397,7 +531,8 @@ public: } /// Overridden to also store the remaining factor and gradient contribution - void setEliminationResult(const FactorGraphType::EliminationResult& eliminationResult); + void setEliminationResult( + const FactorGraphType::EliminationResult& eliminationResult); /** Access the cached factor */ Base::FactorType::shared_ptr& cachedFactor() { return cachedFactor_; } @@ -405,44 +540,45 @@ public: /** Access the gradient contribution */ const Vector& gradientContribution() const { return gradientContribution_; } - bool equals(const This& other, double tol=1e-9) const; + bool equals(const This& other, double tol = 1e-9) const; /** print this node */ - void print(const std::string& s = "", const KeyFormatter& formatter = DefaultKeyFormatter) const; - -private: + void print(const std::string& s = "", + const KeyFormatter& formatter = DefaultKeyFormatter) const; + private: /** Serialization function */ friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar & BOOST_SERIALIZATION_NVP(cachedFactor_); - ar & BOOST_SERIALIZATION_NVP(gradientContribution_); + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar& BOOST_SERIALIZATION_NVP(cachedFactor_); + ar& BOOST_SERIALIZATION_NVP(gradientContribution_); } -}; // \struct ISAM2Clique +}; // \struct ISAM2Clique /** * @addtogroup ISAM2 - * Implementation of the full ISAM2 algorithm for incremental nonlinear optimization. + * Implementation of the full ISAM2 algorithm for incremental nonlinear + * optimization. * - * The typical cycle of using this class to create an instance by providing ISAM2Params - * to the constructor, then add measurements and variables as they arrive using the update() - * method. At any time, calculateEstimate() may be called to obtain the current - * estimate of all variables. + * The typical cycle of using this class to create an instance by providing + * ISAM2Params to the constructor, then add measurements and variables as they + * arrive using the update() method. At any time, calculateEstimate() may be + * called to obtain the current estimate of all variables. * */ -class GTSAM_EXPORT ISAM2: public BayesTree { - -protected: - +class GTSAM_EXPORT ISAM2 : public BayesTree { + protected: /** The current linearization point */ Values theta_; - /** VariableIndex lets us look up factors by involved variable and keeps track of dimensions */ + /** VariableIndex lets us look up factors by involved variable and keeps track + * of dimensions */ VariableIndex variableIndex_; - /** The linear delta from the last linear solution, an update to the estimate in theta + /** The linear delta from the last linear solution, an update to the estimate + * in theta * * This is \c mutable because it is a "cached" variable - it is not updated * until either requested with getDelta() or calculateEstimate(), or needed @@ -450,8 +586,10 @@ protected: */ mutable VectorValues delta_; - mutable VectorValues deltaNewton_; // Only used when using Dogleg - stores the Gauss-Newton update - mutable VectorValues RgProd_; // Only used when using Dogleg - stores R*g and is updated incrementally + mutable VectorValues deltaNewton_; // Only used when using Dogleg - stores + // the Gauss-Newton update + mutable VectorValues RgProd_; // Only used when using Dogleg - stores R*g and + // is updated incrementally /** A cumulative mask for the variables that were replaced and have not yet * been updated in the linear solution delta_, this is only used internally, @@ -461,9 +599,11 @@ protected: * This is \c mutable because it is used internally to not update delta_ * until it is needed. */ - mutable KeySet deltaReplacedMask_; // TODO: Make sure accessed in the right way + mutable KeySet + deltaReplacedMask_; // TODO: Make sure accessed in the right way - /** All original nonlinear factors are stored here to use during relinearization */ + /** All original nonlinear factors are stored here to use during + * relinearization */ NonlinearFactorGraph nonlinearFactors_; /** The current linear factors, which are only updated as needed */ @@ -479,20 +619,21 @@ protected: * variables and thus cannot have their linearization points changed. */ KeySet fixedVariables_; - int update_count_; ///< Counter incremented every update(), used to determine periodic relinearization + int update_count_; ///< Counter incremented every update(), used to determine + ///< periodic relinearization -public: - - typedef ISAM2 This; ///< This class - typedef BayesTree Base; ///< The BayesTree base class - typedef Base::Clique Clique; ///< A clique - typedef Base::sharedClique sharedClique; ///< Shared pointer to a clique - typedef Base::Cliques Cliques; ///< List of Clique typedef from base class + public: + typedef ISAM2 This; ///< This class + typedef BayesTree Base; ///< The BayesTree base class + typedef Base::Clique Clique; ///< A clique + typedef Base::sharedClique sharedClique; ///< Shared pointer to a clique + typedef Base::Cliques Cliques; ///< List of Clique typedef from base class /** Create an empty ISAM2 instance */ - ISAM2(const ISAM2Params& params); + explicit ISAM2(const ISAM2Params& params); - /** Create an empty ISAM2 instance using the default set of parameters (see ISAM2Params) */ + /** Create an empty ISAM2 instance using the default set of parameters (see + * ISAM2Params) */ ISAM2(); /** default virtual destructor */ @@ -513,26 +654,31 @@ public: * thresholds. * * @param newFactors The new factors to be added to the system - * @param newTheta Initialization points for new variables to be added to the system. - * You must include here all new variables occuring in newFactors (which were not already - * in the system). There must not be any variables here that do not occur in newFactors, - * and additionally, variables that were already in the system must not be included here. + * @param newTheta Initialization points for new variables to be added to the + * system. You must include here all new variables occuring in newFactors + * (which were not already in the system). There must not be any variables + * here that do not occur in newFactors, and additionally, variables that were + * already in the system must not be included here. * @param removeFactorIndices Indices of factors to remove from system - * @param force_relinearize Relinearize any variables whose delta magnitude is sufficiently - * large (Params::relinearizeThreshold), regardless of the relinearization interval - * (Params::relinearizeSkip). - * @param constrainedKeys is an optional map of keys to group labels, such that a variable can - * be constrained to a particular grouping in the BayesTree - * @param noRelinKeys is an optional set of nonlinear keys that iSAM2 will hold at a constant linearization - * point, regardless of the size of the linear delta - * @param extraReelimKeys is an optional set of nonlinear keys that iSAM2 will re-eliminate, regardless - * of the size of the linear delta. This allows the provided keys to be reordered. + * @param force_relinearize Relinearize any variables whose delta magnitude is + * sufficiently large (Params::relinearizeThreshold), regardless of the + * relinearization interval (Params::relinearizeSkip). + * @param constrainedKeys is an optional map of keys to group labels, such + * that a variable can be constrained to a particular grouping in the + * BayesTree + * @param noRelinKeys is an optional set of nonlinear keys that iSAM2 will + * hold at a constant linearization point, regardless of the size of the + * linear delta + * @param extraReelimKeys is an optional set of nonlinear keys that iSAM2 will + * re-eliminate, regardless of the size of the linear delta. This allows the + * provided keys to be reordered. * @return An ISAM2Result struct containing information about the update */ - virtual ISAM2Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), + virtual ISAM2Result update( + const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(), const FactorIndices& removeFactorIndices = FactorIndices(), - const boost::optional >& constrainedKeys = boost::none, + const boost::optional >& constrainedKeys = boost::none, const boost::optional >& noRelinKeys = boost::none, const boost::optional >& extraReelimKeys = boost::none, bool force_relinearize = false); @@ -542,48 +688,50 @@ public: * requested to be marginalized. Marginalization leaves a linear * approximation of the marginal in the system, and the linearization points * of any variables involved in this linear marginal become fixed. The set - * fixed variables will include any key involved with the marginalized variables - * in the original factors, and possibly additional ones due to fill-in. + * fixed variables will include any key involved with the marginalized + * variables in the original factors, and possibly additional ones due to + * fill-in. * - * If provided, 'marginalFactorsIndices' will be augmented with the factor graph - * indices of the marginal factors added during the 'marginalizeLeaves' call + * If provided, 'marginalFactorsIndices' will be augmented with the factor + * graph indices of the marginal factors added during the 'marginalizeLeaves' + * call * - * If provided, 'deletedFactorsIndices' will be augmented with the factor graph - * indices of any factor that was removed during the 'marginalizeLeaves' call + * If provided, 'deletedFactorsIndices' will be augmented with the factor + * graph indices of any factor that was removed during the 'marginalizeLeaves' + * call */ - void marginalizeLeaves(const FastList& leafKeys, - boost::optional marginalFactorsIndices = boost::none, - boost::optional deletedFactorsIndices = boost::none); + void marginalizeLeaves( + const FastList& leafKeys, + boost::optional marginalFactorsIndices = boost::none, + boost::optional deletedFactorsIndices = boost::none); /// Access the current linearization point - const Values& getLinearizationPoint() const { - return theta_; - } + const Values& getLinearizationPoint() const { return theta_; } /// Check whether variable with given key exists in linearization point - bool valueExists(Key key) const { - return theta_.exists(key); - } + bool valueExists(Key key) const { return theta_.exists(key); } - /** Compute an estimate from the incomplete linear delta computed during the last update. - * This delta is incomplete because it was not updated below wildfire_threshold. If only - * a single variable is needed, it is faster to call calculateEstimate(const KEY&). + /** Compute an estimate from the incomplete linear delta computed during the + * last update. This delta is incomplete because it was not updated below + * wildfire_threshold. If only a single variable is needed, it is faster to + * call calculateEstimate(const KEY&). */ Values calculateEstimate() const; - /** Compute an estimate for a single variable using its incomplete linear delta computed - * during the last update. This is faster than calling the no-argument version of - * calculateEstimate, which operates on all variables. + /** Compute an estimate for a single variable using its incomplete linear + * delta computed during the last update. This is faster than calling the + * no-argument version of calculateEstimate, which operates on all variables. * @param key * @return */ - template + template VALUE calculateEstimate(Key key) const; - /** Compute an estimate for a single variable using its incomplete linear delta computed - * during the last update. This is faster than calling the no-argument version of - * calculateEstimate, which operates on all variables. This is a non-templated version - * that returns a Value base class for use with the MATLAB wrapper. + /** Compute an estimate for a single variable using its incomplete linear + * delta computed during the last update. This is faster than calling the + * no-argument version of calculateEstimate, which operates on all variables. + * This is a non-templated version that returns a Value base class for use + * with the MATLAB wrapper. * @param key * @return */ @@ -598,7 +746,8 @@ public: /** Internal implementation functions */ struct Impl; - /** Compute an estimate using a complete delta computed by a full back-substitution. + /** Compute an estimate using a complete delta computed by a full + * back-substitution. */ Values calculateBestEstimate() const; @@ -609,7 +758,9 @@ public: double error(const VectorValues& x) const; /** Access the set of nonlinear factors */ - const NonlinearFactorGraph& getFactorsUnsafe() const { return nonlinearFactors_; } + const NonlinearFactorGraph& getFactorsUnsafe() const { + return nonlinearFactors_; + } /** Access the nonlinear variable index */ const VariableIndex& getVariableIndex() const { return variableIndex_; } @@ -628,31 +779,36 @@ public: /** prints out clique statistics */ void printStats() const { getCliqueData().getStats().print(); } - - /** Compute the gradient of the energy function, \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} R x - d - * \right\Vert^2 \f$, centered around zero. The gradient about zero is \f$ -R^T d \f$. See also - * gradient(const GaussianBayesNet&, const VectorValues&). + + /** Compute the gradient of the energy function, \f$ \nabla_{x=0} \left\Vert + * \Sigma^{-1} R x - d \right\Vert^2 \f$, centered around zero. The gradient + * about zero is \f$ -R^T d \f$. See also gradient(const GaussianBayesNet&, + * const VectorValues&). * * @return A VectorValues storing the gradient. */ VectorValues gradientAtZero() const; - + /// @} -protected: - + protected: FastSet getAffectedFactors(const FastList& keys) const; - GaussianFactorGraph::shared_ptr relinearizeAffectedFactors(const FastList& affectedKeys, const KeySet& relinKeys) const; - GaussianFactorGraph getCachedBoundaryFactors(Cliques& orphans); + GaussianFactorGraph::shared_ptr relinearizeAffectedFactors( + const FastList& affectedKeys, const KeySet& relinKeys) const; + GaussianFactorGraph getCachedBoundaryFactors(const Cliques& orphans); - virtual boost::shared_ptr recalculate(const KeySet& markedKeys, const KeySet& relinKeys, - const std::vector& observedKeys, const KeySet& unusedIndices, const boost::optional >& constrainKeys, ISAM2Result& result); + virtual boost::shared_ptr recalculate( + const KeySet& markedKeys, const KeySet& relinKeys, + const std::vector& observedKeys, const KeySet& unusedIndices, + const boost::optional >& constrainKeys, + ISAM2Result& result); void updateDelta(bool forceFullSolve = false) const; -}; // ISAM2 +}; // ISAM2 /// traits -template<> struct traits : public Testable {}; +template <> +struct traits : public Testable {}; /// Optimize the BayesTree, starting from the root. /// @param replaced Needs to contain @@ -665,19 +821,21 @@ template<> struct traits : public Testable {}; /// and recursive backsubstitution might eventually stop if none of the changed /// variables are contained in the subtree. /// @return The number of variables that were solved for -template -size_t optimizeWildfire(const boost::shared_ptr& root, - double threshold, const KeySet& replaced, VectorValues& delta); +template +size_t optimizeWildfire(const boost::shared_ptr& root, double threshold, + const KeySet& replaced, VectorValues& delta); -template +template size_t optimizeWildfireNonRecursive(const boost::shared_ptr& root, - double threshold, const KeySet& replaced, VectorValues& delta); + double threshold, const KeySet& replaced, + VectorValues& delta); -/// calculate the number of non-zero entries for the tree starting at clique (use root for complete matrix) -template +/// calculate the number of non-zero entries for the tree starting at clique +/// (use root for complete matrix) +template int calculate_nnz(const boost::shared_ptr& clique); -} /// namespace gtsam +} // namespace gtsam -#include #include +#include From 315ea79e2119c913c0ab773807dfd17375a2c0d2 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 29 Sep 2018 00:06:31 -0400 Subject: [PATCH 09/51] Ignore VS code .env file --- .gitignore | 1 + 1 file changed, 1 insertion(+) diff --git a/.gitignore b/.gitignore index b62617d21..2682a6748 100644 --- a/.gitignore +++ b/.gitignore @@ -16,3 +16,4 @@ cython/gtsam.pyx cython/gtsam.so cython/gtsam_wrapper.pxd .vscode +.env From 05d5179bc399ab86eaa40b0fe1c8b2d0f0bbc3f4 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 29 Sep 2018 17:31:46 -0400 Subject: [PATCH 10/51] Some more cleanup --- examples/ISAM2Example_SmartFactor.cpp | 31 ++++++++++++++++----------- 1 file changed, 18 insertions(+), 13 deletions(-) diff --git a/examples/ISAM2Example_SmartFactor.cpp b/examples/ISAM2Example_SmartFactor.cpp index 77e29b8ff..8331ade04 100644 --- a/examples/ISAM2Example_SmartFactor.cpp +++ b/examples/ISAM2Example_SmartFactor.cpp @@ -5,11 +5,10 @@ */ #include +#include #include #include -#include - #include #include @@ -56,27 +55,32 @@ int main(int argc, char* argv[]) { vector poses = {pose1, pose2, pose3, pose4, pose5}; - SmartFactor::shared_ptr smartFactor(new SmartFactor(measurementNoise, K)); - graph.push_back(smartFactor); - + // Add first pose graph.emplace_shared>(X(0), poses[0], noise); initialEstimate.insert(X(0), poses[0].compose(delta)); - smartFactor->add(PinholePose(poses[0], K).project(point), X(0)); + // Create smart factor with measurement from first pose only + SmartFactor::shared_ptr smartFactor(new SmartFactor(measurementNoise, K)); + smartFactor->add(PinholePose(poses[0], K).project(point), X(0)); + graph.push_back(smartFactor); + + // loop over remaining poses for (size_t i = 1; i < 5; i++) { cout << "****************************************************" << endl; cout << "i = " << i << endl; - // Add measurement to smart factor - PinholePose camera(poses[i], K); - Point2 measurement = camera.project(point); - smartFactor->add(measurement, X(i)); - cout << "Measurement " << i << "" << measurement << endl; - // Add prior on new pose graph.emplace_shared>(X(i), poses[i], noise); initialEstimate.insert(X(i), poses[i].compose(delta)); + // "Simulate" measurement from this pose + PinholePose camera(poses[i], K); + Point2 measurement = camera.project(point); + cout << "Measurement " << i << "" << measurement << endl; + + // Add measurement to smart factor + smartFactor->add(measurement, X(i)); + // Update iSAM2 ISAM2Result result = isam.update(graph, initialEstimate); result.print(); @@ -84,7 +88,8 @@ int main(int argc, char* argv[]) { cout << "Detailed results:" << endl; for (auto keyedStatus : result.detail->variableStatus) { const auto& status = keyedStatus.second; - cout << keyedStatus.first << " {" << endl; + PrintKey(keyedStatus.first); + cout << " {" << endl; cout << "reeliminated: " << status.isReeliminated << endl; cout << "relinearized above thresh: " << status.isAboveRelinThreshold << endl; From a31294d7778934d51f6e5ff1e389edf210e57236 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 29 Sep 2018 17:52:20 -0400 Subject: [PATCH 11/51] Modernized, cleaned up, and turned off non-recursive version (fow now) because it has a bug. --- gtsam/nonlinear/ISAM2-impl.cpp | 317 +++++---- gtsam/nonlinear/ISAM2-impl.h | 11 +- gtsam/nonlinear/ISAM2-inl.h | 287 +------- gtsam/nonlinear/ISAM2.cpp | 1149 +++++++++++++++++++++----------- gtsam/nonlinear/ISAM2.h | 78 ++- tests/testGaussianISAM2.cpp | 2 +- 6 files changed, 1011 insertions(+), 833 deletions(-) diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index 3b5156535..aa201138c 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -11,33 +11,35 @@ /** * @file ISAM2-impl.cpp - * @brief Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization. + * @brief Incremental update functionality (ISAM2) for BayesTree, with fluid + * relinearization. * @author Michael Kaess * @author Richard Roberts */ -#include -#include // for selective linearization thresholds #include -#include // for GTSAM_USE_TBB +#include // for GTSAM_USE_TBB +#include // for selective linearization thresholds +#include -#include #include +#include +#include +#include using namespace std; namespace gtsam { /* ************************************************************************* */ -void ISAM2::Impl::AddVariables( - const Values& newTheta, Values& theta, VectorValues& delta, - VectorValues& deltaNewton, VectorValues& RgProd, - const KeyFormatter& keyFormatter) -{ +void ISAM2::Impl::AddVariables(const Values& newTheta, Values& theta, + VectorValues& delta, VectorValues& deltaNewton, + VectorValues& RgProd, + const KeyFormatter& keyFormatter) { const bool debug = ISDEBUG("ISAM2 AddVariables"); theta.insert(newTheta); - if(debug) newTheta.print("The new variables are: "); + if (debug) newTheta.print("The new variables are: "); // Add zeros into the VectorValues delta.insert(newTheta.zeroVectors()); deltaNewton.insert(newTheta.zeroVectors()); @@ -45,55 +47,55 @@ void ISAM2::Impl::AddVariables( } /* ************************************************************************* */ -void ISAM2::Impl::AddFactorsStep1(const NonlinearFactorGraph& newFactors, bool useUnusedSlots, - NonlinearFactorGraph& nonlinearFactors, FactorIndices& newFactorIndices) -{ +void ISAM2::Impl::AddFactorsStep1(const NonlinearFactorGraph& newFactors, + bool useUnusedSlots, + NonlinearFactorGraph& nonlinearFactors, + FactorIndices& newFactorIndices) { newFactorIndices.resize(newFactors.size()); - if(useUnusedSlots) - { + if (useUnusedSlots) { size_t globalFactorIndex = 0; - for(size_t newFactorIndex = 0; newFactorIndex < newFactors.size(); ++newFactorIndex) - { + for (size_t newFactorIndex = 0; newFactorIndex < newFactors.size(); + ++newFactorIndex) { // Loop to find the next available factor slot - do - { - // If we need to add more factors than we have room for, resize nonlinearFactors, - // filling the new slots with NULL factors. Otherwise, check if the current - // factor in nonlinearFactors is already used, and if so, increase - // globalFactorIndex. If the current factor in nonlinearFactors is unused, break - // out of the loop and use the current slot. - if(globalFactorIndex >= nonlinearFactors.size()) - nonlinearFactors.resize(nonlinearFactors.size() + newFactors.size() - newFactorIndex); - else if(nonlinearFactors[globalFactorIndex]) - ++ globalFactorIndex; + do { + // If we need to add more factors than we have room for, resize + // nonlinearFactors, filling the new slots with NULL factors. Otherwise, + // check if the current factor in nonlinearFactors is already used, and + // if so, increase globalFactorIndex. If the current factor in + // nonlinearFactors is unused, break out of the loop and use the current + // slot. + if (globalFactorIndex >= nonlinearFactors.size()) + nonlinearFactors.resize(nonlinearFactors.size() + newFactors.size() - + newFactorIndex); + else if (nonlinearFactors[globalFactorIndex]) + ++globalFactorIndex; else break; - } while(true); + } while (true); // Use the current slot, updating nonlinearFactors and newFactorSlots. nonlinearFactors[globalFactorIndex] = newFactors[newFactorIndex]; newFactorIndices[newFactorIndex] = globalFactorIndex; } - } - else - { + } else { // We're not looking for unused slots, so just add the factors at the end. - for(size_t i = 0; i < newFactors.size(); ++i) + for (size_t i = 0; i < newFactors.size(); ++i) newFactorIndices[i] = i + nonlinearFactors.size(); nonlinearFactors.push_back(newFactors); } } /* ************************************************************************* */ -void ISAM2::Impl::RemoveVariables(const KeySet& unusedKeys, const FastVector& roots, +void ISAM2::Impl::RemoveVariables(const KeySet& unusedKeys, + const FastVector& roots, Values& theta, VariableIndex& variableIndex, - VectorValues& delta, VectorValues& deltaNewton, VectorValues& RgProd, - KeySet& replacedKeys, Base::Nodes& nodes, - KeySet& fixedVariables) -{ + VectorValues& delta, + VectorValues& deltaNewton, + VectorValues& RgProd, KeySet& replacedKeys, + Base::Nodes& nodes, KeySet& fixedVariables) { variableIndex.removeUnusedVariables(unusedKeys.begin(), unusedKeys.end()); - for(Key key: unusedKeys) { + for (Key key : unusedKeys) { delta.erase(key); deltaNewton.erase(key); RgProd.erase(key); @@ -105,26 +107,28 @@ void ISAM2::Impl::RemoveVariables(const KeySet& unusedKeys, const FastVector(&relinearizeThreshold)) - { - for(const VectorValues::KeyValuePair& key_delta: delta) { + if (const double* threshold = boost::get(&relinearizeThreshold)) { + for (const VectorValues::KeyValuePair& key_delta : delta) { double maxDelta = key_delta.second.lpNorm(); - if(maxDelta >= *threshold) - relinKeys.insert(key_delta.first); + if (maxDelta >= *threshold) relinKeys.insert(key_delta.first); } - } - else if(const FastMap* thresholds = boost::get >(&relinearizeThreshold)) - { - for(const VectorValues::KeyValuePair& key_delta: delta) { - const Vector& threshold = thresholds->find(Symbol(key_delta.first).chr())->second; - if(threshold.rows() != key_delta.second.rows()) - throw std::invalid_argument("Relinearization threshold vector dimensionality for '" + std::string(1, Symbol(key_delta.first).chr()) + "' passed into iSAM2 parameters does not match actual variable dimensionality."); - if((key_delta.second.array().abs() > threshold.array()).any()) + } else if (const FastMap* thresholds = + boost::get >(&relinearizeThreshold)) { + for (const VectorValues::KeyValuePair& key_delta : delta) { + const Vector& threshold = + thresholds->find(Symbol(key_delta.first).chr())->second; + if (threshold.rows() != key_delta.second.rows()) + throw std::invalid_argument( + "Relinearization threshold vector dimensionality for '" + + std::string(1, Symbol(key_delta.first).chr()) + + "' passed into iSAM2 parameters does not match actual variable " + "dimensionality."); + if ((key_delta.second.array().abs() > threshold.array()).any()) relinKeys.insert(key_delta.first); } } @@ -133,81 +137,86 @@ KeySet ISAM2::Impl::CheckRelinearizationFull(const VectorValues& delta, } /* ************************************************************************* */ -void CheckRelinearizationRecursiveDouble(KeySet& relinKeys, double threshold, - const VectorValues& delta, const ISAM2Clique::shared_ptr& clique) -{ +static void CheckRelinearizationRecursiveDouble( + double threshold, const VectorValues& delta, + const ISAM2::sharedClique& clique, KeySet* relinKeys) { // Check the current clique for relinearization bool relinearize = false; - for(Key var: *clique->conditional()) { + for (Key var : *clique->conditional()) { double maxDelta = delta[var].lpNorm(); - if(maxDelta >= threshold) { - relinKeys.insert(var); + if (maxDelta >= threshold) { + relinKeys->insert(var); relinearize = true; } } // If this node was relinearized, also check its children - if(relinearize) { - for(const ISAM2Clique::shared_ptr& child: clique->children) { - CheckRelinearizationRecursiveDouble(relinKeys, threshold, delta, child); + if (relinearize) { + for (const ISAM2::sharedClique& child : clique->children) { + CheckRelinearizationRecursiveDouble(threshold, delta, child, relinKeys); } } } /* ************************************************************************* */ -void CheckRelinearizationRecursiveMap(KeySet& relinKeys, const FastMap& thresholds, - const VectorValues& delta, - const ISAM2Clique::shared_ptr& clique) -{ +static void CheckRelinearizationRecursiveMap( + const FastMap& thresholds, const VectorValues& delta, + const ISAM2::sharedClique& clique, KeySet* relinKeys) { // Check the current clique for relinearization bool relinearize = false; - for(Key var: *clique->conditional()) { + for (Key var : *clique->conditional()) { // Find the threshold for this variable type const Vector& threshold = thresholds.find(Symbol(var).chr())->second; const Vector& deltaVar = delta[var]; // Verify the threshold vector matches the actual variable size - if(threshold.rows() != deltaVar.rows()) - throw std::invalid_argument("Relinearization threshold vector dimensionality for '" + std::string(1, Symbol(var).chr()) + "' passed into iSAM2 parameters does not match actual variable dimensionality."); + if (threshold.rows() != deltaVar.rows()) + throw std::invalid_argument( + "Relinearization threshold vector dimensionality for '" + + std::string(1, Symbol(var).chr()) + + "' passed into iSAM2 parameters does not match actual variable " + "dimensionality."); // Check for relinearization - if((deltaVar.array().abs() > threshold.array()).any()) { - relinKeys.insert(var); + if ((deltaVar.array().abs() > threshold.array()).any()) { + relinKeys->insert(var); relinearize = true; } } // If this node was relinearized, also check its children - if(relinearize) { - for(const ISAM2Clique::shared_ptr& child: clique->children) { - CheckRelinearizationRecursiveMap(relinKeys, thresholds, delta, child); + if (relinearize) { + for (const ISAM2::sharedClique& child : clique->children) { + CheckRelinearizationRecursiveMap(thresholds, delta, child, relinKeys); } } } /* ************************************************************************* */ -KeySet ISAM2::Impl::CheckRelinearizationPartial(const FastVector& roots, - const VectorValues& delta, - const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) -{ +KeySet ISAM2::Impl::CheckRelinearizationPartial( + const FastVector& roots, const VectorValues& delta, + const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) { KeySet relinKeys; - for(const ISAM2::sharedClique& root: roots) { - if(relinearizeThreshold.type() == typeid(double)) - CheckRelinearizationRecursiveDouble(relinKeys, boost::get(relinearizeThreshold), delta, root); - else if(relinearizeThreshold.type() == typeid(FastMap)) - CheckRelinearizationRecursiveMap(relinKeys, boost::get >(relinearizeThreshold), delta, root); + for (const ISAM2::sharedClique& root : roots) { + if (relinearizeThreshold.type() == typeid(double)) + CheckRelinearizationRecursiveDouble( + boost::get(relinearizeThreshold), delta, root, &relinKeys); + else if (relinearizeThreshold.type() == typeid(FastMap)) + CheckRelinearizationRecursiveMap( + boost::get >(relinearizeThreshold), delta, root, + &relinKeys); } return relinKeys; } /* ************************************************************************* */ -void ISAM2::Impl::FindAll(ISAM2Clique::shared_ptr clique, KeySet& keys, const KeySet& markedMask) -{ +void ISAM2::Impl::FindAll(ISAM2::sharedClique clique, KeySet& keys, + const KeySet& markedMask) { static const bool debug = false; // does the separator contain any of the variables? bool found = false; - for(Key key: clique->conditional()->parents()) { + for (Key key : clique->conditional()->parents()) { if (markedMask.exists(key)) { found = true; break; @@ -215,19 +224,22 @@ void ISAM2::Impl::FindAll(ISAM2Clique::shared_ptr clique, KeySet& keys, const Ke } if (found) { // then add this clique - keys.insert(clique->conditional()->beginFrontals(), clique->conditional()->endFrontals()); - if(debug) clique->print("Key(s) marked in clique "); - if(debug) cout << "so marking key " << clique->conditional()->front() << endl; + keys.insert(clique->conditional()->beginFrontals(), + clique->conditional()->endFrontals()); + if (debug) clique->print("Key(s) marked in clique "); + if (debug) + cout << "so marking key " << clique->conditional()->front() << endl; } - for(const ISAM2Clique::shared_ptr& child: clique->children) { + for (const ISAM2::sharedClique& child : clique->children) { FindAll(child, keys, markedMask); } } /* ************************************************************************* */ -void ISAM2::Impl::ExpmapMasked(Values& values, const VectorValues& delta, - const KeySet& mask, boost::optional invalidateIfDebug, const KeyFormatter& keyFormatter) -{ +void ISAM2::Impl::ExpmapMasked(const VectorValues& delta, const KeySet& mask, + Values* values, + boost::optional invalidateIfDebug, + const KeyFormatter& keyFormatter) { // If debugging, invalidate if requested, otherwise do not invalidate. // Invalidating means setting expmapped entries to Inf, to trigger assertions // if we try to re-use them. @@ -235,65 +247,70 @@ void ISAM2::Impl::ExpmapMasked(Values& values, const VectorValues& delta, invalidateIfDebug = boost::none; #endif - assert(values.size() == delta.size()); + assert(values->size() == delta.size()); Values::iterator key_value; VectorValues::const_iterator key_delta; #ifdef GTSAM_USE_TBB - for(key_value = values.begin(); key_value != values.end(); ++key_value) - { + for (key_value = values->begin(); key_value != values->end(); ++key_value) { key_delta = delta.find(key_value->key); #else - for(key_value = values.begin(), key_delta = delta.begin(); key_value != values.end(); ++key_value, ++key_delta) - { + for (key_value = values->begin(), key_delta = delta.begin(); + key_value != values->end(); ++key_value, ++key_delta) { assert(key_value->key == key_delta->first); #endif Key var = key_value->key; - assert(delta[var].size() == (int)key_value->value.dim()); + assert(static_cast(delta[var].size()) == key_value->value.dim()); assert(delta[var].allFinite()); - if(mask.exists(var)) { + if (mask.exists(var)) { Value* retracted = key_value->value.retract_(delta[var]); key_value->value = *retracted; retracted->deallocate_(); - if(invalidateIfDebug) - (*invalidateIfDebug)[var].operator=(Vector::Constant(delta[var].rows(), numeric_limits::infinity())); // Strange syntax to work with clang++ (bug in clang?) + if (invalidateIfDebug) + (*invalidateIfDebug)[var].operator=(Vector::Constant( + delta[var].rows(), + numeric_limits::infinity())); // Strange syntax to work + // with clang++ (bug in + // clang?) } } } /* ************************************************************************* */ namespace internal { -inline static void optimizeInPlace(const boost::shared_ptr& clique, VectorValues& result) { +inline static void optimizeInPlace(const ISAM2::sharedClique& clique, + VectorValues* result) { // parents are assumed to already be solved and available in result - result.update(clique->conditional()->solve(result)); + result->update(clique->conditional()->solve(*result)); // starting from the root, call optimize on each conditional - for(const boost::shared_ptr& child: clique->children) + for (const ISAM2::sharedClique& child : clique->children) optimizeInPlace(child, result); } -} +} // namespace internal /* ************************************************************************* */ -size_t ISAM2::Impl::UpdateGaussNewtonDelta(const FastVector& roots, - const KeySet& replacedKeys, VectorValues& delta, double wildfireThreshold) { - +size_t ISAM2::Impl::UpdateGaussNewtonDelta( + const FastVector& roots, const KeySet& replacedKeys, + double wildfireThreshold, VectorValues* delta) { size_t lastBacksubVariableCount; if (wildfireThreshold <= 0.0) { // Threshold is zero or less, so do a full recalculation - for(const ISAM2::sharedClique& root: roots) + for (const ISAM2::sharedClique& root : roots) internal::optimizeInPlace(root, delta); - lastBacksubVariableCount = delta.size(); + lastBacksubVariableCount = delta->size(); } else { // Optimize with wildfire lastBacksubVariableCount = 0; - for(const ISAM2::sharedClique& root: roots) - lastBacksubVariableCount += optimizeWildfireNonRecursive( - root, wildfireThreshold, replacedKeys, delta); // modifies delta + for (const ISAM2::sharedClique& root : roots) + lastBacksubVariableCount += optimizeWildfire( + root, wildfireThreshold, replacedKeys, delta); // modifies delta #if !defined(NDEBUG) && defined(GTSAM_EXTRA_CONSISTENCY_CHECKS) - for (VectorValues::const_iterator key_delta = delta.begin(); key_delta != delta.end(); ++key_delta) { - assert(delta[key_delta->first].allFinite()); + for (VectorValues::const_iterator key_delta = delta->begin(); + key_delta != delta->end(); ++key_delta) { + assert((*delta)[key_delta->first].allFinite()); } #endif } @@ -303,69 +320,77 @@ size_t ISAM2::Impl::UpdateGaussNewtonDelta(const FastVector /* ************************************************************************* */ namespace internal { -void updateRgProd(const boost::shared_ptr& clique, const KeySet& replacedKeys, - const VectorValues& grad, VectorValues& RgProd, size_t& varsUpdated) { - +void updateRgProd(const ISAM2::sharedClique& clique, const KeySet& replacedKeys, + const VectorValues& grad, VectorValues* RgProd, + size_t* varsUpdated) { // Check if any frontal or separator keys were recalculated, if so, we need // update deltas and recurse to children, but if not, we do not need to // recurse further because of the running separator property. bool anyReplaced = false; - for(Key j: *clique->conditional()) { - if(replacedKeys.exists(j)) { + for (Key j : *clique->conditional()) { + if (replacedKeys.exists(j)) { anyReplaced = true; break; } } - if(anyReplaced) { + if (anyReplaced) { // Update the current variable // Get VectorValues slice corresponding to current variables - Vector gR = grad.vector(FastVector(clique->conditional()->beginFrontals(), clique->conditional()->endFrontals())); - Vector gS = grad.vector(FastVector(clique->conditional()->beginParents(), clique->conditional()->endParents())); + Vector gR = + grad.vector(FastVector(clique->conditional()->beginFrontals(), + clique->conditional()->endFrontals())); + Vector gS = + grad.vector(FastVector(clique->conditional()->beginParents(), + clique->conditional()->endParents())); // Compute R*g and S*g for this clique - Vector RSgProd = clique->conditional()->get_R() * gR + clique->conditional()->get_S() * gS; + Vector RSgProd = clique->conditional()->get_R() * gR + + clique->conditional()->get_S() * gS; // Write into RgProd vector DenseIndex vectorPosition = 0; - for(Key frontal: clique->conditional()->frontals()) { - Vector& RgProdValue = RgProd[frontal]; + for (Key frontal : clique->conditional()->frontals()) { + Vector& RgProdValue = (*RgProd)[frontal]; RgProdValue = RSgProd.segment(vectorPosition, RgProdValue.size()); vectorPosition += RgProdValue.size(); } - // Now solve the part of the Newton's method point for this clique (back-substitution) - //(*clique)->solveInPlace(deltaNewton); + // Now solve the part of the Newton's method point for this clique + // (back-substitution) + // (*clique)->solveInPlace(deltaNewton); - varsUpdated += clique->conditional()->nrFrontals(); + *varsUpdated += clique->conditional()->nrFrontals(); // Recurse to children - for(const ISAM2Clique::shared_ptr& child: clique->children) { - updateRgProd(child, replacedKeys, grad, RgProd, varsUpdated); } + for (const ISAM2::sharedClique& child : clique->children) { + updateRgProd(child, replacedKeys, grad, RgProd, varsUpdated); + } } } -} +} // namespace internal /* ************************************************************************* */ -size_t ISAM2::Impl::UpdateRgProd(const ISAM2::Roots& roots, const KeySet& replacedKeys, - const VectorValues& gradAtZero, VectorValues& RgProd) { - +size_t ISAM2::Impl::UpdateRgProd(const ISAM2::Roots& roots, + const KeySet& replacedKeys, + const VectorValues& gradAtZero, + VectorValues* RgProd) { // Update variables size_t varsUpdated = 0; - for(const ISAM2::sharedClique& root: roots) { - internal::updateRgProd(root, replacedKeys, gradAtZero, RgProd, varsUpdated); + for (const ISAM2::sharedClique& root : roots) { + internal::updateRgProd(root, replacedKeys, gradAtZero, RgProd, + &varsUpdated); } return varsUpdated; } - + /* ************************************************************************* */ VectorValues ISAM2::Impl::ComputeGradientSearch(const VectorValues& gradAtZero, - const VectorValues& RgProd) -{ + const VectorValues& RgProd) { // Compute gradient squared-magnitude const double gradientSqNorm = gradAtZero.dot(gradAtZero); - + // Compute minimizing step size double RgNormSq = RgProd.vector().squaredNorm(); double step = -gradientSqNorm / RgNormSq; @@ -374,4 +399,4 @@ VectorValues ISAM2::Impl::ComputeGradientSearch(const VectorValues& gradAtZero, return step * gradAtZero; } -} +} // namespace gtsam diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index 161932344..0e05ab453 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -108,18 +108,17 @@ struct GTSAM_EXPORT ISAM2::Impl { /** * Apply expmap to the given values, but only for indices appearing in * \c markedRelinMask. Values are expmapped in-place. - * \param [in, out] values The value to expmap in-place * \param delta The linear delta with which to expmap - * \param ordering The ordering * \param mask Mask on linear indices, only \c true entries are expmapped + * \param [in, out] values The value to expmap in-place * \param invalidateIfDebug If this is true, *and* NDEBUG is not defined, * expmapped deltas will be set to an invalid value (infinity) to catch bugs * where we might expmap something twice, or expmap it but then not * recalculate its delta. * @param keyFormatter Formatter for printing nonlinear keys during debugging */ - static void ExpmapMasked(Values& values, const VectorValues& delta, - const KeySet& mask, + static void ExpmapMasked( + const VectorValues& delta, const KeySet& mask, Values* values, boost::optional invalidateIfDebug = boost::none, const KeyFormatter& keyFormatter = DefaultKeyFormatter); @@ -127,14 +126,14 @@ struct GTSAM_EXPORT ISAM2::Impl { * Update the Newton's method step point, using wildfire */ static size_t UpdateGaussNewtonDelta(const FastVector& roots, - const KeySet& replacedKeys, VectorValues& delta, double wildfireThreshold); + const KeySet& replacedKeys, double wildfireThreshold, VectorValues* delta); /** * Update the RgProd (R*g) incrementally taking into account which variables * have been recalculated in \c replacedKeys. Only used in Dogleg. */ static size_t UpdateRgProd(const ISAM2::Roots& roots, const KeySet& replacedKeys, - const VectorValues& gradAtZero, VectorValues& RgProd); + const VectorValues& gradAtZero, VectorValues* RgProd); /** * Compute the gradient-search point. Only used in Dogleg. diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h index fecefd2a5..fb63dc428 100644 --- a/gtsam/nonlinear/ISAM2-inl.h +++ b/gtsam/nonlinear/ISAM2-inl.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -11,301 +11,24 @@ /** * @file ISAM2-inl.h - * @brief + * @brief * @author Richard Roberts * @date Mar 16, 2012 */ - #pragma once -#include -#include -#include +#include namespace gtsam { /* ************************************************************************* */ -template +template VALUE ISAM2::calculateEstimate(Key key) const { const Vector& delta = getDelta()[key]; return traits::Retract(theta_.at(key), delta); } /* ************************************************************************* */ -namespace internal { -template -void optimizeWildfire(const boost::shared_ptr& clique, double threshold, - KeySet& changed, const KeySet& replaced, VectorValues& delta, size_t& count) -{ - // if none of the variables in this clique (frontal and separator!) changed - // significantly, then by the running intersection property, none of the - // cliques in the children need to be processed - - // Are any clique variables part of the tree that has been redone? - bool cliqueReplaced = replaced.exists((*clique)->frontals().front()); -#if !defined(NDEBUG) && defined(GTSAM_EXTRA_CONSISTENCY_CHECKS) - for(Key frontal: clique->conditional()->frontals()) { - assert(cliqueReplaced == replaced.exists(frontal)); - } -#endif - - // If not redone, then has one of the separator variables changed significantly? - bool recalculate = cliqueReplaced; - if(!recalculate) { - for(Key parent: clique->conditional()->parents()) { - if(changed.exists(parent)) { - recalculate = true; - break; - } - } - } - - // Solve clique if it was replaced, or if any parents were changed above the - // threshold or themselves replaced. - if(recalculate) { - - // Temporary copy of the original values, to check how much they change - FastVector originalValues(clique->conditional()->nrFrontals()); - GaussianConditional::const_iterator it; - for(it = clique->conditional()->beginFrontals(); it!=clique->conditional()->endFrontals(); it++) { - originalValues[it - clique->conditional()->beginFrontals()] = delta[*it]; - } - - // Back-substitute - delta.update(clique->conditional()->solve(delta)); - count += clique->conditional()->nrFrontals(); - - // Whether the values changed above a threshold, or always true if the - // clique was replaced. - bool valuesChanged = cliqueReplaced; - for(it = clique->conditional()->beginFrontals(); it != clique->conditional()->endFrontals(); it++) { - if(!valuesChanged) { - const Vector& oldValue(originalValues[it - clique->conditional()->beginFrontals()]); - const Vector& newValue(delta[*it]); - if((oldValue - newValue).lpNorm() >= threshold) { - valuesChanged = true; - break; - } - } else - break; - } - - // If the values were above the threshold or this clique was replaced - if(valuesChanged) { - // Set changed flag for each frontal variable and leave the new values - for(Key frontal: clique->conditional()->frontals()) { - changed.insert(frontal); - } - } else { - // Replace with the old values - for(it = clique->conditional()->beginFrontals(); it != clique->conditional()->endFrontals(); it++) { - delta[*it] = originalValues[it - clique->conditional()->beginFrontals()]; - } - } - - // Recurse to children - for(const typename CLIQUE::shared_ptr& child: clique->children) { - optimizeWildfire(child, threshold, changed, replaced, delta, count); - } - } -} - -template -bool optimizeWildfireNode(const boost::shared_ptr& clique, double threshold, - KeySet& changed, const KeySet& replaced, VectorValues& delta, size_t& count) -{ - // if none of the variables in this clique (frontal and separator!) changed - // significantly, then by the running intersection property, none of the - // cliques in the children need to be processed - - // Are any clique variables part of the tree that has been redone? - bool cliqueReplaced = replaced.exists(clique->conditional()->frontals().front()); -#if !defined(NDEBUG) && defined(GTSAM_EXTRA_CONSISTENCY_CHECKS) - for(Key frontal: clique->conditional()->frontals()) { - assert(cliqueReplaced == replaced.exists(frontal)); - } -#endif - - // If not redone, then has one of the separator variables changed significantly? - bool recalculate = cliqueReplaced; - if(!recalculate) { - for(Key parent: clique->conditional()->parents()) { - if(changed.exists(parent)) { - recalculate = true; - break; - } - } - } - - // Solve clique if it was replaced, or if any parents were changed above the - // threshold or themselves replaced. - // TODO(gareth): This code shares a lot of logic w/ linearAlgorithms-inst, potentially refactor - if(recalculate) - { - // Temporary copy of the original values, to check how much they change - FastVector originalValues(clique->conditional()->nrFrontals()); - GaussianConditional::const_iterator it; - for(it = clique->conditional()->beginFrontals(); it != clique->conditional()->endFrontals(); it++) { - originalValues[it - clique->conditional()->beginFrontals()] = delta[*it]; - } - - // Back-substitute - special version stores solution pointers in cliques for fast access. - { - // Create solution part pointers if necessary and possible - necessary if solnPointers_ is - // empty, and possible if either we're a root, or we have a parent with valid solnPointers_. - boost::shared_ptr parent = clique->parent_.lock(); - if(clique->solnPointers_.empty() && (clique->isRoot() || !parent->solnPointers_.empty())) - { - for(Key key: clique->conditional()->frontals()) - clique->solnPointers_.insert(std::make_pair(key, delta.find(key))); - for(Key key: clique->conditional()->parents()) - clique->solnPointers_.insert(std::make_pair(key, parent->solnPointers_.at(key))); - } - - // See if we can use solution part pointers - we can if they either already existed or were - // created above. - if(!clique->solnPointers_.empty()) - { - GaussianConditional& c = *clique->conditional(); - // Solve matrix - Vector xS; - { - // Count dimensions of vector - DenseIndex dim = 0; - FastVector parentPointers; - parentPointers.reserve(clique->conditional()->nrParents()); - for(Key parent: clique->conditional()->parents()) { - parentPointers.push_back(clique->solnPointers_.at(parent)); - dim += parentPointers.back()->second.size(); - } - - // Fill parent vector - xS.resize(dim); - DenseIndex vectorPos = 0; - for(const VectorValues::const_iterator& parentPointer: parentPointers) { - const Vector& parentVector = parentPointer->second; - xS.block(vectorPos,0,parentVector.size(),1) = parentVector.block(0,0,parentVector.size(),1); - vectorPos += parentVector.size(); - } - } - - // NOTE(gareth): We can no longer write: xS = b - S * xS - // This is because Eigen (as of 3.3) no longer evaluates S * xS into - // a temporary, and the operation trashes valus in xS. - // See: http://eigen.tuxfamily.org/index.php?title=3.3 - const Vector rhs = c.getb() - c.get_S() * xS; - const Vector solution = c.get_R().triangularView().solve(rhs); - - // Check for indeterminant solution - if(solution.hasNaN()) throw IndeterminantLinearSystemException(c.keys().front()); - - // Insert solution into a VectorValues - DenseIndex vectorPosition = 0; - for(GaussianConditional::const_iterator frontal = c.beginFrontals(); frontal != c.endFrontals(); ++frontal) { - clique->solnPointers_.at(*frontal)->second = solution.segment(vectorPosition, c.getDim(frontal)); - vectorPosition += c.getDim(frontal); - } - } - else - { - // Just call plain solve because we couldn't use solution pointers. - delta.update(clique->conditional()->solve(delta)); - } - } - count += clique->conditional()->nrFrontals(); - - // Whether the values changed above a threshold, or always true if the - // clique was replaced. - bool valuesChanged = cliqueReplaced; - for(it = clique->conditional()->beginFrontals(); it != clique->conditional()->endFrontals(); it++) { - if(!valuesChanged) { - const Vector& oldValue(originalValues[it - clique->conditional()->beginFrontals()]); - const Vector& newValue(delta[*it]); - if((oldValue - newValue).lpNorm() >= threshold) { - valuesChanged = true; - break; - } - } else - break; - } - - // If the values were above the threshold or this clique was replaced - if(valuesChanged) { - // Set changed flag for each frontal variable and leave the new values - for(Key frontal: clique->conditional()->frontals()) { - changed.insert(frontal); - } - } else { - // Replace with the old values - for(it = clique->conditional()->beginFrontals(); it != clique->conditional()->endFrontals(); it++) { - delta[*it] = originalValues[it - clique->conditional()->beginFrontals()]; - } - } - } - - return recalculate; -} - -} // namespace internal - -/* ************************************************************************* */ -template -size_t optimizeWildfire(const boost::shared_ptr& root, double threshold, const KeySet& keys, VectorValues& delta) { - KeySet changed; - int count = 0; - // starting from the root, call optimize on each conditional - if(root) - internal::optimizeWildfire(root, threshold, changed, keys, delta, count); - return count; -} - -/* ************************************************************************* */ -template -size_t optimizeWildfireNonRecursive(const boost::shared_ptr& root, double threshold, const KeySet& keys, VectorValues& delta) -{ - KeySet changed; - size_t count = 0; - - if (root) { - std::stack > travStack; - travStack.push(root); - boost::shared_ptr currentNode = root; - while (!travStack.empty()) { - currentNode = travStack.top(); - travStack.pop(); - bool recalculate = internal::optimizeWildfireNode(currentNode, threshold, changed, keys, delta, count); - if (recalculate) { - for(const typename CLIQUE::shared_ptr& child: currentNode->children) { - travStack.push(child); - } - } - } - } - - return count; -} - -/* ************************************************************************* */ -template -void nnz_internal(const boost::shared_ptr& clique, int& result) { - int dimR = (int)clique->conditional()->rows(); - int dimSep = (int)clique->conditional()->get_S().cols(); - result += ((dimR+1)*dimR)/2 + dimSep*dimR; - // traverse the children - for(const typename CLIQUE::shared_ptr& child: clique->children) { - nnz_internal(child, result); - } -} - -/* ************************************************************************* */ -template -int calculate_nnz(const boost::shared_ptr& clique) { - int result = 0; - // starting from the root, add up entries of frontal and conditional matrices of each conditional - nnz_internal(clique, result); - return result; -} - -} +} // namespace gtsam diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index d4df01a68..bdd6b42bb 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -11,31 +11,41 @@ /** * @file ISAM2-inl.h - * @brief Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization. + * @brief Incremental update functionality (ISAM2) for BayesTree, with fluid + * relinearization. * @author Michael Kaess, Richard Roberts */ -#include // for operator += +#include + +#include +#include +#include +#include + +#include // for operator += using namespace boost::assign; +#include #include #include -#include -namespace br { using namespace boost::range; using namespace boost::adaptors; } +namespace br { +using namespace boost::range; +using namespace boost::adaptors; +} // namespace br -#include #include +#include #include #include -#include // We need the inst file because we'll make a special JT templated on ISAM2 -#include -#include -#include +#include // We need the inst file because we'll make a special JT templated on ISAM2 #include +#include +#include +#include -#include #include -#include #include +#include using namespace std; @@ -49,11 +59,10 @@ static const bool disableReordering = false; static const double batchThreshold = 0.65; /* ************************************************************************* */ -// Special BayesTree class that uses ISAM2 cliques - this is the result of reeliminating ISAM2 -// subtrees. -class ISAM2BayesTree : public ISAM2::Base -{ -public: +// Special BayesTree class that uses ISAM2 cliques - this is the result of +// reeliminating ISAM2 subtrees. +class ISAM2BayesTree : public ISAM2::Base { + public: typedef ISAM2::Base Base; typedef ISAM2BayesTree This; typedef boost::shared_ptr shared_ptr; @@ -62,43 +71,58 @@ public: }; /* ************************************************************************* */ -// Special JunctionTree class that produces ISAM2 BayesTree cliques, used for reeliminating ISAM2 -// subtrees. -class ISAM2JunctionTree : public JunctionTree -{ -public: +// Special JunctionTree class that produces ISAM2 BayesTree cliques, used for +// reeliminating ISAM2 subtrees. +class ISAM2JunctionTree + : public JunctionTree { + public: typedef JunctionTree Base; typedef ISAM2JunctionTree This; typedef boost::shared_ptr shared_ptr; - ISAM2JunctionTree(const GaussianEliminationTree& eliminationTree) : - Base(eliminationTree) {} + ISAM2JunctionTree(const GaussianEliminationTree& eliminationTree) + : Base(eliminationTree) {} }; /* ************************************************************************* */ -std::string ISAM2DoglegParams::adaptationModeTranslator(const DoglegOptimizerImpl::TrustRegionAdaptationMode& adaptationMode) const { +std::string ISAM2DoglegParams::adaptationModeTranslator( + const DoglegOptimizerImpl::TrustRegionAdaptationMode& adaptationMode) + const { std::string s; switch (adaptationMode) { - case DoglegOptimizerImpl::SEARCH_EACH_ITERATION: s = "SEARCH_EACH_ITERATION"; break; - case DoglegOptimizerImpl::ONE_STEP_PER_ITERATION: s = "ONE_STEP_PER_ITERATION"; break; - default: s = "UNDEFINED"; break; + case DoglegOptimizerImpl::SEARCH_EACH_ITERATION: + s = "SEARCH_EACH_ITERATION"; + break; + case DoglegOptimizerImpl::ONE_STEP_PER_ITERATION: + s = "ONE_STEP_PER_ITERATION"; + break; + default: + s = "UNDEFINED"; + break; } return s; } /* ************************************************************************* */ -DoglegOptimizerImpl::TrustRegionAdaptationMode ISAM2DoglegParams::adaptationModeTranslator(const std::string& adaptationMode) const { - std::string s = adaptationMode; boost::algorithm::to_upper(s); - if (s == "SEARCH_EACH_ITERATION") return DoglegOptimizerImpl::SEARCH_EACH_ITERATION; - if (s == "ONE_STEP_PER_ITERATION") return DoglegOptimizerImpl::ONE_STEP_PER_ITERATION; +DoglegOptimizerImpl::TrustRegionAdaptationMode +ISAM2DoglegParams::adaptationModeTranslator( + const std::string& adaptationMode) const { + std::string s = adaptationMode; + boost::algorithm::to_upper(s); + if (s == "SEARCH_EACH_ITERATION") + return DoglegOptimizerImpl::SEARCH_EACH_ITERATION; + if (s == "ONE_STEP_PER_ITERATION") + return DoglegOptimizerImpl::ONE_STEP_PER_ITERATION; /* default is SEARCH_EACH_ITERATION */ return DoglegOptimizerImpl::SEARCH_EACH_ITERATION; } /* ************************************************************************* */ -ISAM2Params::Factorization ISAM2Params::factorizationTranslator(const std::string& str) { - std::string s = str; boost::algorithm::to_upper(s); +ISAM2Params::Factorization ISAM2Params::factorizationTranslator( + const std::string& str) { + std::string s = str; + boost::algorithm::to_upper(s); if (s == "QR") return ISAM2Params::QR; if (s == "CHOLESKY") return ISAM2Params::CHOLESKY; @@ -107,84 +131,347 @@ ISAM2Params::Factorization ISAM2Params::factorizationTranslator(const std::strin } /* ************************************************************************* */ -std::string ISAM2Params::factorizationTranslator(const ISAM2Params::Factorization& value) { +std::string ISAM2Params::factorizationTranslator( + const ISAM2Params::Factorization& value) { std::string s; switch (value) { - case ISAM2Params::QR: s = "QR"; break; - case ISAM2Params::CHOLESKY: s = "CHOLESKY"; break; - default: s = "UNDEFINED"; break; + case ISAM2Params::QR: + s = "QR"; + break; + case ISAM2Params::CHOLESKY: + s = "CHOLESKY"; + break; + default: + s = "UNDEFINED"; + break; } return s; } /* ************************************************************************* */ -void ISAM2Clique::setEliminationResult(const FactorGraphType::EliminationResult& eliminationResult) -{ +void ISAM2Clique::setEliminationResult( + const FactorGraphType::EliminationResult& eliminationResult) { conditional_ = eliminationResult.first; cachedFactor_ = eliminationResult.second; // Compute gradient contribution gradientContribution_.resize(conditional_->cols() - 1); - // Rewrite -(R * P')'*d as -(d' * R * P')' for computational speed reasons - gradientContribution_ << -conditional_->get_R().transpose() * conditional_->get_d(), - -conditional_->get_S().transpose() * conditional_->get_d(); + // Rewrite -(R * P')'*d as -(d' * R * P')' for computational speed + // reasons + gradientContribution_ << -conditional_->get_R().transpose() * + conditional_->get_d(), + -conditional_->get_S().transpose() * conditional_->get_d(); } /* ************************************************************************* */ bool ISAM2Clique::equals(const This& other, double tol) const { return Base::equals(other) && - ((!cachedFactor_ && !other.cachedFactor_) - || (cachedFactor_ && other.cachedFactor_ - && cachedFactor_->equals(*other.cachedFactor_, tol))); + ((!cachedFactor_ && !other.cachedFactor_) || + (cachedFactor_ && other.cachedFactor_ && + cachedFactor_->equals(*other.cachedFactor_, tol))); } /* ************************************************************************* */ -void ISAM2Clique::print(const std::string& s, const KeyFormatter& formatter) const -{ - Base::print(s,formatter); - if(cachedFactor_) +void ISAM2Clique::print(const std::string& s, + const KeyFormatter& formatter) const { + Base::print(s, formatter); + if (cachedFactor_) cachedFactor_->print(s + "Cached: ", formatter); else std::cout << s << "Cached empty" << std::endl; - if(gradientContribution_.rows() != 0) + if (gradientContribution_.rows() != 0) gtsam::print(gradientContribution_, "Gradient contribution: "); } /* ************************************************************************* */ -ISAM2::ISAM2(const ISAM2Params& params): params_(params), update_count_(0) { - if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) - doglegDelta_ = boost::get(params_.optimizationParams).initialDelta; +bool ISAM2Clique::isDirty(const KeySet& replaced, const KeySet& changed) const { + // if none of the variables in this clique (frontal and separator!) changed + // significantly, then by the running intersection property, none of the + // cliques in the children need to be processed + + // Are any clique variables part of the tree that has been redone? + bool dirty = replaced.exists(conditional_->frontals().front()); +#if !defined(NDEBUG) && defined(GTSAM_EXTRA_CONSISTENCY_CHECKS) + for (Key frontal : conditional_->frontals()) { + assert(dirty == replaced.exists(frontal)); + } +#endif + + // If not, then has one of the separator variables changed significantly? + if (!dirty) { + for (Key parent : conditional_->parents()) { + if (changed.exists(parent)) { + dirty = true; + break; + } + } + } + return dirty; +} +/* ************************************************************************* */ +FastVector ISAM2Clique::copyRelevantValues( + const VectorValues& delta) const { + FastVector values; + values.reserve(conditional_->nrFrontals()); + for (Key frontal : conditional_->frontals()) { + values.push_back(delta[frontal]); + } + return values; +} + +/* ************************************************************************* */ +bool ISAM2Clique::valuesChanged(const KeySet& replaced, + const FastVector& originalValues, + const VectorValues& delta, + double threshold) const { + if (replaced.exists(conditional_->frontals().front())) return true; + size_t i = 0; + for (Key frontal : conditional_->frontals()) { + const Vector diff = originalValues[i++] - delta[frontal]; + if (diff.lpNorm() >= threshold) { + return true; + } + } + return false; +} + +/* ************************************************************************* */ +/// Set changed flag for each frontal variable +void ISAM2Clique::markFrontalsAsChanged(KeySet* changed) const { + for (Key frontal : conditional_->frontals()) { + changed->insert(frontal); + } +} + +/* ************************************************************************* */ +void ISAM2Clique::restoreFromOriginals(const FastVector& originalValues, + VectorValues* delta) const { + size_t i = 0; + for (Key frontal : conditional_->frontals()) { + delta->at(frontal) = originalValues[i++]; + } +} + +/* ************************************************************************* */ +void ISAM2Clique::optimizeWildfire(const KeySet& replaced, double threshold, + KeySet* changed, VectorValues* delta, + size_t* count) const { + if (isDirty(replaced, *changed)) { + // Temporary copy of the original values, to check how much they change + auto originalValues = copyRelevantValues(*delta); + + // Back-substitute + delta->update(conditional_->solve(*delta)); + count += conditional_->nrFrontals(); + + if (valuesChanged(replaced, originalValues, *delta, threshold)) { + markFrontalsAsChanged(changed); + } else { + restoreFromOriginals(originalValues, delta); + } + + // Recurse to children + for (const auto& child : children) { + child->optimizeWildfire(replaced, threshold, changed, delta, count); + } + } +} + +/* ************************************************************************* */ +bool ISAM2Clique::optimizeWildfireNode(const KeySet& replaced, double threshold, + KeySet* changed, VectorValues* delta, + size_t* count) const { + // TODO(gareth): This code shares a lot of logic w/ linearAlgorithms-inst, + // potentially refactor + bool dirty = isDirty(replaced, *changed); + if (dirty) { + // Temporary copy of the original values, to check how much they change + auto originalValues = copyRelevantValues(*delta); + + // Back-substitute - special version stores solution pointers in cliques for + // fast access. + { + // Create solution part pointers if necessary and possible - necessary if + // solnPointers_ is empty, and possible if either we're a root, or we have + // a parent with valid solnPointers_. + ISAM2::sharedClique parent = parent_.lock(); + if (solnPointers_.empty() && + (isRoot() || !parent->solnPointers_.empty())) { + for (Key frontal : conditional_->frontals()) + solnPointers_.emplace(frontal, delta->find(frontal)); + for (Key parentKey : conditional_->parents()) { + assert(parent->solnPointers_.exists(parentKey)); + solnPointers_.emplace(parentKey, parent->solnPointers_.at(parentKey)); + } + } + + // See if we can use solution part pointers - we can if they either + // already existed or were created above. + if (!solnPointers_.empty()) { + GaussianConditional& c = *conditional_; + // Solve matrix + Vector xS; + { + // Count dimensions of vector + DenseIndex dim = 0; + FastVector parentPointers; + parentPointers.reserve(conditional_->nrParents()); + for (Key parent : conditional_->parents()) { + parentPointers.push_back(solnPointers_.at(parent)); + dim += parentPointers.back()->second.size(); + } + + // Fill parent vector + xS.resize(dim); + DenseIndex vectorPos = 0; + for (const VectorValues::const_iterator& parentPointer : + parentPointers) { + const Vector& parentVector = parentPointer->second; + xS.block(vectorPos, 0, parentVector.size(), 1) = + parentVector.block(0, 0, parentVector.size(), 1); + vectorPos += parentVector.size(); + } + } + + // NOTE(gareth): We can no longer write: xS = b - S * xS + // This is because Eigen (as of 3.3) no longer evaluates S * xS into + // a temporary, and the operation trashes valus in xS. + // See: http://eigen.tuxfamily.org/index.php?title=3.3 + const Vector rhs = c.getb() - c.get_S() * xS; + const Vector solution = + c.get_R().triangularView().solve(rhs); + + // Check for indeterminant solution + if (solution.hasNaN()) + throw IndeterminantLinearSystemException(c.keys().front()); + + // Insert solution into a VectorValues + DenseIndex vectorPosition = 0; + for (GaussianConditional::const_iterator frontal = c.beginFrontals(); + frontal != c.endFrontals(); ++frontal) { + solnPointers_.at(*frontal)->second = + solution.segment(vectorPosition, c.getDim(frontal)); + vectorPosition += c.getDim(frontal); + } + } else { + // Just call plain solve because we couldn't use solution pointers. + delta->update(conditional_->solve(*delta)); + } + } + count += conditional_->nrFrontals(); + + if (valuesChanged(replaced, originalValues, *delta, threshold)) { + markFrontalsAsChanged(changed); + } else { + restoreFromOriginals(originalValues, delta); + } + } + + return dirty; +} + +/* ************************************************************************* */ +void ISAM2Clique::nnz_internal(size_t* result) const { + size_t dimR = conditional_->rows(); + size_t dimSep = conditional_->get_S().cols(); + *result += ((dimR + 1) * dimR) / 2 + dimSep * dimR; + // traverse the children + for (const auto& child : children) { + child->nnz_internal(result); + } +} + +/* ************************************************************************* */ +size_t ISAM2Clique::calculate_nnz() const { + size_t result = 0; + nnz_internal(&result); + return result; +} + +/* ************************************************************************* */ +size_t optimizeWildfire(const ISAM2::sharedClique& root, double threshold, + const KeySet& keys, VectorValues* delta) { + KeySet changed; + size_t count = 0; + // starting from the root, call optimize on each conditional + if (root) root->optimizeWildfire(keys, threshold, &changed, delta, &count); + return count; +} + +/* ************************************************************************* */ +// This version is non-recursive version, but seems to have +// a bug, as was diagnosed with ISAM2Example_SmartFactor. Disabled for now. +size_t optimizeWildfireNonRecursive(const ISAM2::sharedClique& root, + double threshold, const KeySet& keys, + VectorValues* delta) { + KeySet changed; + size_t count = 0; + + if (root) { + std::stack travStack; + travStack.push(root); + ISAM2::sharedClique currentNode = root; + while (!travStack.empty()) { + currentNode = travStack.top(); + travStack.pop(); + bool dirty = currentNode->optimizeWildfireNode(keys, threshold, &changed, + delta, &count); + if (dirty) { + for (const auto& child : currentNode->children) { + travStack.push(child); + } + } + } + } + + return count; +} + +/* ************************************************************************* */ +ISAM2::ISAM2(const ISAM2Params& params) : params_(params), update_count_(0) { + if (params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) + doglegDelta_ = + boost::get(params_.optimizationParams).initialDelta; } /* ************************************************************************* */ ISAM2::ISAM2() : update_count_(0) { - if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) - doglegDelta_ = boost::get(params_.optimizationParams).initialDelta; + if (params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) + doglegDelta_ = + boost::get(params_.optimizationParams).initialDelta; } /* ************************************************************************* */ bool ISAM2::equals(const ISAM2& other, double tol) const { - return Base::equals(other, tol) - && theta_.equals(other.theta_, tol) && variableIndex_.equals(other.variableIndex_, tol) - && nonlinearFactors_.equals(other.nonlinearFactors_, tol) - && fixedVariables_ == other.fixedVariables_; + return Base::equals(other, tol) && theta_.equals(other.theta_, tol) && + variableIndex_.equals(other.variableIndex_, tol) && + nonlinearFactors_.equals(other.nonlinearFactors_, tol) && + fixedVariables_ == other.fixedVariables_; } /* ************************************************************************* */ KeySet ISAM2::getAffectedFactors(const KeyList& keys) const { static const bool debug = false; - if(debug) cout << "Getting affected factors for "; - if(debug) { for(const Key key: keys) { cout << key << " "; } } - if(debug) cout << endl; + if (debug) cout << "Getting affected factors for "; + if (debug) { + for (const Key key : keys) { + cout << key << " "; + } + } + if (debug) cout << endl; NonlinearFactorGraph allAffected; KeySet indices; - for(const Key key: keys) { + for (const Key key : keys) { const VariableIndex::Factors& factors(variableIndex_[key]); indices.insert(factors.begin(), factors.end()); } - if(debug) cout << "Affected factors are: "; - if(debug) { for(const size_t index: indices) { cout << index << " "; } } - if(debug) cout << endl; + if (debug) cout << "Affected factors are: "; + if (debug) { + for (const size_t index : indices) { + cout << index << " "; + } + } + if (debug) cout << endl; return indices; } @@ -192,9 +479,8 @@ KeySet ISAM2::getAffectedFactors(const KeyList& keys) const { // retrieve all factors that ONLY contain the affected variables // (note that the remaining stuff is summarized in the cached factors) -GaussianFactorGraph::shared_ptr -ISAM2::relinearizeAffectedFactors(const FastList& affectedKeys, const KeySet& relinKeys) const -{ +GaussianFactorGraph::shared_ptr ISAM2::relinearizeAffectedFactors( + const FastList& affectedKeys, const KeySet& relinKeys) const { gttic(getAffectedFactors); KeySet candidates = getAffectedFactors(affectedKeys); gttoc(getAffectedFactors); @@ -208,29 +494,31 @@ ISAM2::relinearizeAffectedFactors(const FastList& affectedKeys, const KeySe gttoc(affectedKeysSet); gttic(check_candidates_and_linearize); - GaussianFactorGraph::shared_ptr linearized = boost::make_shared(); - for(Key idx: candidates) { + GaussianFactorGraph::shared_ptr linearized = + boost::make_shared(); + for (Key idx : candidates) { bool inside = true; bool useCachedLinear = params_.cacheLinearizedFactors; - for(Key key: nonlinearFactors_[idx]->keys()) { - if(affectedKeysSet.find(key) == affectedKeysSet.end()) { + for (Key key : nonlinearFactors_[idx]->keys()) { + if (affectedKeysSet.find(key) == affectedKeysSet.end()) { inside = false; break; } - if(useCachedLinear && relinKeys.find(key) != relinKeys.end()) + if (useCachedLinear && relinKeys.find(key) != relinKeys.end()) useCachedLinear = false; } - if(inside) { - if(useCachedLinear) { + if (inside) { + if (useCachedLinear) { #ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS assert(linearFactors_[idx]); assert(linearFactors_[idx]->keys() == nonlinearFactors_[idx]->keys()); #endif linearized->push_back(linearFactors_[idx]); } else { - GaussianFactor::shared_ptr linearFactor = nonlinearFactors_[idx]->linearize(theta_); + GaussianFactor::shared_ptr linearFactor = + nonlinearFactors_[idx]->linearize(theta_); linearized->push_back(linearFactor); - if(params_.cacheLinearizedFactors) { + if (params_.cacheLinearizedFactors) { #ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS assert(linearFactors_[idx]->keys() == linearFactor->keys()); #endif @@ -245,12 +533,13 @@ ISAM2::relinearizeAffectedFactors(const FastList& affectedKeys, const KeySe } /* ************************************************************************* */ -// find intermediate (linearized) factors from cache that are passed into the affected area +// find intermediate (linearized) factors from cache that are passed into the +// affected area GaussianFactorGraph ISAM2::getCachedBoundaryFactors(const Cliques& orphans) { GaussianFactorGraph cachedBoundary; - for(sharedClique orphan: orphans) { + for (sharedClique orphan : orphans) { // retrieve the cached factor and add to boundary cachedBoundary.push_back(orphan->cachedFactor()); } @@ -259,27 +548,27 @@ GaussianFactorGraph ISAM2::getCachedBoundaryFactors(const Cliques& orphans) { } /* ************************************************************************* */ -boost::shared_ptr ISAM2::recalculate(const KeySet& markedKeys, const KeySet& relinKeys, - const vector& observedKeys, - const KeySet& unusedIndices, - const boost::optional >& constrainKeys, - ISAM2Result& result) -{ - // TODO: new factors are linearized twice, the newFactors passed in are not used. +boost::shared_ptr ISAM2::recalculate( + const KeySet& markedKeys, const KeySet& relinKeys, + const vector& observedKeys, const KeySet& unusedIndices, + const boost::optional >& constrainKeys, + ISAM2Result& result) { + // TODO(dellaert): new factors are linearized twice, + // the newFactors passed in are not used. const bool debug = ISDEBUG("ISAM2 recalculate"); // Input: BayesTree(this), newFactors -//#define PRINT_STATS // figures for paper, disable for timing +// figures for paper, disable for timing #ifdef PRINT_STATS static int counter = 0; int maxClique = 0; double avgClique = 0; int numCliques = 0; int nnzR = 0; - if (counter>0) { // cannot call on empty tree - GaussianISAM2_P::CliqueData cdata = this->getCliqueData(); + if (counter > 0) { // cannot call on empty tree + GaussianISAM2_P::CliqueData cdata = this->getCliqueData(); GaussianISAM2_P::CliqueStats cstats = cdata.getStats(); maxClique = cstats.maxCONDITIONALSize; avgClique = cstats.avgCONDITIONALSize; @@ -289,81 +578,89 @@ boost::shared_ptr ISAM2::recalculate(const KeySet& markedKeys, const Ke counter++; #endif - if(debug) { + if (debug) { cout << "markedKeys: "; - for(const Key key: markedKeys) { cout << key << " "; } + for (const Key key : markedKeys) { + cout << key << " "; + } cout << endl; cout << "observedKeys: "; - for(const Key key: observedKeys) { cout << key << " "; } + for (const Key key : observedKeys) { + cout << key << " "; + } cout << endl; } // 1. Remove top of Bayes tree and convert to a factor graph: - // (a) For each affected variable, remove the corresponding clique and all parents up to the root. - // (b) Store orphaned sub-trees \BayesTree_{O} of removed cliques. + // (a) For each affected variable, remove the corresponding clique and all + // parents up to the root. (b) Store orphaned sub-trees \BayesTree_{O} of + // removed cliques. gttic(removetop); Cliques orphans; GaussianBayesNet affectedBayesNet; - this->removeTop(FastVector(markedKeys.begin(), markedKeys.end()), affectedBayesNet, orphans); + this->removeTop(FastVector(markedKeys.begin(), markedKeys.end()), + affectedBayesNet, orphans); gttoc(removetop); // FactorGraph factors(affectedBayesNet); - // bug was here: we cannot reuse the original factors, because then the cached factors get messed up - // [all the necessary data is actually contained in the affectedBayesNet, including what was passed in from the boundaries, - // so this would be correct; however, in the process we also generate new cached_ entries that will be wrong (ie. they don't - // contain what would be passed up at a certain point if batch elimination was done, but that's what we need); we could choose - // not to update cached_ from here, but then the new information (and potentially different variable ordering) is not reflected - // in the cached_ values which again will be wrong] - // so instead we have to retrieve the original linearized factors AND add the cached factors from the boundary + // bug was here: we cannot reuse the original factors, because then the cached + // factors get messed up [all the necessary data is actually contained in the + // affectedBayesNet, including what was passed in from the boundaries, + // so this would be correct; however, in the process we also generate new + // cached_ entries that will be wrong (ie. they don't contain what would be + // passed up at a certain point if batch elimination was done, but that's + // what we need); we could choose not to update cached_ from here, but then + // the new information (and potentially different variable ordering) is not + // reflected in the cached_ values which again will be wrong] + // so instead we have to retrieve the original linearized factors AND add the + // cached factors from the boundary // BEGIN OF COPIED CODE - // ordering provides all keys in conditionals, there cannot be others because path to root included + // ordering provides all keys in conditionals, there cannot be others because + // path to root included gttic(affectedKeys); FastList affectedKeys; - for(const ConditionalType::shared_ptr& conditional: affectedBayesNet) - affectedKeys.insert(affectedKeys.end(), conditional->beginFrontals(), conditional->endFrontals()); + for (const ConditionalType::shared_ptr& conditional : affectedBayesNet) + affectedKeys.insert(affectedKeys.end(), conditional->beginFrontals(), + conditional->endFrontals()); gttoc(affectedKeys); - boost::shared_ptr affectedKeysSet(new KeySet()); // Will return this result + boost::shared_ptr affectedKeysSet( + new KeySet()); // Will return this result - if(affectedKeys.size() >= theta_.size() * batchThreshold) - { + if (affectedKeys.size() >= theta_.size() * batchThreshold) { // Do a batch step - reorder and relinearize all variables gttic(batch); gttic(add_keys); - br::copy(variableIndex_ | br::map_keys, std::inserter(*affectedKeysSet, affectedKeysSet->end())); + br::copy(variableIndex_ | br::map_keys, + std::inserter(*affectedKeysSet, affectedKeysSet->end())); // Removed unused keys: VariableIndex affectedFactorsVarIndex = variableIndex_; - affectedFactorsVarIndex.removeUnusedVariables(unusedIndices.begin(), unusedIndices.end()); + affectedFactorsVarIndex.removeUnusedVariables(unusedIndices.begin(), + unusedIndices.end()); - for (const Key key: unusedIndices) - { + for (const Key key : unusedIndices) { affectedKeysSet->erase(key); } gttoc(add_keys); gttic(ordering); Ordering order; - if(constrainKeys) - { - order = Ordering::ColamdConstrained(affectedFactorsVarIndex, *constrainKeys); - } - else - { - if(theta_.size() > observedKeys.size()) - { + if (constrainKeys) { + order = + Ordering::ColamdConstrained(affectedFactorsVarIndex, *constrainKeys); + } else { + if (theta_.size() > observedKeys.size()) { // Only if some variables are unconstrained FastMap constraintGroups; - for(Key var: observedKeys) - constraintGroups[var] = 1; - order = Ordering::ColamdConstrained(affectedFactorsVarIndex, constraintGroups); - } - else - { + for (Key var : observedKeys) constraintGroups[var] = 1; + order = Ordering::ColamdConstrained(affectedFactorsVarIndex, + constraintGroups); + } else { order = Ordering::Colamd(affectedFactorsVarIndex); } } @@ -371,18 +668,21 @@ boost::shared_ptr ISAM2::recalculate(const KeySet& markedKeys, const Ke gttic(linearize); GaussianFactorGraph linearized = *nonlinearFactors_.linearize(theta_); - if(params_.cacheLinearizedFactors) - linearFactors_ = linearized; + if (params_.cacheLinearizedFactors) linearFactors_ = linearized; gttoc(linearize); gttic(eliminate); - ISAM2BayesTree::shared_ptr bayesTree = ISAM2JunctionTree(GaussianEliminationTree(linearized, affectedFactorsVarIndex, order)) - .eliminate(params_.getEliminationFunction()).first; + ISAM2BayesTree::shared_ptr bayesTree = + ISAM2JunctionTree( + GaussianEliminationTree(linearized, affectedFactorsVarIndex, order)) + .eliminate(params_.getEliminationFunction()) + .first; gttoc(eliminate); gttic(insert); this->clear(); - this->roots_.insert(this->roots_.end(), bayesTree->roots().begin(), bayesTree->roots().end()); + this->roots_.insert(this->roots_.end(), bayesTree->roots().begin(), + bayesTree->roots().end()); this->nodes_.insert(bayesTree->nodes().begin(), bayesTree->nodes().end()); gttoc(insert); @@ -394,8 +694,8 @@ boost::shared_ptr ISAM2::recalculate(const KeySet& markedKeys, const Ke lastAffectedFactorCount = linearized.size(); // Reeliminated keys for detailed results - if(params_.enableDetailedResults) { - for(Key key: theta_.keys()) { + if (params_.enableDetailedResults) { + for (Key key : theta_.keys()) { result.detail->variableStatus[key].isReeliminated = true; } } @@ -403,23 +703,31 @@ boost::shared_ptr ISAM2::recalculate(const KeySet& markedKeys, const Ke gttoc(batch); } else { - gttic(incremental); // 2. Add the new factors \Factors' into the resulting factor graph FastList affectedAndNewKeys; - affectedAndNewKeys.insert(affectedAndNewKeys.end(), affectedKeys.begin(), affectedKeys.end()); - affectedAndNewKeys.insert(affectedAndNewKeys.end(), observedKeys.begin(), observedKeys.end()); + affectedAndNewKeys.insert(affectedAndNewKeys.end(), affectedKeys.begin(), + affectedKeys.end()); + affectedAndNewKeys.insert(affectedAndNewKeys.end(), observedKeys.begin(), + observedKeys.end()); gttic(relinearizeAffected); - GaussianFactorGraph factors(*relinearizeAffectedFactors(affectedAndNewKeys, relinKeys)); - if(debug) factors.print("Relinearized factors: "); + GaussianFactorGraph factors( + *relinearizeAffectedFactors(affectedAndNewKeys, relinKeys)); + if (debug) factors.print("Relinearized factors: "); gttoc(relinearizeAffected); - if(debug) { cout << "Affected keys: "; for(const Key key: affectedKeys) { cout << key << " "; } cout << endl; } + if (debug) { + cout << "Affected keys: "; + for (const Key key : affectedKeys) { + cout << key << " "; + } + cout << endl; + } // Reeliminated keys for detailed results - if(params_.enableDetailedResults) { - for(Key key: affectedAndNewKeys) { + if (params_.enableDetailedResults) { + for (Key key : affectedAndNewKeys) { result.detail->variableStatus[key].isReeliminated = true; } } @@ -432,34 +740,38 @@ boost::shared_ptr ISAM2::recalculate(const KeySet& markedKeys, const Ke #ifdef PRINT_STATS // output for generating figures - cout << "linear: #markedKeys: " << markedKeys.size() << " #affectedVariables: " << affectedKeys.size() - << " #affectedFactors: " << factors.size() << " maxCliqueSize: " << maxClique - << " avgCliqueSize: " << avgClique << " #Cliques: " << numCliques << " nnzR: " << nnzR << endl; + cout << "linear: #markedKeys: " << markedKeys.size() + << " #affectedVariables: " << affectedKeys.size() + << " #affectedFactors: " << factors.size() + << " maxCliqueSize: " << maxClique << " avgCliqueSize: " << avgClique + << " #Cliques: " << numCliques << " nnzR: " << nnzR << endl; #endif gttic(cached); // add the cached intermediate results from the boundary of the orphans ... GaussianFactorGraph cachedBoundary = getCachedBoundaryFactors(orphans); - if(debug) cachedBoundary.print("Boundary factors: "); + if (debug) cachedBoundary.print("Boundary factors: "); factors.push_back(cachedBoundary); gttoc(cached); gttic(orphans); // Add the orphaned subtrees - for(const sharedClique& orphan: orphans) + for (const sharedClique& orphan : orphans) factors += boost::make_shared >(orphan); gttoc(orphans); - // END OF COPIED CODE - // 3. Re-order and eliminate the factor graph into a Bayes net (Algorithm [alg:eliminate]), and re-assemble into a new Bayes tree (Algorithm [alg:BayesTree]) + // 3. Re-order and eliminate the factor graph into a Bayes net (Algorithm + // [alg:eliminate]), and re-assemble into a new Bayes tree (Algorithm + // [alg:BayesTree]) gttic(reorder_and_eliminate); gttic(list_to_set); // create a partial reordering for the new and contaminated factors - // markedKeys are passed in: those variables will be forced to the end in the ordering + // markedKeys are passed in: those variables will be forced to the end in + // the ordering affectedKeysSet->insert(markedKeys.begin(), markedKeys.end()); affectedKeysSet->insert(affectedKeys.begin(), affectedKeys.end()); gttoc(list_to_set); @@ -468,37 +780,46 @@ boost::shared_ptr ISAM2::recalculate(const KeySet& markedKeys, const Ke gttic(ordering_constraints); // Create ordering constraints - FastMap constraintGroups; - if(constrainKeys) { + FastMap constraintGroups; + if (constrainKeys) { constraintGroups = *constrainKeys; } else { - constraintGroups = FastMap(); - const int group = observedKeys.size() < affectedFactorsVarIndex.size() ? 1 : 0; - for (Key var: observedKeys) + constraintGroups = FastMap(); + const int group = + observedKeys.size() < affectedFactorsVarIndex.size() ? 1 : 0; + for (Key var : observedKeys) constraintGroups.insert(make_pair(var, group)); } // Remove unaffected keys from the constraints - for(FastMap::iterator iter = constraintGroups.begin(); iter != constraintGroups.end(); /*Incremented in loop ++iter*/) { - if(unusedIndices.exists(iter->first) || !affectedKeysSet->exists(iter->first)) - constraintGroups.erase(iter ++); + for (FastMap::iterator iter = constraintGroups.begin(); + iter != constraintGroups.end(); + /*Incremented in loop ++iter*/) { + if (unusedIndices.exists(iter->first) || + !affectedKeysSet->exists(iter->first)) + constraintGroups.erase(iter++); else - ++ iter; + ++iter; } gttoc(ordering_constraints); // Generate ordering gttic(Ordering); - Ordering ordering = Ordering::ColamdConstrained(affectedFactorsVarIndex, constraintGroups); + Ordering ordering = + Ordering::ColamdConstrained(affectedFactorsVarIndex, constraintGroups); gttoc(Ordering); - ISAM2BayesTree::shared_ptr bayesTree = ISAM2JunctionTree(GaussianEliminationTree( - factors, affectedFactorsVarIndex, ordering)).eliminate(params_.getEliminationFunction()).first; + ISAM2BayesTree::shared_ptr bayesTree = + ISAM2JunctionTree( + GaussianEliminationTree(factors, affectedFactorsVarIndex, ordering)) + .eliminate(params_.getEliminationFunction()) + .first; gttoc(reorder_and_eliminate); gttic(reassemble); - this->roots_.insert(this->roots_.end(), bayesTree->roots().begin(), bayesTree->roots().end()); + this->roots_.insert(this->roots_.end(), bayesTree->roots().begin(), + bayesTree->roots().end()); this->nodes_.insert(bayesTree->nodes().begin(), bayesTree->nodes().end()); gttoc(reassemble); @@ -508,9 +829,9 @@ boost::shared_ptr ISAM2::recalculate(const KeySet& markedKeys, const Ke } // Root clique variables for detailed results - if(params_.enableDetailedResults) { - for(const sharedNode& root: this->roots()) - for(Key var: *root->conditional()) + if (params_.enableDetailedResults) { + for (const sharedNode& root : this->roots()) + for (Key var : *root->conditional()) result.detail->variableStatus[var].inRootClique = true; } @@ -519,11 +840,12 @@ boost::shared_ptr ISAM2::recalculate(const KeySet& markedKeys, const Ke /* ************************************************************************* */ ISAM2Result ISAM2::update( - const NonlinearFactorGraph& newFactors, const Values& newTheta, const FactorIndices& removeFactorIndices, - const boost::optional >& constrainedKeys, const boost::optional >& noRelinKeys, - const boost::optional >& extraReelimKeys, bool force_relinearize) -{ - + const NonlinearFactorGraph& newFactors, const Values& newTheta, + const FactorIndices& removeFactorIndices, + const boost::optional >& constrainedKeys, + const boost::optional >& noRelinKeys, + const boost::optional >& extraReelimKeys, + bool force_relinearize) { const bool debug = ISDEBUG("ISAM2 update"); const bool verbose = ISDEBUG("ISAM2 update verbose"); @@ -538,18 +860,19 @@ ISAM2Result ISAM2::update( lastBacksubVariableCount = 0; lastNnzTop = 0; ISAM2Result result; - if(params_.enableDetailedResults) + if (params_.enableDetailedResults) result.detail = ISAM2Result::DetailedResults(); - const bool relinearizeThisStep = force_relinearize - || (params_.enableRelinearization && update_count_ % params_.relinearizeSkip == 0); + const bool relinearizeThisStep = + force_relinearize || (params_.enableRelinearization && + update_count_ % params_.relinearizeSkip == 0); - if(verbose) { + if (verbose) { cout << "ISAM2::update\n"; this->print("ISAM2: "); } // Update delta if we need it to check relinearization later - if(relinearizeThisStep) { + if (relinearizeThisStep) { gttic(updateDelta); updateDelta(disableReordering); gttoc(updateDelta); @@ -558,20 +881,23 @@ ISAM2Result ISAM2::update( gttic(push_back_factors); // 1. Add any new factors \Factors:=\Factors\cup\Factors'. // Add the new factor indices to the result struct - if(debug || verbose) newFactors.print("The new factors are: "); - Impl::AddFactorsStep1(newFactors, params_.findUnusedFactorSlots, nonlinearFactors_, result.newFactorsIndices); + if (debug || verbose) newFactors.print("The new factors are: "); + Impl::AddFactorsStep1(newFactors, params_.findUnusedFactorSlots, + nonlinearFactors_, result.newFactorsIndices); // Remove the removed factors - NonlinearFactorGraph removeFactors; removeFactors.reserve(removeFactorIndices.size()); - for(size_t index: removeFactorIndices) { + NonlinearFactorGraph removeFactors; + removeFactors.reserve(removeFactorIndices.size()); + for (size_t index : removeFactorIndices) { removeFactors.push_back(nonlinearFactors_[index]); nonlinearFactors_.remove(index); - if(params_.cacheLinearizedFactors) - linearFactors_.remove(index); + if (params_.cacheLinearizedFactors) linearFactors_.remove(index); } - // Remove removed factors from the variable index so we do not attempt to relinearize them - variableIndex_.remove(removeFactorIndices.begin(), removeFactorIndices.end(), removeFactors); + // Remove removed factors from the variable index so we do not attempt to + // relinearize them + variableIndex_.remove(removeFactorIndices.begin(), removeFactorIndices.end(), + removeFactors); // Compute unused keys and indices KeySet unusedKeys; @@ -580,123 +906,145 @@ ISAM2Result ISAM2::update( // Get keys from removed factors and new factors, and compute unused keys, // i.e., keys that are empty now and do not appear in the new factors. KeySet removedAndEmpty; - for(Key key: removeFactors.keys()) { - if(variableIndex_[key].empty()) + for (Key key : removeFactors.keys()) { + if (variableIndex_[key].empty()) removedAndEmpty.insert(removedAndEmpty.end(), key); } KeySet newFactorSymbKeys = newFactors.keys(); std::set_difference(removedAndEmpty.begin(), removedAndEmpty.end(), - newFactorSymbKeys.begin(), newFactorSymbKeys.end(), std::inserter(unusedKeys, unusedKeys.end())); + newFactorSymbKeys.begin(), newFactorSymbKeys.end(), + std::inserter(unusedKeys, unusedKeys.end())); // Get indices for unused keys - for(Key key: unusedKeys) { + for (Key key : unusedKeys) { unusedIndices.insert(unusedIndices.end(), key); } } gttoc(push_back_factors); gttic(add_new_variables); - // 2. Initialize any new variables \Theta_{new} and add \Theta:=\Theta\cup\Theta_{new}. + // 2. Initialize any new variables \Theta_{new} and add + // \Theta:=\Theta\cup\Theta_{new}. Impl::AddVariables(newTheta, theta_, delta_, deltaNewton_, RgProd_); // New keys for detailed results - if(params_.enableDetailedResults) { - for(Key key: newTheta.keys()) { result.detail->variableStatus[key].isNew = true; } } + if (params_.enableDetailedResults) { + for (Key key : newTheta.keys()) { + result.detail->variableStatus[key].isNew = true; + } + } gttoc(add_new_variables); gttic(evaluate_error_before); - if(params_.evaluateNonlinearError) + if (params_.evaluateNonlinearError) result.errorBefore.reset(nonlinearFactors_.error(calculateEstimate())); gttoc(evaluate_error_before); gttic(gather_involved_keys); // 3. Mark linear update - KeySet markedKeys = newFactors.keys(); // Get keys from new factors + KeySet markedKeys = newFactors.keys(); // Get keys from new factors // Also mark keys involved in removed factors { - KeySet markedRemoveKeys = removeFactors.keys(); // Get keys involved in removed factors - markedKeys.insert(markedRemoveKeys.begin(), markedRemoveKeys.end()); // Add to the overall set of marked keys + KeySet markedRemoveKeys = + removeFactors.keys(); // Get keys involved in removed factors + markedKeys.insert( + markedRemoveKeys.begin(), + markedRemoveKeys.end()); // Add to the overall set of marked keys } // Also mark any provided extra re-eliminate keys - if(extraReelimKeys) { - for(Key key: *extraReelimKeys) { + if (extraReelimKeys) { + for (Key key : *extraReelimKeys) { markedKeys.insert(key); } } // Observed keys for detailed results - if(params_.enableDetailedResults) { - for(Key key: markedKeys) { + if (params_.enableDetailedResults) { + for (Key key : markedKeys) { result.detail->variableStatus[key].isObserved = true; } } // NOTE: we use assign instead of the iterator constructor here because this // is a vector of size_t, so the constructor unintentionally resolves to // vector(size_t count, Key value) instead of the iterator constructor. - FastVector observedKeys; observedKeys.reserve(markedKeys.size()); - for(Key index: markedKeys) { - if(unusedIndices.find(index) == unusedIndices.end()) // Only add if not unused - observedKeys.push_back(index); // Make a copy of these, as we'll soon add to them + FastVector observedKeys; + observedKeys.reserve(markedKeys.size()); + for (Key index : markedKeys) { + if (unusedIndices.find(index) == + unusedIndices.end()) // Only add if not unused + observedKeys.push_back( + index); // Make a copy of these, as we'll soon add to them } gttoc(gather_involved_keys); - // Check relinearization if we're at the nth step, or we are using a looser loop relin threshold + // Check relinearization if we're at the nth step, or we are using a looser + // loop relin threshold KeySet relinKeys; if (relinearizeThisStep) { gttic(gather_relinearize_keys); - // 4. Mark keys in \Delta above threshold \beta: J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}. - if(params_.enablePartialRelinearizationCheck) - relinKeys = Impl::CheckRelinearizationPartial(roots_, delta_, params_.relinearizeThreshold); + // 4. Mark keys in \Delta above threshold \beta: + // J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}. + if (params_.enablePartialRelinearizationCheck) + relinKeys = Impl::CheckRelinearizationPartial( + roots_, delta_, params_.relinearizeThreshold); else - relinKeys = Impl::CheckRelinearizationFull(delta_, params_.relinearizeThreshold); - if(disableReordering) relinKeys = Impl::CheckRelinearizationFull(delta_, 0.0); // This is used for debugging + relinKeys = + Impl::CheckRelinearizationFull(delta_, params_.relinearizeThreshold); + if (disableReordering) + relinKeys = Impl::CheckRelinearizationFull( + delta_, 0.0); // This is used for debugging // Remove from relinKeys any keys whose linearization points are fixed - for(Key key: fixedVariables_) { + for (Key key : fixedVariables_) { relinKeys.erase(key); } - if(noRelinKeys) { - for(Key key: *noRelinKeys) { + if (noRelinKeys) { + for (Key key : *noRelinKeys) { relinKeys.erase(key); } } // Above relin threshold keys for detailed results - if(params_.enableDetailedResults) { - for(Key key: relinKeys) { + if (params_.enableDetailedResults) { + for (Key key : relinKeys) { result.detail->variableStatus[key].isAboveRelinThreshold = true; - result.detail->variableStatus[key].isRelinearized = true; } } + result.detail->variableStatus[key].isRelinearized = true; + } + } // Add the variables being relinearized to the marked keys KeySet markedRelinMask; - for(const Key key: relinKeys) - markedRelinMask.insert(key); + for (const Key key : relinKeys) markedRelinMask.insert(key); markedKeys.insert(relinKeys.begin(), relinKeys.end()); gttoc(gather_relinearize_keys); gttic(fluid_find_all); - // 5. Mark all cliques that involve marked variables \Theta_{J} and all their ancestors. + // 5. Mark all cliques that involve marked variables \Theta_{J} and all + // their ancestors. if (!relinKeys.empty()) { - for(const sharedClique& root: roots_) + for (const sharedClique& root : roots_) // add other cliques that have the marked ones in the separator Impl::FindAll(root, markedKeys, markedRelinMask); // Relin involved keys for detailed results - if(params_.enableDetailedResults) { + if (params_.enableDetailedResults) { KeySet involvedRelinKeys; - for(const sharedClique& root: roots_) + for (const sharedClique& root : roots_) Impl::FindAll(root, involvedRelinKeys, markedRelinMask); - for(Key key: involvedRelinKeys) { - if(!result.detail->variableStatus[key].isAboveRelinThreshold) { + for (Key key : involvedRelinKeys) { + if (!result.detail->variableStatus[key].isAboveRelinThreshold) { result.detail->variableStatus[key].isRelinearizeInvolved = true; - result.detail->variableStatus[key].isRelinearized = true; } } + result.detail->variableStatus[key].isRelinearized = true; + } + } } } gttoc(fluid_find_all); gttic(expmap); - // 6. Update linearization point for marked variables: \Theta_{J}:=\Theta_{J}+\Delta_{J}. + // 6. Update linearization point for marked variables: + // \Theta_{J}:=\Theta_{J}+\Delta_{J}. if (!relinKeys.empty()) - Impl::ExpmapMasked(theta_, delta_, markedRelinMask, delta_); + Impl::ExpmapMasked(delta_, markedRelinMask, &theta_, delta_); gttoc(expmap); result.variablesRelinearized = markedKeys.size(); @@ -706,17 +1054,16 @@ ISAM2Result ISAM2::update( gttic(linearize_new); // 7. Linearize new factors - if(params_.cacheLinearizedFactors) { + if (params_.cacheLinearizedFactors) { gttic(linearize); - GaussianFactorGraph::shared_ptr linearFactors = newFactors.linearize(theta_); - if(params_.findUnusedFactorSlots) - { + GaussianFactorGraph::shared_ptr linearFactors = + newFactors.linearize(theta_); + if (params_.findUnusedFactorSlots) { linearFactors_.resize(nonlinearFactors_.size()); - for(size_t newFactorI = 0; newFactorI < newFactors.size(); ++newFactorI) - linearFactors_[result.newFactorsIndices[newFactorI]] = (*linearFactors)[newFactorI]; - } - else - { + for (size_t newFactorI = 0; newFactorI < newFactors.size(); ++newFactorI) + linearFactors_[result.newFactorsIndices[newFactorI]] = + (*linearFactors)[newFactorI]; + } else { linearFactors_.push_back(*linearFactors); } assert(nonlinearFactors_.size() == linearFactors_.size()); @@ -726,7 +1073,7 @@ ISAM2Result ISAM2::update( gttic(augment_VI); // Augment the variable index with the new factors - if(params_.findUnusedFactorSlots) + if (params_.findUnusedFactorSlots) variableIndex_.augment(newFactors, result.newFactorsIndices); else variableIndex_.augment(newFactors); @@ -734,26 +1081,28 @@ ISAM2Result ISAM2::update( gttic(recalculate); // 8. Redo top of Bayes tree - boost::shared_ptr replacedKeys; - if(!markedKeys.empty() || !observedKeys.empty()) - replacedKeys = recalculate(markedKeys, relinKeys, observedKeys, unusedIndices, constrainedKeys, result); + boost::shared_ptr replacedKeys; + if (!markedKeys.empty() || !observedKeys.empty()) + replacedKeys = recalculate(markedKeys, relinKeys, observedKeys, + unusedIndices, constrainedKeys, result); // Update replaced keys mask (accumulates until back-substitution takes place) - if(replacedKeys) + if (replacedKeys) deltaReplacedMask_.insert(replacedKeys->begin(), replacedKeys->end()); gttoc(recalculate); // Update data structures to remove unused keys - if(!unusedKeys.empty()) { + if (!unusedKeys.empty()) { gttic(remove_variables); - Impl::RemoveVariables(unusedKeys, roots_, theta_, variableIndex_, delta_, deltaNewton_, RgProd_, - deltaReplacedMask_, Base::nodes_, fixedVariables_); + Impl::RemoveVariables(unusedKeys, roots_, theta_, variableIndex_, delta_, + deltaNewton_, RgProd_, deltaReplacedMask_, + Base::nodes_, fixedVariables_); gttoc(remove_variables); } result.cliques = this->nodes().size(); gttic(evaluate_error_after); - if(params_.evaluateNonlinearError) + if (params_.evaluateNonlinearError) result.errorAfter.reset(nonlinearFactors_.error(calculateEstimate())); gttoc(evaluate_error_after); @@ -761,16 +1110,16 @@ ISAM2Result ISAM2::update( } /* ************************************************************************* */ -void ISAM2::marginalizeLeaves(const FastList& leafKeysList, - boost::optional marginalFactorsIndices, - boost::optional deletedFactorsIndices) -{ +void ISAM2::marginalizeLeaves( + const FastList& leafKeysList, + boost::optional marginalFactorsIndices, + boost::optional deletedFactorsIndices) { // Convert to ordered set KeySet leafKeys(leafKeysList.begin(), leafKeysList.end()); // Keep track of marginal factors - map from clique to the marginal factors // that should be incorporated into it, passed up from it's children. -// multimap marginalFactors; + // multimap marginalFactors; map > marginalFactors; // Keep track of factors that get summarized by removing cliques @@ -780,17 +1129,17 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeysList, KeySet leafKeysRemoved; // Remove each variable and its subtrees - for(Key j: leafKeys) { - if(!leafKeysRemoved.exists(j)) { // If the index was not already removed by removing another subtree + for (Key j : leafKeys) { + if (!leafKeysRemoved.exists(j)) { // If the index was not already removed + // by removing another subtree // Traverse up the tree to find the root of the marginalized subtree sharedClique clique = nodes_[j]; - while(!clique->parent_._empty()) - { - // Check if parent contains a marginalized leaf variable. Only need to check the first - // variable because it is the closest to the leaves. + while (!clique->parent_._empty()) { + // Check if parent contains a marginalized leaf variable. Only need to + // check the first variable because it is the closest to the leaves. sharedClique parent = clique->parent(); - if(leafKeys.exists(parent->conditional()->front())) + if (leafKeys.exists(parent->conditional()->front())) clique = parent; else break; @@ -798,39 +1147,48 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeysList, // See if we should remove the whole clique bool marginalizeEntireClique = true; - for(Key frontal: clique->conditional()->frontals()) { - if(!leafKeys.exists(frontal)) { + for (Key frontal : clique->conditional()->frontals()) { + if (!leafKeys.exists(frontal)) { marginalizeEntireClique = false; - break; } } + break; + } + } // Remove either the whole clique or part of it - if(marginalizeEntireClique) { - // Remove the whole clique and its subtree, and keep the marginal factor. + if (marginalizeEntireClique) { + // Remove the whole clique and its subtree, and keep the marginal + // factor. GaussianFactor::shared_ptr marginalFactor = clique->cachedFactor(); // 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); + 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. - const Cliques removedCliques = this->removeSubtree(clique); // Remove the subtree and throw away the cliques - for(const sharedClique& removedClique: removedCliques) { + const Cliques removedCliques = this->removeSubtree( + clique); // Remove the subtree and throw away the cliques + for (const sharedClique& removedClique : removedCliques) { marginalFactors.erase(removedClique->conditional()->front()); - leafKeysRemoved.insert(removedClique->conditional()->beginFrontals(), removedClique->conditional()->endFrontals()); - for(Key frontal: removedClique->conditional()->frontals()) - { + leafKeysRemoved.insert(removedClique->conditional()->beginFrontals(), + removedClique->conditional()->endFrontals()); + for (Key frontal : removedClique->conditional()->frontals()) { // Add to factors to remove - const VariableIndex::Factors& involvedFactors = variableIndex_[frontal]; - factorIndicesToRemove.insert(involvedFactors.begin(), involvedFactors.end()); + const VariableIndex::Factors& involvedFactors = + variableIndex_[frontal]; + factorIndicesToRemove.insert(involvedFactors.begin(), + involvedFactors.end()); // Check for non-leaf keys - if(!leafKeys.exists(frontal)) - throw runtime_error("Requesting to marginalize variables that are not leaves, the ISAM2 object is now in an inconsistent state so should no longer be used."); + if (!leafKeys.exists(frontal)) + throw runtime_error( + "Requesting to marginalize variables that are not leaves, " + "the ISAM2 object is now in an inconsistent state so should " + "no longer be used."); } } - } - else { + } else { // Reeliminate the current clique and the marginals from its children, // then keep only the marginal on the non-marginalized variables. We // get the childrens' marginals from any existing children, plus @@ -841,181 +1199,220 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeysList, GaussianFactorGraph graph; KeySet factorsInSubtreeRoot; Cliques subtreesToRemove; - for(const sharedClique& child: clique->children) { + for (const sharedClique& child : clique->children) { // Remove subtree if child depends on any marginalized keys - for(Key parent: child->conditional()->parents()) { - if(leafKeys.exists(parent)) { + for (Key parent : child->conditional()->parents()) { + if (leafKeys.exists(parent)) { subtreesToRemove.push_back(child); - graph.push_back(child->cachedFactor()); // Add child marginal + graph.push_back(child->cachedFactor()); // Add child marginal break; } } } Cliques childrenRemoved; - for(const sharedClique& childToRemove: subtreesToRemove) { - const Cliques removedCliques = this->removeSubtree(childToRemove); // Remove the subtree and throw away the cliques - childrenRemoved.insert(childrenRemoved.end(), removedCliques.begin(), removedCliques.end()); - for(const sharedClique& removedClique: removedCliques) { + for (const sharedClique& childToRemove : subtreesToRemove) { + const Cliques removedCliques = this->removeSubtree( + childToRemove); // Remove the subtree and throw away the cliques + childrenRemoved.insert(childrenRemoved.end(), removedCliques.begin(), + removedCliques.end()); + for (const sharedClique& removedClique : removedCliques) { marginalFactors.erase(removedClique->conditional()->front()); - leafKeysRemoved.insert(removedClique->conditional()->beginFrontals(), removedClique->conditional()->endFrontals()); - for(Key frontal: removedClique->conditional()->frontals()) - { + leafKeysRemoved.insert( + removedClique->conditional()->beginFrontals(), + removedClique->conditional()->endFrontals()); + for (Key frontal : removedClique->conditional()->frontals()) { // Add to factors to remove - const VariableIndex::Factors& involvedFactors = variableIndex_[frontal]; - factorIndicesToRemove.insert(involvedFactors.begin(), involvedFactors.end()); + const VariableIndex::Factors& involvedFactors = + variableIndex_[frontal]; + factorIndicesToRemove.insert(involvedFactors.begin(), + involvedFactors.end()); // Check for non-leaf keys - if(!leafKeys.exists(frontal)) - throw runtime_error("Requesting to marginalize variables that are not leaves, the ISAM2 object is now in an inconsistent state so should no longer be used."); + if (!leafKeys.exists(frontal)) + throw runtime_error( + "Requesting to marginalize variables that are not leaves, " + "the ISAM2 object is now in an inconsistent state so " + "should no longer be used."); } } } - // Add the factors that are pulled into the current clique by the marginalized variables. - // These are the factors that involve *marginalized* frontal variables in this clique - // but do not involve frontal variables of any of its children. - // TODO: reuse cached linear factors + // Add the factors that are pulled into the current clique by the + // marginalized variables. These are the factors that involve + // *marginalized* frontal variables in this clique but do not involve + // frontal variables of any of its children. + // TODO(dellaert): reuse cached linear factors KeySet factorsFromMarginalizedInClique_step1; - for(Key frontal: clique->conditional()->frontals()) { - if(leafKeys.exists(frontal)) - factorsFromMarginalizedInClique_step1.insert(variableIndex_[frontal].begin(), variableIndex_[frontal].end()); } + for (Key frontal : clique->conditional()->frontals()) { + if (leafKeys.exists(frontal)) + factorsFromMarginalizedInClique_step1.insert( + variableIndex_[frontal].begin(), variableIndex_[frontal].end()); + } // Remove any factors in subtrees that we're removing at this step - for(const sharedClique& removedChild: childrenRemoved) { - for(Key indexInClique: removedChild->conditional()->frontals()) { - for(Key factorInvolving: variableIndex_[indexInClique]) { - factorsFromMarginalizedInClique_step1.erase(factorInvolving); } } } + for (const sharedClique& removedChild : childrenRemoved) { + for (Key indexInClique : removedChild->conditional()->frontals()) { + for (Key factorInvolving : variableIndex_[indexInClique]) { + factorsFromMarginalizedInClique_step1.erase(factorInvolving); + } + } + } // Create factor graph from factor indices - for(size_t i: factorsFromMarginalizedInClique_step1) { - graph.push_back(nonlinearFactors_[i]->linearize(theta_)); } + for (size_t i : factorsFromMarginalizedInClique_step1) { + graph.push_back(nonlinearFactors_[i]->linearize(theta_)); + } - // Reeliminate the linear graph to get the marginal and discard the conditional - const KeySet cliqueFrontals(clique->conditional()->beginFrontals(), clique->conditional()->endFrontals()); + // Reeliminate the linear graph to get the marginal and discard the + // conditional + const KeySet cliqueFrontals(clique->conditional()->beginFrontals(), + clique->conditional()->endFrontals()); FastVector cliqueFrontalsToEliminate; - std::set_intersection(cliqueFrontals.begin(), cliqueFrontals.end(), leafKeys.begin(), leafKeys.end(), - std::back_inserter(cliqueFrontalsToEliminate)); - pair eliminationResult1 = - params_.getEliminationFunction()(graph, Ordering(cliqueFrontalsToEliminate)); + std::set_intersection(cliqueFrontals.begin(), cliqueFrontals.end(), + leafKeys.begin(), leafKeys.end(), + std::back_inserter(cliqueFrontalsToEliminate)); + pair + eliminationResult1 = params_.getEliminationFunction()( + graph, Ordering(cliqueFrontalsToEliminate)); // Add the resulting marginal - if(eliminationResult1.second) - marginalFactors[clique->conditional()->front()].push_back(eliminationResult1.second); + if (eliminationResult1.second) + marginalFactors[clique->conditional()->front()].push_back( + eliminationResult1.second); // Split the current clique // Find the position of the last leaf key in this clique DenseIndex nToRemove = 0; - while(leafKeys.exists(clique->conditional()->keys()[nToRemove])) - ++ nToRemove; + while (leafKeys.exists(clique->conditional()->keys()[nToRemove])) + ++nToRemove; // Make the clique's matrix appear as a subset - const DenseIndex dimToRemove = clique->conditional()->matrixObject().offset(nToRemove); + const DenseIndex dimToRemove = + clique->conditional()->matrixObject().offset(nToRemove); clique->conditional()->matrixObject().firstBlock() = nToRemove; clique->conditional()->matrixObject().rowStart() = dimToRemove; // Change the keys in the clique - FastVector originalKeys; originalKeys.swap(clique->conditional()->keys()); - clique->conditional()->keys().assign(originalKeys.begin() + nToRemove, originalKeys.end()); + FastVector originalKeys; + originalKeys.swap(clique->conditional()->keys()); + clique->conditional()->keys().assign(originalKeys.begin() + nToRemove, + originalKeys.end()); clique->conditional()->nrFrontals() -= nToRemove; - // Add to factors to remove factors involved in frontals of current clique - for(Key frontal: cliqueFrontalsToEliminate) - { - const VariableIndex::Factors& involvedFactors = variableIndex_[frontal]; - factorIndicesToRemove.insert(involvedFactors.begin(), involvedFactors.end()); + // Add to factors to remove factors involved in frontals of current + // clique + for (Key frontal : cliqueFrontalsToEliminate) { + const VariableIndex::Factors& involvedFactors = + variableIndex_[frontal]; + factorIndicesToRemove.insert(involvedFactors.begin(), + involvedFactors.end()); } // Add removed keys - leafKeysRemoved.insert(cliqueFrontalsToEliminate.begin(), cliqueFrontalsToEliminate.end()); + leafKeysRemoved.insert(cliqueFrontalsToEliminate.begin(), + cliqueFrontalsToEliminate.end()); } } } - // At this point we have updated the BayesTree, now update the remaining iSAM2 data structures + // At this point we have updated the BayesTree, now update the remaining iSAM2 + // data structures // Gather factors to add - the new marginal factors GaussianFactorGraph factorsToAdd; typedef pair > Key_Factors; - for(const Key_Factors& key_factors: marginalFactors) { - for(const GaussianFactor::shared_ptr& factor: key_factors.second) { - if(factor) { + for (const Key_Factors& key_factors : marginalFactors) { + for (const GaussianFactor::shared_ptr& factor : key_factors.second) { + if (factor) { factorsToAdd.push_back(factor); - if(marginalFactorsIndices) + if (marginalFactorsIndices) marginalFactorsIndices->push_back(nonlinearFactors_.size()); - nonlinearFactors_.push_back(boost::make_shared( - factor)); - if(params_.cacheLinearizedFactors) - linearFactors_.push_back(factor); - for(Key factorKey: *factor) { - fixedVariables_.insert(factorKey); } + nonlinearFactors_.push_back( + boost::make_shared(factor)); + if (params_.cacheLinearizedFactors) linearFactors_.push_back(factor); + for (Key factorKey : *factor) { + fixedVariables_.insert(factorKey); + } } } } - variableIndex_.augment(factorsToAdd); // Augment the variable index + variableIndex_.augment(factorsToAdd); // Augment the variable index - // Remove the factors to remove that have been summarized in the newly-added marginal factors + // Remove the factors to remove that have been summarized in the newly-added + // marginal factors NonlinearFactorGraph removedFactors; - for(size_t i: factorIndicesToRemove) { + for (size_t i : factorIndicesToRemove) { removedFactors.push_back(nonlinearFactors_[i]); nonlinearFactors_.remove(i); - if(params_.cacheLinearizedFactors) - linearFactors_.remove(i); + if (params_.cacheLinearizedFactors) linearFactors_.remove(i); } - variableIndex_.remove(factorIndicesToRemove.begin(), factorIndicesToRemove.end(), removedFactors); + variableIndex_.remove(factorIndicesToRemove.begin(), + factorIndicesToRemove.end(), removedFactors); - if(deletedFactorsIndices) - deletedFactorsIndices->assign(factorIndicesToRemove.begin(), factorIndicesToRemove.end()); + if (deletedFactorsIndices) + deletedFactorsIndices->assign(factorIndicesToRemove.begin(), + factorIndicesToRemove.end()); // Remove the marginalized variables - Impl::RemoveVariables(KeySet(leafKeys.begin(), leafKeys.end()), roots_, theta_, variableIndex_, delta_, deltaNewton_, RgProd_, - deltaReplacedMask_, nodes_, fixedVariables_); + Impl::RemoveVariables(KeySet(leafKeys.begin(), leafKeys.end()), roots_, + theta_, variableIndex_, delta_, deltaNewton_, RgProd_, + deltaReplacedMask_, nodes_, fixedVariables_); } /* ************************************************************************* */ -void ISAM2::updateDelta(bool forceFullSolve) const -{ +void ISAM2::updateDelta(bool forceFullSolve) const { gttic(updateDelta); - if(params_.optimizationParams.type() == typeid(ISAM2GaussNewtonParams)) { + if (params_.optimizationParams.type() == typeid(ISAM2GaussNewtonParams)) { // If using Gauss-Newton, update with wildfireThreshold const ISAM2GaussNewtonParams& gaussNewtonParams = boost::get(params_.optimizationParams); - const double effectiveWildfireThreshold = forceFullSolve ? 0.0 : gaussNewtonParams.wildfireThreshold; + const double effectiveWildfireThreshold = + forceFullSolve ? 0.0 : gaussNewtonParams.wildfireThreshold; gttic(Wildfire_update); lastBacksubVariableCount = Impl::UpdateGaussNewtonDelta( - roots_, deltaReplacedMask_, delta_, effectiveWildfireThreshold); + roots_, deltaReplacedMask_, effectiveWildfireThreshold, &delta_); deltaReplacedMask_.clear(); gttoc(Wildfire_update); - } else if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) { + } else if (params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) { // If using Dogleg, do a Dogleg step const ISAM2DoglegParams& doglegParams = boost::get(params_.optimizationParams); - const double effectiveWildfireThreshold = forceFullSolve ? 0.0 : doglegParams.wildfireThreshold; + const double effectiveWildfireThreshold = + forceFullSolve ? 0.0 : doglegParams.wildfireThreshold; // Do one Dogleg iteration gttic(Dogleg_Iterate); // Compute Newton's method step gttic(Wildfire_update); - lastBacksubVariableCount = Impl::UpdateGaussNewtonDelta(roots_, deltaReplacedMask_, deltaNewton_, effectiveWildfireThreshold); + lastBacksubVariableCount = Impl::UpdateGaussNewtonDelta( + roots_, deltaReplacedMask_, effectiveWildfireThreshold, &deltaNewton_); gttoc(Wildfire_update); // Compute steepest descent step - const VectorValues gradAtZero = this->gradientAtZero(); // Compute gradient - Impl::UpdateRgProd(roots_, deltaReplacedMask_, gradAtZero, RgProd_); // Update RgProd - const VectorValues dx_u = Impl::ComputeGradientSearch(gradAtZero, RgProd_); // Compute gradient search point + const VectorValues gradAtZero = this->gradientAtZero(); // Compute gradient + Impl::UpdateRgProd(roots_, deltaReplacedMask_, gradAtZero, + &RgProd_); // Update RgProd + const VectorValues dx_u = Impl::ComputeGradientSearch( + gradAtZero, RgProd_); // Compute gradient search point - // Clear replaced keys mask because now we've updated deltaNewton_ and RgProd_ + // Clear replaced keys mask because now we've updated deltaNewton_ and + // RgProd_ deltaReplacedMask_.clear(); // Compute dogleg point - DoglegOptimizerImpl::IterationResult doglegResult(DoglegOptimizerImpl::Iterate( - *doglegDelta_, doglegParams.adaptationMode, dx_u, deltaNewton_, *this, nonlinearFactors_, - theta_, nonlinearFactors_.error(theta_), doglegParams.verbose)); + DoglegOptimizerImpl::IterationResult doglegResult( + DoglegOptimizerImpl::Iterate( + *doglegDelta_, doglegParams.adaptationMode, dx_u, deltaNewton_, + *this, nonlinearFactors_, theta_, nonlinearFactors_.error(theta_), + doglegParams.verbose)); gttoc(Dogleg_Iterate); gttic(Copy_dx_d); // Update Delta and linear step doglegDelta_ = doglegResult.delta; - delta_ = doglegResult.dx_d; // Copy the VectorValues containing with the linear solution + delta_ = + doglegResult + .dx_d; // Copy the VectorValues containing with the linear solution gttoc(Copy_dx_d); } } @@ -1037,60 +1434,62 @@ const Value& ISAM2::calculateEstimate(Key key) const { /* ************************************************************************* */ Values ISAM2::calculateBestEstimate() const { - updateDelta(true); // Force full solve when updating delta_ + updateDelta(true); // Force full solve when updating delta_ return theta_.retract(delta_); } /* ************************************************************************* */ Matrix ISAM2::marginalCovariance(Key key) const { - return marginalFactor(key, params_.getEliminationFunction())->information().inverse(); + return marginalFactor(key, params_.getEliminationFunction()) + ->information() + .inverse(); } /* ************************************************************************* */ const VectorValues& ISAM2::getDelta() const { - if(!deltaReplacedMask_.empty()) - updateDelta(); + if (!deltaReplacedMask_.empty()) updateDelta(); return delta_; } /* ************************************************************************* */ -double ISAM2::error(const VectorValues& x) const -{ +double ISAM2::error(const VectorValues& x) const { return GaussianFactorGraph(*this).error(x); } /* ************************************************************************* */ -static void gradientAtZeroTreeAdder(const boost::shared_ptr& root, VectorValues& g) { +static void gradientAtZeroTreeAdder(const boost::shared_ptr& root, + VectorValues* g) { // Loop through variables in each clique, adding contributions DenseIndex variablePosition = 0; - for(GaussianConditional::const_iterator jit = root->conditional()->begin(); jit != root->conditional()->end(); ++jit) { + for (GaussianConditional::const_iterator jit = root->conditional()->begin(); + jit != root->conditional()->end(); ++jit) { const DenseIndex dim = root->conditional()->getDim(jit); - pair pos_ins = - g.tryInsert(*jit, root->gradientContribution().segment(variablePosition, dim)); - if(!pos_ins.second) - pos_ins.first->second += root->gradientContribution().segment(variablePosition, dim); + pair pos_ins = g->tryInsert( + *jit, root->gradientContribution().segment(variablePosition, dim)); + if (!pos_ins.second) + pos_ins.first->second += + root->gradientContribution().segment(variablePosition, dim); variablePosition += dim; } // Recursively add contributions from children typedef boost::shared_ptr sharedClique; - for(const sharedClique& child: root->children) { + for (const sharedClique& child : root->children) { gradientAtZeroTreeAdder(child, g); } } /* ************************************************************************* */ -VectorValues ISAM2::gradientAtZero() const -{ +VectorValues ISAM2::gradientAtZero() const { // Create result VectorValues g; // Sum up contributions for each clique - for(const ISAM2::sharedClique& root: this->roots()) - gradientAtZeroTreeAdder(root, g); + for (const ISAM2::sharedClique& root : this->roots()) + gradientAtZeroTreeAdder(root, &g); return g; } -} +} // namespace gtsam /// namespace gtsam diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 235cb65a5..d3904067c 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -509,7 +509,7 @@ class GTSAM_EXPORT ISAM2Clique Base::FactorType::shared_ptr cachedFactor_; Vector gradientContribution_; - FastMap solnPointers_; + mutable FastMap solnPointers_; /// Default constructor ISAM2Clique() : Base() {} @@ -546,6 +546,45 @@ class GTSAM_EXPORT ISAM2Clique void print(const std::string& s = "", const KeyFormatter& formatter = DefaultKeyFormatter) const; + /** + * Check if clique was replaced, or if any parents were changed above the + * threshold or themselves replaced. + */ + bool isDirty(const KeySet& replaced, const KeySet& changed) const; + + /// Copy values in delta pertaining to this clique. + FastVector copyRelevantValues(const VectorValues& delta) const; + + /* + * Check whether the values changed above a threshold, or always true if the + * clique was replaced. + */ + bool valuesChanged(const KeySet& replaced, + const FastVector& originalValues, + const VectorValues& delta, double threshold) const; + + /// Set changed flag for each frontal variable + void markFrontalsAsChanged(KeySet* changed) const; + + /// Restore delta to original values, guided by frontal keys. + void restoreFromOriginals(const FastVector& originalValues, + VectorValues* delta) const; + + void optimizeWildfire(const KeySet& replaced, double threshold, KeySet* changed, + VectorValues* delta, + size_t* count) const; + + bool optimizeWildfireNode(const KeySet& replaced, double threshold, KeySet* changed, + VectorValues* delta, + size_t* count) const; + + /** + * Starting from the root, add up entries of frontal and conditional matrices + * of each conditional + */ + void nnz_internal(size_t* result) const; + size_t calculate_nnz() const; + private: /** Serialization function */ friend class boost::serialization::access; @@ -810,30 +849,23 @@ class GTSAM_EXPORT ISAM2 : public BayesTree { template <> struct traits : public Testable {}; -/// Optimize the BayesTree, starting from the root. -/// @param replaced Needs to contain -/// all variables that are contained in the top of the Bayes tree that has been -/// redone. -/// @param delta The current solution, an offset from the linearization -/// point. -/// @param threshold The maximum change against the PREVIOUS delta for -/// non-replaced variables that can be ignored, ie. the old delta entry is kept -/// and recursive backsubstitution might eventually stop if none of the changed -/// variables are contained in the subtree. -/// @return The number of variables that were solved for -template -size_t optimizeWildfire(const boost::shared_ptr& root, double threshold, - const KeySet& replaced, VectorValues& delta); +/** + * Optimize the BayesTree, starting from the root. + * @param threshold The maximum change against the PREVIOUS delta for + * non-replaced variables that can be ignored, ie. the old delta entry is kept + * and recursive backsubstitution might eventually stop if none of the changed + * variables are contained in the subtree. + * @param replaced Needs to contain all variables that are contained in the top + * of the Bayes tree that has been redone. + * @return The number of variables that were solved for. + * @param delta The current solution, an offset from the linearization point. + */ +size_t optimizeWildfire(const ISAM2::sharedClique& root, double threshold, + const KeySet& replaced, VectorValues* delta); -template -size_t optimizeWildfireNonRecursive(const boost::shared_ptr& root, +size_t optimizeWildfireNonRecursive(const ISAM2::sharedClique& root, double threshold, const KeySet& replaced, - VectorValues& delta); - -/// calculate the number of non-zero entries for the tree starting at clique -/// (use root for complete matrix) -template -int calculate_nnz(const boost::shared_ptr& clique); + VectorValues* delta); } // namespace gtsam diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index f56b458be..681c30587 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -843,7 +843,7 @@ TEST(ISAM2, calculate_nnz) { ISAM2 isam = createSlamlikeISAM2(); int expected = 241; - int actual = calculate_nnz(isam.roots().front()); + int actual = isam.roots().front()->calculate_nnz(); EXPECT_LONGS_EQUAL(expected, actual); } From e770490ae78d39b26b1b528e5f0a7fe5c85c04f5 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 30 Sep 2018 10:28:45 -0400 Subject: [PATCH 12/51] Made vector templated on Key container. --- gtsam/linear/VectorValues.cpp | 22 ---------------------- gtsam/linear/VectorValues.h | 21 ++++++++++++++++++++- 2 files changed, 20 insertions(+), 23 deletions(-) diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index 35afddb3a..b037aad92 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -160,28 +160,6 @@ namespace gtsam { return result; } - /* ************************************************************************* */ - Vector VectorValues::vector(const FastVector& keys) const - { - // Count dimensions and collect pointers to avoid double lookups - DenseIndex totalDim = 0; - FastVector items(keys.size()); - for(size_t i = 0; i < keys.size(); ++i) { - items[i] = &at(keys[i]); - totalDim += items[i]->size(); - } - - // Copy vectors - Vector result(totalDim); - DenseIndex pos = 0; - for(const Vector *v: items) { - result.segment(pos, v->size()) = *v; - pos += v->size(); - } - - return result; - } - /* ************************************************************************* */ Vector VectorValues::vector(const Dims& keys) const { diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index cb1e08f2d..2d8eb0ec0 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -244,7 +244,26 @@ namespace gtsam { Vector vector() const; /** Access a vector that is a subset of relevant keys. */ - Vector vector(const FastVector& keys) const; + template + Vector vector(const CONTAINER& keys) const { + DenseIndex totalDim = 0; + FastVector items; + items.reserve(keys.end() - keys.begin()); + for (Key key : keys) { + const Vector* v = &at(key); + totalDim += v->size(); + items.push_back(v); + } + + Vector result(totalDim); + DenseIndex pos = 0; + for (const Vector* v : items) { + result.segment(pos, v->size()) = *v; + pos += v->size(); + } + + return result; + } /** Access a vector that is a subset of relevant keys, dims version. */ Vector vector(const Dims& dims) const; From 34326c20ec5a39266e0e9a4a4f33381c3761b2d7 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 30 Sep 2018 10:29:12 -0400 Subject: [PATCH 13/51] Cleaned up code cruft --- gtsam/linear/VectorValues.h | 128 ++++++++++++------------------------ 1 file changed, 41 insertions(+), 87 deletions(-) diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index 2d8eb0ec0..f187b56de 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -26,6 +26,9 @@ #include +#include +#include + namespace gtsam { /** @@ -43,10 +46,6 @@ namespace gtsam { * - \ref exists (Key) to check if a variable is present * - Other facilities like iterators, size(), dim(), etc. * - * Indices can be non-consecutive and inserted out-of-order, but you should not - * use indices that are larger than a reasonable array size because the indices - * correspond to positions in an internal array. - * * Example: * \code VectorValues values; @@ -64,12 +63,6 @@ namespace gtsam { * *

Advanced Interface and Performance Information

* - * Internally, all vector values are stored as part of one large vector. In - * gtsam this vector is always pre-allocated for efficiency, using the - * advanced interface described below. Accessing and modifying already-allocated - * values is \f$ O(1) \f$. Using the insert() function of the standard interface - * is slow because it requires re-allocating the internal vector. - * * For advanced usage, or where speed is important: * - Allocate space ahead of time using a pre-allocating constructor * (\ref AdvancedConstructors "Advanced Constructors"), Zero(), @@ -88,20 +81,18 @@ namespace gtsam { * \nosubgrouping */ class GTSAM_EXPORT VectorValues { - protected: + protected: typedef VectorValues This; - typedef ConcurrentMap Values; ///< Typedef for the collection of Vectors making up a VectorValues - Values values_; ///< Collection of Vectors making up this VectorValues + typedef ConcurrentMap Values; ///< Collection of Vectors making up a VectorValues + Values values_; ///< Vectors making up this VectorValues - public: - typedef Values::iterator iterator; ///< Iterator over vector values - typedef Values::const_iterator const_iterator; ///< Const iterator over vector values - //typedef Values::reverse_iterator reverse_iterator; ///< Reverse iterator over vector values - //typedef Values::const_reverse_iterator const_reverse_iterator; ///< Const reverse iterator over vector values - typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class - typedef Values::value_type value_type; ///< Typedef to pair, a key-value pair - typedef value_type KeyValuePair; ///< Typedef to pair, a key-value pair - typedef std::map Dims; + public: + typedef Values::iterator iterator; ///< Iterator over vector values + typedef Values::const_iterator const_iterator; ///< Const iterator over vector values + typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class + typedef Values::value_type value_type; ///< Typedef to pair + typedef value_type KeyValuePair; ///< Typedef to pair + typedef std::map Dims; ///< Keyed vector dimensions /// @name Standard Constructors /// @{ @@ -111,7 +102,8 @@ namespace gtsam { */ VectorValues() {} - /** Merge two VectorValues into one, this is more efficient than inserting elements one by one. */ + /** Merge two VectorValues into one, this is more efficient than inserting + * elements one by one. */ VectorValues(const VectorValues& first, const VectorValues& second); /** Create from another container holding pair. */ @@ -147,20 +139,26 @@ namespace gtsam { /** Check whether a variable with key \c j exists. */ bool exists(Key j) const { return find(j) != end(); } - /** Read/write access to the vector value with key \c j, throws std::out_of_range if \c j does not exist, identical to operator[](Key). */ + /** + * Read/write access to the vector value with key \c j, throws + * std::out_of_range if \c j does not exist, identical to operator[](Key). + */ Vector& at(Key j) { iterator item = find(j); - if(item == end()) + if (item == end()) throw std::out_of_range( "Requested variable '" + DefaultKeyFormatter(j) + "' is not in this VectorValues."); else return item->second; } - /** Access the vector value with key \c j (const version), throws std::out_of_range if \c j does not exist, identical to operator[](Key). */ + /** + * Access the vector value with key \c j (const version), throws + * std::out_of_range if \c j does not exist, identical to operator[](Key). + */ const Vector& at(Key j) const { const_iterator item = find(j); - if(item == end()) + if (item == end()) throw std::out_of_range( "Requested variable '" + DefaultKeyFormatter(j) + "' is not in this VectorValues."); else @@ -207,26 +205,30 @@ namespace gtsam { /** Erase the vector with the given key, or throw std::out_of_range if it does not exist */ void erase(Key var) { - if(values_.unsafe_erase(var) == 0) - throw std::invalid_argument("Requested variable '" + DefaultKeyFormatter(var) + "', is not in this VectorValues."); + if (values_.unsafe_erase(var) == 0) + throw std::invalid_argument("Requested variable '" + + DefaultKeyFormatter(var) + + "', is not in this VectorValues."); } /** Set all values to zero vectors. */ void setZero(); - iterator begin() { return values_.begin(); } ///< Iterator over variables - const_iterator begin() const { return values_.begin(); } ///< Iterator over variables - iterator end() { return values_.end(); } ///< Iterator over variables - const_iterator end() const { return values_.end(); } ///< Iterator over variables - //reverse_iterator rbegin() { return values_.rbegin(); } ///< Reverse iterator over variables - //const_reverse_iterator rbegin() const { return values_.rbegin(); } ///< Reverse iterator over variables - //reverse_iterator rend() { return values_.rend(); } ///< Reverse iterator over variables - //const_reverse_iterator rend() const { return values_.rend(); } ///< Reverse iterator over variables + iterator begin() { return values_.begin(); } ///< Iterator over variables + const_iterator begin() const { return values_.begin(); } ///< Iterator over variables + iterator end() { return values_.end(); } ///< Iterator over variables + const_iterator end() const { return values_.end(); } ///< Iterator over variables - /** Return the iterator corresponding to the requested key, or end() if no variable is present with this key. */ + /** + * Return the iterator corresponding to the requested key, or end() if no + * variable is present with this key. + */ iterator find(Key j) { return values_.find(j); } - /** Return the iterator corresponding to the requested key, or end() if no variable is present with this key. */ + /** + * Return the iterator corresponding to the requested key, or end() if no + * variable is present with this key. + */ const_iterator find(Key j) const { return values_.find(j); } /** print required by Testable for unit testing */ @@ -338,54 +340,6 @@ namespace gtsam { /// @} - /** - * scale a vector by a scalar - */ - //friend VectorValues operator*(const double a, const VectorValues &v) { - // VectorValues result = VectorValues::SameStructure(v); - // for(Key j = 0; j < v.size(); ++j) - // result.values_[j] = a * v.values_[j]; - // return result; - //} - - //// TODO: linear algebra interface seems to have been added for SPCG. - //friend void axpy(double alpha, const VectorValues& x, VectorValues& y) { - // if(x.size() != y.size()) - // throw std::invalid_argument("axpy(VectorValues) called with different vector sizes"); - // for(Key j = 0; j < x.size(); ++j) - // if(x.values_[j].size() == y.values_[j].size()) - // y.values_[j] += alpha * x.values_[j]; - // else - // throw std::invalid_argument("axpy(VectorValues) called with different vector sizes"); - //} - //// TODO: linear algebra interface seems to have been added for SPCG. - //friend void sqrt(VectorValues &x) { - // for(Key j = 0; j < x.size(); ++j) - // x.values_[j] = x.values_[j].cwiseSqrt(); - //} - - //// TODO: linear algebra interface seems to have been added for SPCG. - //friend void ediv(const VectorValues& numerator, const VectorValues& denominator, VectorValues &result) { - // if(numerator.size() != denominator.size() || numerator.size() != result.size()) - // throw std::invalid_argument("ediv(VectorValues) called with different vector sizes"); - // for(Key j = 0; j < numerator.size(); ++j) - // if(numerator.values_[j].size() == denominator.values_[j].size() && numerator.values_[j].size() == result.values_[j].size()) - // result.values_[j] = numerator.values_[j].cwiseQuotient(denominator.values_[j]); - // else - // throw std::invalid_argument("ediv(VectorValues) called with different vector sizes"); - //} - - //// TODO: linear algebra interface seems to have been added for SPCG. - //friend void edivInPlace(VectorValues& x, const VectorValues& y) { - // if(x.size() != y.size()) - // throw std::invalid_argument("edivInPlace(VectorValues) called with different vector sizes"); - // for(Key j = 0; j < x.size(); ++j) - // if(x.values_[j].size() == y.values_[j].size()) - // x.values_[j].array() /= y.values_[j].array(); - // else - // throw std::invalid_argument("edivInPlace(VectorValues) called with different vector sizes"); - //} - private: /** Serialization function */ friend class boost::serialization::access; From 5223713c1877be08f30a5b8a3307c282025ad175 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 30 Sep 2018 10:30:27 -0400 Subject: [PATCH 14/51] Use VectorValues::vector rather than bespoke code for saving delta values. --- gtsam/nonlinear/ISAM2.cpp | 58 +++++++++++++++------------------------ gtsam/nonlinear/ISAM2.h | 52 ++++++++++++++++------------------- 2 files changed, 46 insertions(+), 64 deletions(-) diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index bdd6b42bb..599219145 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -85,10 +85,10 @@ class ISAM2JunctionTree }; /* ************************************************************************* */ -std::string ISAM2DoglegParams::adaptationModeTranslator( +string ISAM2DoglegParams::adaptationModeTranslator( const DoglegOptimizerImpl::TrustRegionAdaptationMode& adaptationMode) const { - std::string s; + string s; switch (adaptationMode) { case DoglegOptimizerImpl::SEARCH_EACH_ITERATION: s = "SEARCH_EACH_ITERATION"; @@ -106,8 +106,8 @@ std::string ISAM2DoglegParams::adaptationModeTranslator( /* ************************************************************************* */ DoglegOptimizerImpl::TrustRegionAdaptationMode ISAM2DoglegParams::adaptationModeTranslator( - const std::string& adaptationMode) const { - std::string s = adaptationMode; + const string& adaptationMode) const { + string s = adaptationMode; boost::algorithm::to_upper(s); if (s == "SEARCH_EACH_ITERATION") return DoglegOptimizerImpl::SEARCH_EACH_ITERATION; @@ -120,8 +120,8 @@ ISAM2DoglegParams::adaptationModeTranslator( /* ************************************************************************* */ ISAM2Params::Factorization ISAM2Params::factorizationTranslator( - const std::string& str) { - std::string s = str; + const string& str) { + string s = str; boost::algorithm::to_upper(s); if (s == "QR") return ISAM2Params::QR; if (s == "CHOLESKY") return ISAM2Params::CHOLESKY; @@ -131,9 +131,9 @@ ISAM2Params::Factorization ISAM2Params::factorizationTranslator( } /* ************************************************************************* */ -std::string ISAM2Params::factorizationTranslator( +string ISAM2Params::factorizationTranslator( const ISAM2Params::Factorization& value) { - std::string s; + string s; switch (value) { case ISAM2Params::QR: s = "QR"; @@ -171,13 +171,12 @@ bool ISAM2Clique::equals(const This& other, double tol) const { } /* ************************************************************************* */ -void ISAM2Clique::print(const std::string& s, - const KeyFormatter& formatter) const { +void ISAM2Clique::print(const string& s, const KeyFormatter& formatter) const { Base::print(s, formatter); if (cachedFactor_) cachedFactor_->print(s + "Cached: ", formatter); else - std::cout << s << "Cached empty" << std::endl; + cout << s << "Cached empty" << endl; if (gradientContribution_.rows() != 0) gtsam::print(gradientContribution_, "Gradient contribution: "); } @@ -207,31 +206,16 @@ bool ISAM2Clique::isDirty(const KeySet& replaced, const KeySet& changed) const { } return dirty; } -/* ************************************************************************* */ -FastVector ISAM2Clique::copyRelevantValues( - const VectorValues& delta) const { - FastVector values; - values.reserve(conditional_->nrFrontals()); - for (Key frontal : conditional_->frontals()) { - values.push_back(delta[frontal]); - } - return values; -} /* ************************************************************************* */ bool ISAM2Clique::valuesChanged(const KeySet& replaced, - const FastVector& originalValues, + const Vector& originalValues, const VectorValues& delta, double threshold) const { - if (replaced.exists(conditional_->frontals().front())) return true; - size_t i = 0; - for (Key frontal : conditional_->frontals()) { - const Vector diff = originalValues[i++] - delta[frontal]; - if (diff.lpNorm() >= threshold) { - return true; - } - } - return false; + auto frontals = conditional_->frontals(); + if (replaced.exists(frontals.front())) return true; + auto diff = originalValues - delta.vector(frontals); + return diff.lpNorm() >= threshold; } /* ************************************************************************* */ @@ -243,11 +227,13 @@ void ISAM2Clique::markFrontalsAsChanged(KeySet* changed) const { } /* ************************************************************************* */ -void ISAM2Clique::restoreFromOriginals(const FastVector& originalValues, +void ISAM2Clique::restoreFromOriginals(const Vector& originalValues, VectorValues* delta) const { - size_t i = 0; + size_t pos = 0; for (Key frontal : conditional_->frontals()) { - delta->at(frontal) = originalValues[i++]; + auto v = delta->at(frontal); + v = originalValues.segment(pos, v.size()); + pos += v.size(); } } @@ -257,7 +243,7 @@ void ISAM2Clique::optimizeWildfire(const KeySet& replaced, double threshold, size_t* count) const { if (isDirty(replaced, *changed)) { // Temporary copy of the original values, to check how much they change - auto originalValues = copyRelevantValues(*delta); + auto originalValues = delta->vector(conditional_->frontals()); // Back-substitute delta->update(conditional_->solve(*delta)); @@ -285,7 +271,7 @@ bool ISAM2Clique::optimizeWildfireNode(const KeySet& replaced, double threshold, bool dirty = isDirty(replaced, *changed); if (dirty) { // Temporary copy of the original values, to check how much they change - auto originalValues = copyRelevantValues(*delta); + auto originalValues = delta->vector(conditional_->frontals()); // Back-substitute - special version stores solution pointers in cliques for // fast access. diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index d3904067c..465e5cd7f 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -546,36 +546,12 @@ class GTSAM_EXPORT ISAM2Clique void print(const std::string& s = "", const KeyFormatter& formatter = DefaultKeyFormatter) const; - /** - * Check if clique was replaced, or if any parents were changed above the - * threshold or themselves replaced. - */ - bool isDirty(const KeySet& replaced, const KeySet& changed) const; - - /// Copy values in delta pertaining to this clique. - FastVector copyRelevantValues(const VectorValues& delta) const; - - /* - * Check whether the values changed above a threshold, or always true if the - * clique was replaced. - */ - bool valuesChanged(const KeySet& replaced, - const FastVector& originalValues, - const VectorValues& delta, double threshold) const; - - /// Set changed flag for each frontal variable - void markFrontalsAsChanged(KeySet* changed) const; - - /// Restore delta to original values, guided by frontal keys. - void restoreFromOriginals(const FastVector& originalValues, - VectorValues* delta) const; - - void optimizeWildfire(const KeySet& replaced, double threshold, KeySet* changed, - VectorValues* delta, + void optimizeWildfire(const KeySet& replaced, double threshold, + KeySet* changed, VectorValues* delta, size_t* count) const; - bool optimizeWildfireNode(const KeySet& replaced, double threshold, KeySet* changed, - VectorValues* delta, + bool optimizeWildfireNode(const KeySet& replaced, double threshold, + KeySet* changed, VectorValues* delta, size_t* count) const; /** @@ -586,6 +562,26 @@ class GTSAM_EXPORT ISAM2Clique size_t calculate_nnz() const; private: + /** + * Check if clique was replaced, or if any parents were changed above the + * threshold or themselves replaced. + */ + bool isDirty(const KeySet& replaced, const KeySet& changed) const; + + /* + * Check whether the values changed above a threshold, or always true if the + * clique was replaced. + */ + bool valuesChanged(const KeySet& replaced, const Vector& originalValues, + const VectorValues& delta, double threshold) const; + + /// Set changed flag for each frontal variable + void markFrontalsAsChanged(KeySet* changed) const; + + /// Restore delta to original values, guided by frontal keys. + void restoreFromOriginals(const Vector& originalValues, + VectorValues* delta) const; + /** Serialization function */ friend class boost::serialization::access; template From ba644029853d360158a53c6e2ee1e7d3d2b2a25c Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 30 Sep 2018 10:47:32 -0400 Subject: [PATCH 15/51] Added fastBackSubstitute method --- gtsam/nonlinear/ISAM2.cpp | 145 ++++++++++++++++++++------------------ gtsam/nonlinear/ISAM2.h | 6 ++ 2 files changed, 81 insertions(+), 70 deletions(-) diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 599219145..c16fe04da 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -207,6 +207,80 @@ bool ISAM2Clique::isDirty(const KeySet& replaced, const KeySet& changed) const { return dirty; } +/* ************************************************************************* */ +/** + * Back-substitute - special version stores solution pointers in cliques for + * fast access. + */ +void ISAM2Clique::fastBackSubstitute(VectorValues* delta) const { + // TODO(gareth): This code shares a lot of logic w/ linearAlgorithms-inst, + // potentially refactor + + // Create solution part pointers if necessary and possible - necessary if + // solnPointers_ is empty, and possible if either we're a root, or we have + // a parent with valid solnPointers_. + ISAM2::sharedClique parent = parent_.lock(); + if (solnPointers_.empty() && (isRoot() || !parent->solnPointers_.empty())) { + for (Key frontal : conditional_->frontals()) + solnPointers_.emplace(frontal, delta->find(frontal)); + for (Key parentKey : conditional_->parents()) { + assert(parent->solnPointers_.exists(parentKey)); + solnPointers_.emplace(parentKey, parent->solnPointers_.at(parentKey)); + } + } + + // See if we can use solution part pointers - we can if they either + // already existed or were created above. + if (!solnPointers_.empty()) { + GaussianConditional& c = *conditional_; + // Solve matrix + Vector xS; + { + // Count dimensions of vector + DenseIndex dim = 0; + FastVector parentPointers; + parentPointers.reserve(conditional_->nrParents()); + for (Key parent : conditional_->parents()) { + parentPointers.push_back(solnPointers_.at(parent)); + dim += parentPointers.back()->second.size(); + } + + // Fill parent vector + xS.resize(dim); + DenseIndex vectorPos = 0; + for (const VectorValues::const_iterator& parentPointer : parentPointers) { + const Vector& parentVector = parentPointer->second; + xS.block(vectorPos, 0, parentVector.size(), 1) = + parentVector.block(0, 0, parentVector.size(), 1); + vectorPos += parentVector.size(); + } + } + + // NOTE(gareth): We can no longer write: xS = b - S * xS + // This is because Eigen (as of 3.3) no longer evaluates S * xS into + // a temporary, and the operation trashes valus in xS. + // See: http://eigen.tuxfamily.org/index.php?title=3.3 + const Vector rhs = c.getb() - c.get_S() * xS; + const Vector solution = c.get_R().triangularView().solve(rhs); + + // Check for indeterminant solution + if (solution.hasNaN()) + throw IndeterminantLinearSystemException(c.keys().front()); + + // Insert solution into a VectorValues + DenseIndex vectorPosition = 0; + for (GaussianConditional::const_iterator frontal = c.beginFrontals(); + frontal != c.endFrontals(); ++frontal) { + solnPointers_.at(*frontal)->second = + solution.segment(vectorPosition, c.getDim(frontal)); + vectorPosition += c.getDim(frontal); + } + } else { + // Just call plain solve because we couldn't use solution pointers. + delta->update(conditional_->solve(*delta)); + } +} + /* ************************************************************************* */ bool ISAM2Clique::valuesChanged(const KeySet& replaced, const Vector& originalValues, @@ -273,76 +347,7 @@ bool ISAM2Clique::optimizeWildfireNode(const KeySet& replaced, double threshold, // Temporary copy of the original values, to check how much they change auto originalValues = delta->vector(conditional_->frontals()); - // Back-substitute - special version stores solution pointers in cliques for - // fast access. - { - // Create solution part pointers if necessary and possible - necessary if - // solnPointers_ is empty, and possible if either we're a root, or we have - // a parent with valid solnPointers_. - ISAM2::sharedClique parent = parent_.lock(); - if (solnPointers_.empty() && - (isRoot() || !parent->solnPointers_.empty())) { - for (Key frontal : conditional_->frontals()) - solnPointers_.emplace(frontal, delta->find(frontal)); - for (Key parentKey : conditional_->parents()) { - assert(parent->solnPointers_.exists(parentKey)); - solnPointers_.emplace(parentKey, parent->solnPointers_.at(parentKey)); - } - } - - // See if we can use solution part pointers - we can if they either - // already existed or were created above. - if (!solnPointers_.empty()) { - GaussianConditional& c = *conditional_; - // Solve matrix - Vector xS; - { - // Count dimensions of vector - DenseIndex dim = 0; - FastVector parentPointers; - parentPointers.reserve(conditional_->nrParents()); - for (Key parent : conditional_->parents()) { - parentPointers.push_back(solnPointers_.at(parent)); - dim += parentPointers.back()->second.size(); - } - - // Fill parent vector - xS.resize(dim); - DenseIndex vectorPos = 0; - for (const VectorValues::const_iterator& parentPointer : - parentPointers) { - const Vector& parentVector = parentPointer->second; - xS.block(vectorPos, 0, parentVector.size(), 1) = - parentVector.block(0, 0, parentVector.size(), 1); - vectorPos += parentVector.size(); - } - } - - // NOTE(gareth): We can no longer write: xS = b - S * xS - // This is because Eigen (as of 3.3) no longer evaluates S * xS into - // a temporary, and the operation trashes valus in xS. - // See: http://eigen.tuxfamily.org/index.php?title=3.3 - const Vector rhs = c.getb() - c.get_S() * xS; - const Vector solution = - c.get_R().triangularView().solve(rhs); - - // Check for indeterminant solution - if (solution.hasNaN()) - throw IndeterminantLinearSystemException(c.keys().front()); - - // Insert solution into a VectorValues - DenseIndex vectorPosition = 0; - for (GaussianConditional::const_iterator frontal = c.beginFrontals(); - frontal != c.endFrontals(); ++frontal) { - solnPointers_.at(*frontal)->second = - solution.segment(vectorPosition, c.getDim(frontal)); - vectorPosition += c.getDim(frontal); - } - } else { - // Just call plain solve because we couldn't use solution pointers. - delta->update(conditional_->solve(*delta)); - } - } + fastBackSubstitute(delta); count += conditional_->nrFrontals(); if (valuesChanged(replaced, originalValues, *delta, threshold)) { diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 465e5cd7f..b60a24644 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -568,6 +568,12 @@ class GTSAM_EXPORT ISAM2Clique */ bool isDirty(const KeySet& replaced, const KeySet& changed) const; + /** + * Back-substitute - special version stores solution pointers in cliques for + * fast access. + */ + void fastBackSubstitute(VectorValues* delta) const; + /* * Check whether the values changed above a threshold, or always true if the * clique was replaced. From 7fd8bc1bf5e5039a1b270620b48823298c2c8e10 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 30 Sep 2018 11:19:25 -0400 Subject: [PATCH 16/51] Restored non-recursive version, disabled solution pointers back-substitute. --- gtsam/nonlinear/ISAM2-impl.cpp | 2 +- gtsam/nonlinear/ISAM2.cpp | 66 +++++++++++++++++----------------- gtsam/nonlinear/ISAM2.h | 2 ++ 3 files changed, 37 insertions(+), 33 deletions(-) diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index aa201138c..bc43c9b9d 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -304,7 +304,7 @@ size_t ISAM2::Impl::UpdateGaussNewtonDelta( // Optimize with wildfire lastBacksubVariableCount = 0; for (const ISAM2::sharedClique& root : roots) - lastBacksubVariableCount += optimizeWildfire( + lastBacksubVariableCount += optimizeWildfireNonRecursive( root, wildfireThreshold, replacedKeys, delta); // modifies delta #if !defined(NDEBUG) && defined(GTSAM_EXTRA_CONSISTENCY_CHECKS) diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index c16fe04da..f2b154cc4 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -213,6 +213,7 @@ bool ISAM2Clique::isDirty(const KeySet& replaced, const KeySet& changed) const { * fast access. */ void ISAM2Clique::fastBackSubstitute(VectorValues* delta) const { +#ifdef USE_BROKEN_FAST_BACKSUBSTITUTE // TODO(gareth): This code shares a lot of logic w/ linearAlgorithms-inst, // potentially refactor @@ -279,6 +280,9 @@ void ISAM2Clique::fastBackSubstitute(VectorValues* delta) const { // Just call plain solve because we couldn't use solution pointers. delta->update(conditional_->solve(*delta)); } +#else + delta->update(conditional_->solve(*delta)); +#endif } /* ************************************************************************* */ @@ -312,6 +316,7 @@ void ISAM2Clique::restoreFromOriginals(const Vector& originalValues, } /* ************************************************************************* */ +// Note: not being used right now in favor of non-recursive version below. void ISAM2Clique::optimizeWildfire(const KeySet& replaced, double threshold, KeySet* changed, VectorValues* delta, size_t* count) const { @@ -320,7 +325,7 @@ void ISAM2Clique::optimizeWildfire(const KeySet& replaced, double threshold, auto originalValues = delta->vector(conditional_->frontals()); // Back-substitute - delta->update(conditional_->solve(*delta)); + fastBackSubstitute(delta); count += conditional_->nrFrontals(); if (valuesChanged(replaced, originalValues, *delta, threshold)) { @@ -336,6 +341,15 @@ void ISAM2Clique::optimizeWildfire(const KeySet& replaced, double threshold, } } +size_t optimizeWildfire(const ISAM2::sharedClique& root, double threshold, + const KeySet& keys, VectorValues* delta) { + KeySet changed; + size_t count = 0; + // starting from the root, call optimize on each conditional + if (root) root->optimizeWildfire(keys, threshold, &changed, delta, &count); + return count; +} + /* ************************************************************************* */ bool ISAM2Clique::optimizeWildfireNode(const KeySet& replaced, double threshold, KeySet* changed, VectorValues* delta, @@ -347,6 +361,7 @@ bool ISAM2Clique::optimizeWildfireNode(const KeySet& replaced, double threshold, // Temporary copy of the original values, to check how much they change auto originalValues = delta->vector(conditional_->frontals()); + // Back-substitute fastBackSubstitute(delta); count += conditional_->nrFrontals(); @@ -360,37 +375,6 @@ bool ISAM2Clique::optimizeWildfireNode(const KeySet& replaced, double threshold, return dirty; } -/* ************************************************************************* */ -void ISAM2Clique::nnz_internal(size_t* result) const { - size_t dimR = conditional_->rows(); - size_t dimSep = conditional_->get_S().cols(); - *result += ((dimR + 1) * dimR) / 2 + dimSep * dimR; - // traverse the children - for (const auto& child : children) { - child->nnz_internal(result); - } -} - -/* ************************************************************************* */ -size_t ISAM2Clique::calculate_nnz() const { - size_t result = 0; - nnz_internal(&result); - return result; -} - -/* ************************************************************************* */ -size_t optimizeWildfire(const ISAM2::sharedClique& root, double threshold, - const KeySet& keys, VectorValues* delta) { - KeySet changed; - size_t count = 0; - // starting from the root, call optimize on each conditional - if (root) root->optimizeWildfire(keys, threshold, &changed, delta, &count); - return count; -} - -/* ************************************************************************* */ -// This version is non-recursive version, but seems to have -// a bug, as was diagnosed with ISAM2Example_SmartFactor. Disabled for now. size_t optimizeWildfireNonRecursive(const ISAM2::sharedClique& root, double threshold, const KeySet& keys, VectorValues* delta) { @@ -417,6 +401,24 @@ size_t optimizeWildfireNonRecursive(const ISAM2::sharedClique& root, return count; } +/* ************************************************************************* */ +void ISAM2Clique::nnz_internal(size_t* result) const { + size_t dimR = conditional_->rows(); + size_t dimSep = conditional_->get_S().cols(); + *result += ((dimR + 1) * dimR) / 2 + dimSep * dimR; + // traverse the children + for (const auto& child : children) { + child->nnz_internal(result); + } +} + +/* ************************************************************************* */ +size_t ISAM2Clique::calculate_nnz() const { + size_t result = 0; + nnz_internal(&result); + return result; +} + /* ************************************************************************* */ ISAM2::ISAM2(const ISAM2Params& params) : params_(params), update_count_(0) { if (params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index b60a24644..eb33038a7 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -509,7 +509,9 @@ class GTSAM_EXPORT ISAM2Clique Base::FactorType::shared_ptr cachedFactor_; Vector gradientContribution_; +#ifdef USE_BROKEN_FAST_BACKSUBSTITUTE mutable FastMap solnPointers_; +#endif /// Default constructor ISAM2Clique() : Base() {} From d401edc9175cee01c5307263c0ac60523ca1ec2f Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 30 Sep 2018 12:58:48 -0400 Subject: [PATCH 17/51] Split ISAM2 Clique, Params, and Result out in different files, cleaned up headers, and removed ISAM2-inl.h header --- gtsam/nonlinear/ISAM2-inl.h | 34 -- gtsam/nonlinear/ISAM2.cpp | 384 +------------------- gtsam/nonlinear/ISAM2.h | 612 +------------------------------- gtsam/nonlinear/ISAM2Clique.cpp | 302 ++++++++++++++++ gtsam/nonlinear/ISAM2Clique.h | 157 ++++++++ gtsam/nonlinear/ISAM2Params.cpp | 89 +++++ gtsam/nonlinear/ISAM2Params.h | 365 +++++++++++++++++++ gtsam/nonlinear/ISAM2Result.h | 159 +++++++++ 8 files changed, 1103 insertions(+), 999 deletions(-) delete mode 100644 gtsam/nonlinear/ISAM2-inl.h create mode 100644 gtsam/nonlinear/ISAM2Clique.cpp create mode 100644 gtsam/nonlinear/ISAM2Clique.h create mode 100644 gtsam/nonlinear/ISAM2Params.cpp create mode 100644 gtsam/nonlinear/ISAM2Params.h create mode 100644 gtsam/nonlinear/ISAM2Result.h diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h deleted file mode 100644 index fb63dc428..000000000 --- a/gtsam/nonlinear/ISAM2-inl.h +++ /dev/null @@ -1,34 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file ISAM2-inl.h - * @brief - * @author Richard Roberts - * @date Mar 16, 2012 - */ - -#pragma once - -#include - -namespace gtsam { - -/* ************************************************************************* */ -template -VALUE ISAM2::calculateEstimate(Key key) const { - const Vector& delta = getDelta()[key]; - return traits::Retract(theta_.at(key), delta); -} - -/* ************************************************************************* */ - -} // namespace gtsam diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index f2b154cc4..e543508b2 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -10,22 +10,21 @@ * -------------------------------------------------------------------------- */ /** - * @file ISAM2-inl.h + * @file ISAM2.cpp * @brief Incremental update functionality (ISAM2) for BayesTree, with fluid * relinearization. - * @author Michael Kaess, Richard Roberts + * @author Michael Kaess, Richard Roberts, Frank Dellaert */ #include -#include -#include -#include -#include +#include +#include +#include +#include // We need the inst file because we'll make a special JT templated on ISAM2 +#include +#include -#include // for operator += -using namespace boost::assign; -#include #include #include namespace br { @@ -33,30 +32,19 @@ using namespace boost::range; using namespace boost::adaptors; } // namespace br -#include -#include -#include -#include -#include // We need the inst file because we'll make a special JT templated on ISAM2 -#include -#include -#include -#include - -#include -#include -#include +#include +#include +#include using namespace std; namespace gtsam { -// Instantiate base classes -template class BayesTreeCliqueBase; +// Instantiate base class template class BayesTree; -static const bool disableReordering = false; -static const double batchThreshold = 0.65; +static const bool kDisableReordering = false; +static const double kBatchThreshold = 0.65; /* ************************************************************************* */ // Special BayesTree class that uses ISAM2 cliques - this is the result of @@ -80,345 +68,10 @@ class ISAM2JunctionTree typedef ISAM2JunctionTree This; typedef boost::shared_ptr shared_ptr; - ISAM2JunctionTree(const GaussianEliminationTree& eliminationTree) + explicit ISAM2JunctionTree(const GaussianEliminationTree& eliminationTree) : Base(eliminationTree) {} }; -/* ************************************************************************* */ -string ISAM2DoglegParams::adaptationModeTranslator( - const DoglegOptimizerImpl::TrustRegionAdaptationMode& adaptationMode) - const { - string s; - switch (adaptationMode) { - case DoglegOptimizerImpl::SEARCH_EACH_ITERATION: - s = "SEARCH_EACH_ITERATION"; - break; - case DoglegOptimizerImpl::ONE_STEP_PER_ITERATION: - s = "ONE_STEP_PER_ITERATION"; - break; - default: - s = "UNDEFINED"; - break; - } - return s; -} - -/* ************************************************************************* */ -DoglegOptimizerImpl::TrustRegionAdaptationMode -ISAM2DoglegParams::adaptationModeTranslator( - const string& adaptationMode) const { - string s = adaptationMode; - boost::algorithm::to_upper(s); - if (s == "SEARCH_EACH_ITERATION") - return DoglegOptimizerImpl::SEARCH_EACH_ITERATION; - if (s == "ONE_STEP_PER_ITERATION") - return DoglegOptimizerImpl::ONE_STEP_PER_ITERATION; - - /* default is SEARCH_EACH_ITERATION */ - return DoglegOptimizerImpl::SEARCH_EACH_ITERATION; -} - -/* ************************************************************************* */ -ISAM2Params::Factorization ISAM2Params::factorizationTranslator( - const string& str) { - string s = str; - boost::algorithm::to_upper(s); - if (s == "QR") return ISAM2Params::QR; - if (s == "CHOLESKY") return ISAM2Params::CHOLESKY; - - /* default is CHOLESKY */ - return ISAM2Params::CHOLESKY; -} - -/* ************************************************************************* */ -string ISAM2Params::factorizationTranslator( - const ISAM2Params::Factorization& value) { - string s; - switch (value) { - case ISAM2Params::QR: - s = "QR"; - break; - case ISAM2Params::CHOLESKY: - s = "CHOLESKY"; - break; - default: - s = "UNDEFINED"; - break; - } - return s; -} - -/* ************************************************************************* */ -void ISAM2Clique::setEliminationResult( - const FactorGraphType::EliminationResult& eliminationResult) { - conditional_ = eliminationResult.first; - cachedFactor_ = eliminationResult.second; - // Compute gradient contribution - gradientContribution_.resize(conditional_->cols() - 1); - // Rewrite -(R * P')'*d as -(d' * R * P')' for computational speed - // reasons - gradientContribution_ << -conditional_->get_R().transpose() * - conditional_->get_d(), - -conditional_->get_S().transpose() * conditional_->get_d(); -} - -/* ************************************************************************* */ -bool ISAM2Clique::equals(const This& other, double tol) const { - return Base::equals(other) && - ((!cachedFactor_ && !other.cachedFactor_) || - (cachedFactor_ && other.cachedFactor_ && - cachedFactor_->equals(*other.cachedFactor_, tol))); -} - -/* ************************************************************************* */ -void ISAM2Clique::print(const string& s, const KeyFormatter& formatter) const { - Base::print(s, formatter); - if (cachedFactor_) - cachedFactor_->print(s + "Cached: ", formatter); - else - cout << s << "Cached empty" << endl; - if (gradientContribution_.rows() != 0) - gtsam::print(gradientContribution_, "Gradient contribution: "); -} - -/* ************************************************************************* */ -bool ISAM2Clique::isDirty(const KeySet& replaced, const KeySet& changed) const { - // if none of the variables in this clique (frontal and separator!) changed - // significantly, then by the running intersection property, none of the - // cliques in the children need to be processed - - // Are any clique variables part of the tree that has been redone? - bool dirty = replaced.exists(conditional_->frontals().front()); -#if !defined(NDEBUG) && defined(GTSAM_EXTRA_CONSISTENCY_CHECKS) - for (Key frontal : conditional_->frontals()) { - assert(dirty == replaced.exists(frontal)); - } -#endif - - // If not, then has one of the separator variables changed significantly? - if (!dirty) { - for (Key parent : conditional_->parents()) { - if (changed.exists(parent)) { - dirty = true; - break; - } - } - } - return dirty; -} - -/* ************************************************************************* */ -/** - * Back-substitute - special version stores solution pointers in cliques for - * fast access. - */ -void ISAM2Clique::fastBackSubstitute(VectorValues* delta) const { -#ifdef USE_BROKEN_FAST_BACKSUBSTITUTE - // TODO(gareth): This code shares a lot of logic w/ linearAlgorithms-inst, - // potentially refactor - - // Create solution part pointers if necessary and possible - necessary if - // solnPointers_ is empty, and possible if either we're a root, or we have - // a parent with valid solnPointers_. - ISAM2::sharedClique parent = parent_.lock(); - if (solnPointers_.empty() && (isRoot() || !parent->solnPointers_.empty())) { - for (Key frontal : conditional_->frontals()) - solnPointers_.emplace(frontal, delta->find(frontal)); - for (Key parentKey : conditional_->parents()) { - assert(parent->solnPointers_.exists(parentKey)); - solnPointers_.emplace(parentKey, parent->solnPointers_.at(parentKey)); - } - } - - // See if we can use solution part pointers - we can if they either - // already existed or were created above. - if (!solnPointers_.empty()) { - GaussianConditional& c = *conditional_; - // Solve matrix - Vector xS; - { - // Count dimensions of vector - DenseIndex dim = 0; - FastVector parentPointers; - parentPointers.reserve(conditional_->nrParents()); - for (Key parent : conditional_->parents()) { - parentPointers.push_back(solnPointers_.at(parent)); - dim += parentPointers.back()->second.size(); - } - - // Fill parent vector - xS.resize(dim); - DenseIndex vectorPos = 0; - for (const VectorValues::const_iterator& parentPointer : parentPointers) { - const Vector& parentVector = parentPointer->second; - xS.block(vectorPos, 0, parentVector.size(), 1) = - parentVector.block(0, 0, parentVector.size(), 1); - vectorPos += parentVector.size(); - } - } - - // NOTE(gareth): We can no longer write: xS = b - S * xS - // This is because Eigen (as of 3.3) no longer evaluates S * xS into - // a temporary, and the operation trashes valus in xS. - // See: http://eigen.tuxfamily.org/index.php?title=3.3 - const Vector rhs = c.getb() - c.get_S() * xS; - const Vector solution = c.get_R().triangularView().solve(rhs); - - // Check for indeterminant solution - if (solution.hasNaN()) - throw IndeterminantLinearSystemException(c.keys().front()); - - // Insert solution into a VectorValues - DenseIndex vectorPosition = 0; - for (GaussianConditional::const_iterator frontal = c.beginFrontals(); - frontal != c.endFrontals(); ++frontal) { - solnPointers_.at(*frontal)->second = - solution.segment(vectorPosition, c.getDim(frontal)); - vectorPosition += c.getDim(frontal); - } - } else { - // Just call plain solve because we couldn't use solution pointers. - delta->update(conditional_->solve(*delta)); - } -#else - delta->update(conditional_->solve(*delta)); -#endif -} - -/* ************************************************************************* */ -bool ISAM2Clique::valuesChanged(const KeySet& replaced, - const Vector& originalValues, - const VectorValues& delta, - double threshold) const { - auto frontals = conditional_->frontals(); - if (replaced.exists(frontals.front())) return true; - auto diff = originalValues - delta.vector(frontals); - return diff.lpNorm() >= threshold; -} - -/* ************************************************************************* */ -/// Set changed flag for each frontal variable -void ISAM2Clique::markFrontalsAsChanged(KeySet* changed) const { - for (Key frontal : conditional_->frontals()) { - changed->insert(frontal); - } -} - -/* ************************************************************************* */ -void ISAM2Clique::restoreFromOriginals(const Vector& originalValues, - VectorValues* delta) const { - size_t pos = 0; - for (Key frontal : conditional_->frontals()) { - auto v = delta->at(frontal); - v = originalValues.segment(pos, v.size()); - pos += v.size(); - } -} - -/* ************************************************************************* */ -// Note: not being used right now in favor of non-recursive version below. -void ISAM2Clique::optimizeWildfire(const KeySet& replaced, double threshold, - KeySet* changed, VectorValues* delta, - size_t* count) const { - if (isDirty(replaced, *changed)) { - // Temporary copy of the original values, to check how much they change - auto originalValues = delta->vector(conditional_->frontals()); - - // Back-substitute - fastBackSubstitute(delta); - count += conditional_->nrFrontals(); - - if (valuesChanged(replaced, originalValues, *delta, threshold)) { - markFrontalsAsChanged(changed); - } else { - restoreFromOriginals(originalValues, delta); - } - - // Recurse to children - for (const auto& child : children) { - child->optimizeWildfire(replaced, threshold, changed, delta, count); - } - } -} - -size_t optimizeWildfire(const ISAM2::sharedClique& root, double threshold, - const KeySet& keys, VectorValues* delta) { - KeySet changed; - size_t count = 0; - // starting from the root, call optimize on each conditional - if (root) root->optimizeWildfire(keys, threshold, &changed, delta, &count); - return count; -} - -/* ************************************************************************* */ -bool ISAM2Clique::optimizeWildfireNode(const KeySet& replaced, double threshold, - KeySet* changed, VectorValues* delta, - size_t* count) const { - // TODO(gareth): This code shares a lot of logic w/ linearAlgorithms-inst, - // potentially refactor - bool dirty = isDirty(replaced, *changed); - if (dirty) { - // Temporary copy of the original values, to check how much they change - auto originalValues = delta->vector(conditional_->frontals()); - - // Back-substitute - fastBackSubstitute(delta); - count += conditional_->nrFrontals(); - - if (valuesChanged(replaced, originalValues, *delta, threshold)) { - markFrontalsAsChanged(changed); - } else { - restoreFromOriginals(originalValues, delta); - } - } - - return dirty; -} - -size_t optimizeWildfireNonRecursive(const ISAM2::sharedClique& root, - double threshold, const KeySet& keys, - VectorValues* delta) { - KeySet changed; - size_t count = 0; - - if (root) { - std::stack travStack; - travStack.push(root); - ISAM2::sharedClique currentNode = root; - while (!travStack.empty()) { - currentNode = travStack.top(); - travStack.pop(); - bool dirty = currentNode->optimizeWildfireNode(keys, threshold, &changed, - delta, &count); - if (dirty) { - for (const auto& child : currentNode->children) { - travStack.push(child); - } - } - } - } - - return count; -} - -/* ************************************************************************* */ -void ISAM2Clique::nnz_internal(size_t* result) const { - size_t dimR = conditional_->rows(); - size_t dimSep = conditional_->get_S().cols(); - *result += ((dimR + 1) * dimR) / 2 + dimSep * dimR; - // traverse the children - for (const auto& child : children) { - child->nnz_internal(result); - } -} - -/* ************************************************************************* */ -size_t ISAM2Clique::calculate_nnz() const { - size_t result = 0; - nnz_internal(&result); - return result; -} - /* ************************************************************************* */ ISAM2::ISAM2(const ISAM2Params& params) : params_(params), update_count_(0) { if (params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) @@ -622,7 +275,7 @@ boost::shared_ptr ISAM2::recalculate( boost::shared_ptr affectedKeysSet( new KeySet()); // Will return this result - if (affectedKeys.size() >= theta_.size() * batchThreshold) { + if (affectedKeys.size() >= theta_.size() * kBatchThreshold) { // Do a batch step - reorder and relinearize all variables gttic(batch); @@ -867,7 +520,7 @@ ISAM2Result ISAM2::update( // Update delta if we need it to check relinearization later if (relinearizeThisStep) { gttic(updateDelta); - updateDelta(disableReordering); + updateDelta(kDisableReordering); gttoc(updateDelta); } @@ -982,7 +635,7 @@ ISAM2Result ISAM2::update( else relinKeys = Impl::CheckRelinearizationFull(delta_, params_.relinearizeThreshold); - if (disableReordering) + if (kDisableReordering) relinKeys = Impl::CheckRelinearizationFull( delta_, 0.0); // This is used for debugging @@ -1485,4 +1138,3 @@ VectorValues ISAM2::gradientAtZero() const { } } // namespace gtsam -/// namespace gtsam diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index eb33038a7..069d90cc8 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -13,593 +13,23 @@ * @file ISAM2.h * @brief Incremental update functionality (ISAM2) for BayesTree, with fluid * relinearization. - * @author Michael Kaess, Richard Roberts + * @author Michael Kaess, Richard Roberts, Frank Dellaert */ // \callgraph #pragma once -#include +#include +#include +#include +#include +#include + #include -#include -#include -#include - -#include - namespace gtsam { -/** - * @addtogroup ISAM2 - * Parameters for ISAM2 using Gauss-Newton optimization. Either this class or - * ISAM2DoglegParams should be specified as the optimizationParams in - * ISAM2Params, which should in turn be passed to ISAM2(const ISAM2Params&). - */ -struct GTSAM_EXPORT ISAM2GaussNewtonParams { - double - wildfireThreshold; ///< Continue updating the linear delta only when - ///< changes are above this threshold (default: 0.001) - - /** Specify parameters as constructor arguments */ - ISAM2GaussNewtonParams( - double _wildfireThreshold = - 0.001 ///< see ISAM2GaussNewtonParams public variables, - ///< ISAM2GaussNewtonParams::wildfireThreshold - ) - : wildfireThreshold(_wildfireThreshold) {} - - void print(const std::string str = "") const { - using std::cout; - cout << str << "type: ISAM2GaussNewtonParams\n"; - cout << str << "wildfireThreshold: " << wildfireThreshold << "\n"; - cout.flush(); - } - - double getWildfireThreshold() const { return wildfireThreshold; } - void setWildfireThreshold(double wildfireThreshold) { - this->wildfireThreshold = wildfireThreshold; - } -}; - -/** - * @addtogroup ISAM2 - * Parameters for ISAM2 using Dogleg optimization. Either this class or - * ISAM2GaussNewtonParams should be specified as the optimizationParams in - * ISAM2Params, which should in turn be passed to ISAM2(const ISAM2Params&). - */ -struct GTSAM_EXPORT ISAM2DoglegParams { - double initialDelta; ///< The initial trust region radius for Dogleg - double - wildfireThreshold; ///< Continue updating the linear delta only when - ///< changes are above this threshold (default: 1e-5) - DoglegOptimizerImpl::TrustRegionAdaptationMode - adaptationMode; ///< See description in - ///< DoglegOptimizerImpl::TrustRegionAdaptationMode - bool - verbose; ///< Whether Dogleg prints iteration and convergence information - - /** Specify parameters as constructor arguments */ - ISAM2DoglegParams( - double _initialDelta = 1.0, ///< see ISAM2DoglegParams::initialDelta - double _wildfireThreshold = - 1e-5, ///< see ISAM2DoglegParams::wildfireThreshold - DoglegOptimizerImpl::TrustRegionAdaptationMode _adaptationMode = - DoglegOptimizerImpl:: - SEARCH_EACH_ITERATION, ///< see ISAM2DoglegParams::adaptationMode - bool _verbose = false ///< see ISAM2DoglegParams::verbose - ) - : initialDelta(_initialDelta), - wildfireThreshold(_wildfireThreshold), - adaptationMode(_adaptationMode), - verbose(_verbose) {} - - void print(const std::string str = "") const { - using std::cout; - cout << str << "type: ISAM2DoglegParams\n"; - cout << str << "initialDelta: " << initialDelta << "\n"; - cout << str << "wildfireThreshold: " << wildfireThreshold << "\n"; - cout << str - << "adaptationMode: " << adaptationModeTranslator(adaptationMode) - << "\n"; - cout.flush(); - } - - double getInitialDelta() const { return initialDelta; } - double getWildfireThreshold() const { return wildfireThreshold; } - std::string getAdaptationMode() const { - return adaptationModeTranslator(adaptationMode); - } - bool isVerbose() const { return verbose; } - void setInitialDelta(double initialDelta) { - this->initialDelta = initialDelta; - } - void setWildfireThreshold(double wildfireThreshold) { - this->wildfireThreshold = wildfireThreshold; - } - void setAdaptationMode(const std::string& adaptationMode) { - this->adaptationMode = adaptationModeTranslator(adaptationMode); - } - void setVerbose(bool verbose) { this->verbose = verbose; } - - std::string adaptationModeTranslator( - const DoglegOptimizerImpl::TrustRegionAdaptationMode& adaptationMode) - const; - DoglegOptimizerImpl::TrustRegionAdaptationMode adaptationModeTranslator( - const std::string& adaptationMode) const; -}; - -/** - * @addtogroup ISAM2 - * Parameters for the ISAM2 algorithm. Default parameter values are listed - * below. - */ -typedef FastMap ISAM2ThresholdMap; -typedef ISAM2ThresholdMap::value_type ISAM2ThresholdMapValue; -struct GTSAM_EXPORT ISAM2Params { - typedef boost::variant - OptimizationParams; ///< Either ISAM2GaussNewtonParams or - ///< ISAM2DoglegParams - typedef boost::variant > - RelinearizationThreshold; ///< Either a constant relinearization - ///< threshold or a per-variable-type set of - ///< thresholds - - /** Optimization parameters, this both selects the nonlinear optimization - * method and specifies its parameters, either ISAM2GaussNewtonParams or - * ISAM2DoglegParams. In the former, Gauss-Newton optimization will be used - * with the specified parameters, and in the latter Powell's dog-leg - * algorithm will be used with the specified parameters. - */ - OptimizationParams optimizationParams; - - /** Only relinearize variables whose linear delta magnitude is greater than - * this threshold (default: 0.1). If this is a FastMap instead - * of a double, then the threshold is specified for each dimension of each - * variable type. This parameter then maps from a character indicating the - * variable type to a Vector of thresholds for each dimension of that - * variable. For example, if Pose keys are of type TypedSymbol<'x',Pose3>, - * and landmark keys are of type TypedSymbol<'l',Point3>, then appropriate - * entries would be added with: - * \code - FastMap thresholds; - thresholds['x'] = (Vector(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5).finished(); - // 0.1 rad rotation threshold, 0.5 m translation threshold thresholds['l'] = - Vector3(1.0, 1.0, 1.0); // 1.0 m landmark position threshold - params.relinearizeThreshold = thresholds; - \endcode - */ - RelinearizationThreshold relinearizeThreshold; - - int relinearizeSkip; ///< Only relinearize any variables every - ///< relinearizeSkip calls to ISAM2::update (default: - ///< 10) - - bool enableRelinearization; ///< Controls whether ISAM2 will ever relinearize - ///< any variables (default: true) - - bool evaluateNonlinearError; ///< Whether to evaluate the nonlinear error - ///< before and after the update, to return in - ///< ISAM2Result from update() - - enum Factorization { CHOLESKY, QR }; - /** Specifies whether to use QR or CHOESKY numerical factorization (default: - * CHOLESKY). Cholesky is faster but potentially numerically unstable for - * poorly-conditioned problems, which can occur when uncertainty is very low - * in some variables (or dimensions of variables) and very high in others. QR - * is slower but more numerically stable in poorly-conditioned problems. We - * suggest using the default of Cholesky unless gtsam sometimes throws - * IndefiniteLinearSystemException when your problem's Hessian is actually - * positive definite. For positive definite problems, numerical error - * accumulation can cause the problem to become numerically negative or - * indefinite as solving proceeds, especially when using Cholesky. - */ - Factorization factorization; - - /** Whether to cache linear factors (default: true). - * This can improve performance if linearization is expensive, but can hurt - * performance if linearization is very cleap due to computation to look up - * additional keys. - */ - bool cacheLinearizedFactors; - - KeyFormatter - keyFormatter; ///< A KeyFormatter for when keys are printed during - ///< debugging (default: DefaultKeyFormatter) - - bool enableDetailedResults; ///< Whether to compute and return - ///< ISAM2Result::detailedResults, this can - ///< increase running time (default: false) - - /** Check variables for relinearization in tree-order, stopping the check once - * a variable does not need to be relinearized (default: false). This can - * improve speed by only checking a small part of the top of the tree. - * However, variables below the check cut-off can accumulate significant - * deltas without triggering relinearization. This is particularly useful in - * exploration scenarios where real-time performance is desired over - * correctness. Use with caution. - */ - bool enablePartialRelinearizationCheck; - - /// When you will be removing many factors, e.g. when using ISAM2 as a - /// fixed-lag smoother, enable this option to add factors in the first - /// available factor slots, to avoid accumulating NULL factor slots, at the - /// cost of having to search for slots every time a factor is added. - bool findUnusedFactorSlots; - - /** - * Specify parameters as constructor arguments - * See the documentation of member variables above. - */ - ISAM2Params(OptimizationParams _optimizationParams = ISAM2GaussNewtonParams(), - RelinearizationThreshold _relinearizeThreshold = 0.1, - int _relinearizeSkip = 10, bool _enableRelinearization = true, - bool _evaluateNonlinearError = false, - Factorization _factorization = ISAM2Params::CHOLESKY, - bool _cacheLinearizedFactors = true, - const KeyFormatter& _keyFormatter = - DefaultKeyFormatter ///< see ISAM2::Params::keyFormatter - ) - : optimizationParams(_optimizationParams), - relinearizeThreshold(_relinearizeThreshold), - relinearizeSkip(_relinearizeSkip), - enableRelinearization(_enableRelinearization), - evaluateNonlinearError(_evaluateNonlinearError), - factorization(_factorization), - cacheLinearizedFactors(_cacheLinearizedFactors), - keyFormatter(_keyFormatter), - enableDetailedResults(false), - enablePartialRelinearizationCheck(false), - findUnusedFactorSlots(false) {} - - /// print iSAM2 parameters - void print(const std::string& str = "") const { - using std::cout; - cout << str << "\n"; - - static const std::string kStr("optimizationParams: "); - if (optimizationParams.type() == typeid(ISAM2GaussNewtonParams)) - boost::get(optimizationParams).print(); - else if (optimizationParams.type() == typeid(ISAM2DoglegParams)) - boost::get(optimizationParams).print(kStr); - else - cout << kStr << "{unknown type}\n"; - - cout << "relinearizeThreshold: "; - if (relinearizeThreshold.type() == typeid(double)) { - cout << boost::get(relinearizeThreshold) << "\n"; - } else { - cout << "{mapped}\n"; - for (const ISAM2ThresholdMapValue& value : - boost::get(relinearizeThreshold)) { - cout << " '" << value.first - << "' -> [" << value.second.transpose() << " ]\n"; - } - } - - cout << "relinearizeSkip: " << relinearizeSkip << "\n"; - cout << "enableRelinearization: " << enableRelinearization - << "\n"; - cout << "evaluateNonlinearError: " << evaluateNonlinearError - << "\n"; - cout << "factorization: " - << factorizationTranslator(factorization) << "\n"; - cout << "cacheLinearizedFactors: " << cacheLinearizedFactors - << "\n"; - cout << "enableDetailedResults: " << enableDetailedResults - << "\n"; - cout << "enablePartialRelinearizationCheck: " - << enablePartialRelinearizationCheck << "\n"; - cout << "findUnusedFactorSlots: " << findUnusedFactorSlots - << "\n"; - cout.flush(); - } - - /// @name Getters and Setters for all properties - /// @{ - - OptimizationParams getOptimizationParams() const { - return this->optimizationParams; - } - RelinearizationThreshold getRelinearizeThreshold() const { - return relinearizeThreshold; - } - int getRelinearizeSkip() const { return relinearizeSkip; } - bool isEnableRelinearization() const { return enableRelinearization; } - bool isEvaluateNonlinearError() const { return evaluateNonlinearError; } - std::string getFactorization() const { - return factorizationTranslator(factorization); - } - bool isCacheLinearizedFactors() const { return cacheLinearizedFactors; } - KeyFormatter getKeyFormatter() const { return keyFormatter; } - bool isEnableDetailedResults() const { return enableDetailedResults; } - bool isEnablePartialRelinearizationCheck() const { - return enablePartialRelinearizationCheck; - } - - void setOptimizationParams(OptimizationParams optimizationParams) { - this->optimizationParams = optimizationParams; - } - void setRelinearizeThreshold(RelinearizationThreshold relinearizeThreshold) { - this->relinearizeThreshold = relinearizeThreshold; - } - void setRelinearizeSkip(int relinearizeSkip) { - this->relinearizeSkip = relinearizeSkip; - } - void setEnableRelinearization(bool enableRelinearization) { - this->enableRelinearization = enableRelinearization; - } - void setEvaluateNonlinearError(bool evaluateNonlinearError) { - this->evaluateNonlinearError = evaluateNonlinearError; - } - void setFactorization(const std::string& factorization) { - this->factorization = factorizationTranslator(factorization); - } - void setCacheLinearizedFactors(bool cacheLinearizedFactors) { - this->cacheLinearizedFactors = cacheLinearizedFactors; - } - void setKeyFormatter(KeyFormatter keyFormatter) { - this->keyFormatter = keyFormatter; - } - void setEnableDetailedResults(bool enableDetailedResults) { - this->enableDetailedResults = enableDetailedResults; - } - void setEnablePartialRelinearizationCheck( - bool enablePartialRelinearizationCheck) { - this->enablePartialRelinearizationCheck = enablePartialRelinearizationCheck; - } - - GaussianFactorGraph::Eliminate getEliminationFunction() const { - return factorization == CHOLESKY - ? (GaussianFactorGraph::Eliminate)EliminatePreferCholesky - : (GaussianFactorGraph::Eliminate)EliminateQR; - } - - /// @} - - /// @name Some utilities - /// @{ - - static Factorization factorizationTranslator(const std::string& str); - static std::string factorizationTranslator(const Factorization& value); - - /// @} -}; - -typedef FastVector FactorIndices; - -/** - * @addtogroup ISAM2 - * This struct is returned from ISAM2::update() and contains information about - * the update that is useful for determining whether the solution is - * converging, and about how much work was required for the update. See member - * variables for details and information about each entry. - */ -struct GTSAM_EXPORT ISAM2Result { - /** The nonlinear error of all of the factors, \a including new factors and - * variables added during the current call to ISAM2::update(). This error is - * calculated using the following variable values: - * \li Pre-existing variables will be evaluated by combining their - * linearization point before this call to update, with their partial linear - * delta, as computed by ISAM2::calculateEstimate(). - * \li New variables will be evaluated at their initialization points passed - * into the current call to update. - * \par Note: This will only be computed if - * ISAM2Params::evaluateNonlinearError is set to \c true, because there is - * some cost to this computation. - */ - boost::optional errorBefore; - - /** The nonlinear error of all of the factors computed after the current - * update, meaning that variables above the relinearization threshold - * (ISAM2Params::relinearizeThreshold) have been relinearized and new - * variables have undergone one linear update. Variable values are - * again computed by combining their linearization points with their - * partial linear deltas, by ISAM2::calculateEstimate(). - * \par Note: This will only be computed if - * ISAM2Params::evaluateNonlinearError is set to \c true, because there is - * some cost to this computation. - */ - boost::optional errorAfter; - - /** The number of variables that were relinearized because their linear - * deltas exceeded the reslinearization threshold - * (ISAM2Params::relinearizeThreshold), combined with any additional - * variables that had to be relinearized because they were involved in - * the same factor as a variable above the relinearization threshold. - * On steps where no relinearization is considered - * (see ISAM2Params::relinearizeSkip), this count will be zero. - */ - size_t variablesRelinearized; - - /** The number of variables that were reeliminated as parts of the Bayes' - * Tree were recalculated, due to new factors. When loop closures occur, - * this count will be large as the new loop-closing factors will tend to - * involve variables far away from the root, and everything up to the root - * will be reeliminated. - */ - size_t variablesReeliminated; - - /** The number of factors that were included in reelimination of the Bayes' - * tree. */ - size_t factorsRecalculated; - - /** The number of cliques in the Bayes' Tree */ - size_t cliques; - - /** The indices of the newly-added factors, in 1-to-1 correspondence with the - * factors passed as \c newFactors to ISAM2::update(). These indices may be - * used later to refer to the factors in order to remove them. - */ - FactorIndices newFactorsIndices; - - /** A struct holding detailed results, which must be enabled with - * ISAM2Params::enableDetailedResults. - */ - struct DetailedResults { - /** The status of a single variable, this struct is stored in - * DetailedResults::variableStatus */ - struct VariableStatus { - /** Whether the variable was just reeliminated, due to being relinearized, - * observed, new, or on the path up to the root clique from another - * reeliminated variable. */ - bool isReeliminated; - bool isAboveRelinThreshold; ///< Whether the variable was just - ///< relinearized due to being above the - ///< relinearization threshold - bool isRelinearizeInvolved; ///< Whether the variable was below the - ///< relinearization threshold but was - ///< relinearized by being involved in a - ///< factor with a variable above the - ///< relinearization threshold - bool isRelinearized; /// Whether the variable was relinearized, either by - /// being above the relinearization threshold or by - /// involvement. - bool isObserved; ///< Whether the variable was just involved in new - ///< factors - bool isNew; ///< Whether the variable itself was just added - bool inRootClique; ///< Whether the variable is in the root clique - VariableStatus() - : isReeliminated(false), - isAboveRelinThreshold(false), - isRelinearizeInvolved(false), - isRelinearized(false), - isObserved(false), - isNew(false), - inRootClique(false) {} - }; - - /** The status of each variable during this update, see VariableStatus. - */ - FastMap variableStatus; - }; - - /** Detailed results, if enabled by ISAM2Params::enableDetailedResults. See - * Detail for information about the results data stored here. */ - boost::optional detail; - - void print(const std::string str = "") const { - using std::cout; - cout << str << " Reelimintated: " << variablesReeliminated - << " Relinearized: " << variablesRelinearized - << " Cliques: " << cliques << std::endl; - } - - /** Getters and Setters */ - size_t getVariablesRelinearized() const { return variablesRelinearized; } - size_t getVariablesReeliminated() const { return variablesReeliminated; } - size_t getCliques() const { return cliques; } -}; - -/** - * Specialized Clique structure for ISAM2, incorporating caching and gradient - * contribution - * TODO: more documentation - */ -class GTSAM_EXPORT ISAM2Clique - : public BayesTreeCliqueBase { - public: - typedef ISAM2Clique This; - typedef BayesTreeCliqueBase Base; - typedef boost::shared_ptr shared_ptr; - typedef boost::weak_ptr weak_ptr; - typedef GaussianConditional ConditionalType; - typedef ConditionalType::shared_ptr sharedConditional; - - Base::FactorType::shared_ptr cachedFactor_; - Vector gradientContribution_; -#ifdef USE_BROKEN_FAST_BACKSUBSTITUTE - mutable FastMap solnPointers_; -#endif - - /// Default constructor - ISAM2Clique() : Base() {} - - /// Copy constructor, does *not* copy solution pointers as these are invalid - /// in different trees. - ISAM2Clique(const ISAM2Clique& other) - : Base(other), - cachedFactor_(other.cachedFactor_), - gradientContribution_(other.gradientContribution_) {} - - /// Assignment operator, does *not* copy solution pointers as these are - /// invalid in different trees. - ISAM2Clique& operator=(const ISAM2Clique& other) { - Base::operator=(other); - cachedFactor_ = other.cachedFactor_; - gradientContribution_ = other.gradientContribution_; - return *this; - } - - /// Overridden to also store the remaining factor and gradient contribution - void setEliminationResult( - const FactorGraphType::EliminationResult& eliminationResult); - - /** Access the cached factor */ - Base::FactorType::shared_ptr& cachedFactor() { return cachedFactor_; } - - /** Access the gradient contribution */ - const Vector& gradientContribution() const { return gradientContribution_; } - - bool equals(const This& other, double tol = 1e-9) const; - - /** print this node */ - void print(const std::string& s = "", - const KeyFormatter& formatter = DefaultKeyFormatter) const; - - void optimizeWildfire(const KeySet& replaced, double threshold, - KeySet* changed, VectorValues* delta, - size_t* count) const; - - bool optimizeWildfireNode(const KeySet& replaced, double threshold, - KeySet* changed, VectorValues* delta, - size_t* count) const; - - /** - * Starting from the root, add up entries of frontal and conditional matrices - * of each conditional - */ - void nnz_internal(size_t* result) const; - size_t calculate_nnz() const; - - private: - /** - * Check if clique was replaced, or if any parents were changed above the - * threshold or themselves replaced. - */ - bool isDirty(const KeySet& replaced, const KeySet& changed) const; - - /** - * Back-substitute - special version stores solution pointers in cliques for - * fast access. - */ - void fastBackSubstitute(VectorValues* delta) const; - - /* - * Check whether the values changed above a threshold, or always true if the - * clique was replaced. - */ - bool valuesChanged(const KeySet& replaced, const Vector& originalValues, - const VectorValues& delta, double threshold) const; - - /// Set changed flag for each frontal variable - void markFrontalsAsChanged(KeySet* changed) const; - - /// Restore delta to original values, guided by frontal keys. - void restoreFromOriginals(const Vector& originalValues, - VectorValues* delta) const; - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE& ar, const unsigned int /*version*/) { - ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar& BOOST_SERIALIZATION_NVP(cachedFactor_); - ar& BOOST_SERIALIZATION_NVP(gradientContribution_); - } -}; // \struct ISAM2Clique - /** * @addtogroup ISAM2 * Implementation of the full ISAM2 algorithm for incremental nonlinear @@ -643,7 +73,7 @@ class GTSAM_EXPORT ISAM2 : public BayesTree { * until it is needed. */ mutable KeySet - deltaReplacedMask_; // TODO: Make sure accessed in the right way + deltaReplacedMask_; // TODO(dellaert): Make sure accessed in the right way /** All original nonlinear factors are stored here to use during * relinearization */ @@ -768,7 +198,11 @@ class GTSAM_EXPORT ISAM2 : public BayesTree { * @return */ template - VALUE calculateEstimate(Key key) const; + VALUE calculateEstimate(Key key) const { + const Vector& delta = getDelta()[key]; + return traits::Retract(theta_.at(key), delta); + } + /** Compute an estimate for a single variable using its incomplete linear * delta computed during the last update. This is faster than calling the @@ -846,32 +280,12 @@ class GTSAM_EXPORT ISAM2 : public BayesTree { const boost::optional >& constrainKeys, ISAM2Result& result); void updateDelta(bool forceFullSolve = false) const; - }; // ISAM2 /// traits template <> struct traits : public Testable {}; -/** - * Optimize the BayesTree, starting from the root. - * @param threshold The maximum change against the PREVIOUS delta for - * non-replaced variables that can be ignored, ie. the old delta entry is kept - * and recursive backsubstitution might eventually stop if none of the changed - * variables are contained in the subtree. - * @param replaced Needs to contain all variables that are contained in the top - * of the Bayes tree that has been redone. - * @return The number of variables that were solved for. - * @param delta The current solution, an offset from the linearization point. - */ -size_t optimizeWildfire(const ISAM2::sharedClique& root, double threshold, - const KeySet& replaced, VectorValues* delta); - -size_t optimizeWildfireNonRecursive(const ISAM2::sharedClique& root, - double threshold, const KeySet& replaced, - VectorValues* delta); - } // namespace gtsam #include -#include diff --git a/gtsam/nonlinear/ISAM2Clique.cpp b/gtsam/nonlinear/ISAM2Clique.cpp new file mode 100644 index 000000000..6e03b0680 --- /dev/null +++ b/gtsam/nonlinear/ISAM2Clique.cpp @@ -0,0 +1,302 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file ISAM2Clique.cpp + * @brief Specialized iSAM2 Clique + * @author Michael Kaess, Richard Roberts, Frank Dellaert + */ + +#include +#include +#include +#include +#include + +using namespace std; + +namespace gtsam { + +// Instantiate base class +template class BayesTreeCliqueBase; + +/* ************************************************************************* */ +void ISAM2Clique::setEliminationResult( + const FactorGraphType::EliminationResult& eliminationResult) { + conditional_ = eliminationResult.first; + cachedFactor_ = eliminationResult.second; + // Compute gradient contribution + gradientContribution_.resize(conditional_->cols() - 1); + // Rewrite -(R * P')'*d as -(d' * R * P')' for computational speed + // reasons + gradientContribution_ << -conditional_->get_R().transpose() * + conditional_->get_d(), + -conditional_->get_S().transpose() * conditional_->get_d(); +} + +/* ************************************************************************* */ +bool ISAM2Clique::equals(const This& other, double tol) const { + return Base::equals(other) && + ((!cachedFactor_ && !other.cachedFactor_) || + (cachedFactor_ && other.cachedFactor_ && + cachedFactor_->equals(*other.cachedFactor_, tol))); +} + +/* ************************************************************************* */ +void ISAM2Clique::print(const string& s, const KeyFormatter& formatter) const { + Base::print(s, formatter); + if (cachedFactor_) + cachedFactor_->print(s + "Cached: ", formatter); + else + cout << s << "Cached empty" << endl; + if (gradientContribution_.rows() != 0) + gtsam::print(gradientContribution_, "Gradient contribution: "); +} + +/* ************************************************************************* */ +bool ISAM2Clique::isDirty(const KeySet& replaced, const KeySet& changed) const { + // if none of the variables in this clique (frontal and separator!) changed + // significantly, then by the running intersection property, none of the + // cliques in the children need to be processed + + // Are any clique variables part of the tree that has been redone? + bool dirty = replaced.exists(conditional_->frontals().front()); +#if !defined(NDEBUG) && defined(GTSAM_EXTRA_CONSISTENCY_CHECKS) + for (Key frontal : conditional_->frontals()) { + assert(dirty == replaced.exists(frontal)); + } +#endif + + // If not, then has one of the separator variables changed significantly? + if (!dirty) { + for (Key parent : conditional_->parents()) { + if (changed.exists(parent)) { + dirty = true; + break; + } + } + } + return dirty; +} + +/* ************************************************************************* */ +/** + * Back-substitute - special version stores solution pointers in cliques for + * fast access. + */ +void ISAM2Clique::fastBackSubstitute(VectorValues* delta) const { +#ifdef USE_BROKEN_FAST_BACKSUBSTITUTE + // TODO(gareth): This code shares a lot of logic w/ linearAlgorithms-inst, + // potentially refactor + + // Create solution part pointers if necessary and possible - necessary if + // solnPointers_ is empty, and possible if either we're a root, or we have + // a parent with valid solnPointers_. + ISAM2Clique::shared_ptr parent = parent_.lock(); + if (solnPointers_.empty() && (isRoot() || !parent->solnPointers_.empty())) { + for (Key frontal : conditional_->frontals()) + solnPointers_.emplace(frontal, delta->find(frontal)); + for (Key parentKey : conditional_->parents()) { + assert(parent->solnPointers_.exists(parentKey)); + solnPointers_.emplace(parentKey, parent->solnPointers_.at(parentKey)); + } + } + + // See if we can use solution part pointers - we can if they either + // already existed or were created above. + if (!solnPointers_.empty()) { + GaussianConditional& c = *conditional_; + // Solve matrix + Vector xS; + { + // Count dimensions of vector + DenseIndex dim = 0; + FastVector parentPointers; + parentPointers.reserve(conditional_->nrParents()); + for (Key parent : conditional_->parents()) { + parentPointers.push_back(solnPointers_.at(parent)); + dim += parentPointers.back()->second.size(); + } + + // Fill parent vector + xS.resize(dim); + DenseIndex vectorPos = 0; + for (const VectorValues::const_iterator& parentPointer : parentPointers) { + const Vector& parentVector = parentPointer->second; + xS.block(vectorPos, 0, parentVector.size(), 1) = + parentVector.block(0, 0, parentVector.size(), 1); + vectorPos += parentVector.size(); + } + } + + // NOTE(gareth): We can no longer write: xS = b - S * xS + // This is because Eigen (as of 3.3) no longer evaluates S * xS into + // a temporary, and the operation trashes valus in xS. + // See: http://eigen.tuxfamily.org/index.php?title=3.3 + const Vector rhs = c.getb() - c.get_S() * xS; + const Vector solution = c.get_R().triangularView().solve(rhs); + + // Check for indeterminant solution + if (solution.hasNaN()) + throw IndeterminantLinearSystemException(c.keys().front()); + + // Insert solution into a VectorValues + DenseIndex vectorPosition = 0; + for (GaussianConditional::const_iterator frontal = c.beginFrontals(); + frontal != c.endFrontals(); ++frontal) { + solnPointers_.at(*frontal)->second = + solution.segment(vectorPosition, c.getDim(frontal)); + vectorPosition += c.getDim(frontal); + } + } else { + // Just call plain solve because we couldn't use solution pointers. + delta->update(conditional_->solve(*delta)); + } +#else + delta->update(conditional_->solve(*delta)); +#endif +} + +/* ************************************************************************* */ +bool ISAM2Clique::valuesChanged(const KeySet& replaced, + const Vector& originalValues, + const VectorValues& delta, + double threshold) const { + auto frontals = conditional_->frontals(); + if (replaced.exists(frontals.front())) return true; + auto diff = originalValues - delta.vector(frontals); + return diff.lpNorm() >= threshold; +} + +/* ************************************************************************* */ +/// Set changed flag for each frontal variable +void ISAM2Clique::markFrontalsAsChanged(KeySet* changed) const { + for (Key frontal : conditional_->frontals()) { + changed->insert(frontal); + } +} + +/* ************************************************************************* */ +void ISAM2Clique::restoreFromOriginals(const Vector& originalValues, + VectorValues* delta) const { + size_t pos = 0; + for (Key frontal : conditional_->frontals()) { + auto v = delta->at(frontal); + v = originalValues.segment(pos, v.size()); + pos += v.size(); + } +} + +/* ************************************************************************* */ +// Note: not being used right now in favor of non-recursive version below. +void ISAM2Clique::optimizeWildfire(const KeySet& replaced, double threshold, + KeySet* changed, VectorValues* delta, + size_t* count) const { + if (isDirty(replaced, *changed)) { + // Temporary copy of the original values, to check how much they change + auto originalValues = delta->vector(conditional_->frontals()); + + // Back-substitute + fastBackSubstitute(delta); + count += conditional_->nrFrontals(); + + if (valuesChanged(replaced, originalValues, *delta, threshold)) { + markFrontalsAsChanged(changed); + } else { + restoreFromOriginals(originalValues, delta); + } + + // Recurse to children + for (const auto& child : children) { + child->optimizeWildfire(replaced, threshold, changed, delta, count); + } + } +} + +size_t optimizeWildfire(const ISAM2Clique::shared_ptr& root, double threshold, + const KeySet& keys, VectorValues* delta) { + KeySet changed; + size_t count = 0; + // starting from the root, call optimize on each conditional + if (root) root->optimizeWildfire(keys, threshold, &changed, delta, &count); + return count; +} + +/* ************************************************************************* */ +bool ISAM2Clique::optimizeWildfireNode(const KeySet& replaced, double threshold, + KeySet* changed, VectorValues* delta, + size_t* count) const { + // TODO(gareth): This code shares a lot of logic w/ linearAlgorithms-inst, + // potentially refactor + bool dirty = isDirty(replaced, *changed); + if (dirty) { + // Temporary copy of the original values, to check how much they change + auto originalValues = delta->vector(conditional_->frontals()); + + // Back-substitute + fastBackSubstitute(delta); + count += conditional_->nrFrontals(); + + if (valuesChanged(replaced, originalValues, *delta, threshold)) { + markFrontalsAsChanged(changed); + } else { + restoreFromOriginals(originalValues, delta); + } + } + + return dirty; +} + +size_t optimizeWildfireNonRecursive(const ISAM2Clique::shared_ptr& root, + double threshold, const KeySet& keys, + VectorValues* delta) { + KeySet changed; + size_t count = 0; + + if (root) { + std::stack travStack; + travStack.push(root); + ISAM2Clique::shared_ptr currentNode = root; + while (!travStack.empty()) { + currentNode = travStack.top(); + travStack.pop(); + bool dirty = currentNode->optimizeWildfireNode(keys, threshold, &changed, + delta, &count); + if (dirty) { + for (const auto& child : currentNode->children) { + travStack.push(child); + } + } + } + } + + return count; +} + +/* ************************************************************************* */ +void ISAM2Clique::nnz_internal(size_t* result) const { + size_t dimR = conditional_->rows(); + size_t dimSep = conditional_->get_S().cols(); + *result += ((dimR + 1) * dimR) / 2 + dimSep * dimR; + // traverse the children + for (const auto& child : children) { + child->nnz_internal(result); + } +} + +/* ************************************************************************* */ +size_t ISAM2Clique::calculate_nnz() const { + size_t result = 0; + nnz_internal(&result); + return result; +} + +} // namespace gtsam diff --git a/gtsam/nonlinear/ISAM2Clique.h b/gtsam/nonlinear/ISAM2Clique.h new file mode 100644 index 000000000..fecc8d335 --- /dev/null +++ b/gtsam/nonlinear/ISAM2Clique.h @@ -0,0 +1,157 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file ISAM2Clique.h + * @brief Specialized iSAM2 Clique + * @author Michael Kaess, Richard Roberts + */ + +// \callgraph + +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace gtsam { + +/** + * Specialized Clique structure for ISAM2, incorporating caching and gradient + * contribution + * TODO: more documentation + */ +class GTSAM_EXPORT ISAM2Clique + : public BayesTreeCliqueBase { + public: + typedef ISAM2Clique This; + typedef BayesTreeCliqueBase Base; + typedef boost::shared_ptr shared_ptr; + typedef boost::weak_ptr weak_ptr; + typedef GaussianConditional ConditionalType; + typedef ConditionalType::shared_ptr sharedConditional; + + Base::FactorType::shared_ptr cachedFactor_; + Vector gradientContribution_; +#ifdef USE_BROKEN_FAST_BACKSUBSTITUTE + mutable FastMap solnPointers_; +#endif + + /// Default constructor + ISAM2Clique() : Base() {} + + /// Copy constructor, does *not* copy solution pointers as these are invalid + /// in different trees. + ISAM2Clique(const ISAM2Clique& other) + : Base(other), + cachedFactor_(other.cachedFactor_), + gradientContribution_(other.gradientContribution_) {} + + /// Assignment operator, does *not* copy solution pointers as these are + /// invalid in different trees. + ISAM2Clique& operator=(const ISAM2Clique& other) { + Base::operator=(other); + cachedFactor_ = other.cachedFactor_; + gradientContribution_ = other.gradientContribution_; + return *this; + } + + /// Overridden to also store the remaining factor and gradient contribution + void setEliminationResult( + const FactorGraphType::EliminationResult& eliminationResult); + + /** Access the cached factor */ + Base::FactorType::shared_ptr& cachedFactor() { return cachedFactor_; } + + /** Access the gradient contribution */ + const Vector& gradientContribution() const { return gradientContribution_; } + + bool equals(const This& other, double tol = 1e-9) const; + + /** print this node */ + void print(const std::string& s = "", + const KeyFormatter& formatter = DefaultKeyFormatter) const; + + void optimizeWildfire(const KeySet& replaced, double threshold, + KeySet* changed, VectorValues* delta, + size_t* count) const; + + bool optimizeWildfireNode(const KeySet& replaced, double threshold, + KeySet* changed, VectorValues* delta, + size_t* count) const; + + /** + * Starting from the root, add up entries of frontal and conditional matrices + * of each conditional + */ + void nnz_internal(size_t* result) const; + size_t calculate_nnz() const; + + private: + /** + * Check if clique was replaced, or if any parents were changed above the + * threshold or themselves replaced. + */ + bool isDirty(const KeySet& replaced, const KeySet& changed) const; + + /** + * Back-substitute - special version stores solution pointers in cliques for + * fast access. + */ + void fastBackSubstitute(VectorValues* delta) const; + + /* + * Check whether the values changed above a threshold, or always true if the + * clique was replaced. + */ + bool valuesChanged(const KeySet& replaced, const Vector& originalValues, + const VectorValues& delta, double threshold) const; + + /// Set changed flag for each frontal variable + void markFrontalsAsChanged(KeySet* changed) const; + + /// Restore delta to original values, guided by frontal keys. + void restoreFromOriginals(const Vector& originalValues, + VectorValues* delta) const; + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar& BOOST_SERIALIZATION_NVP(cachedFactor_); + ar& BOOST_SERIALIZATION_NVP(gradientContribution_); + } +}; // \struct ISAM2Clique + +/** + * Optimize the BayesTree, starting from the root. + * @param threshold The maximum change against the PREVIOUS delta for + * non-replaced variables that can be ignored, ie. the old delta entry is kept + * and recursive backsubstitution might eventually stop if none of the changed + * variables are contained in the subtree. + * @param replaced Needs to contain all variables that are contained in the top + * of the Bayes tree that has been redone. + * @return The number of variables that were solved for. + * @param delta The current solution, an offset from the linearization point. + */ +size_t optimizeWildfire(const ISAM2Clique::shared_ptr& root, double threshold, + const KeySet& replaced, VectorValues* delta); + +size_t optimizeWildfireNonRecursive(const ISAM2Clique::shared_ptr& root, + double threshold, const KeySet& replaced, + VectorValues* delta); + +} // namespace gtsam diff --git a/gtsam/nonlinear/ISAM2Params.cpp b/gtsam/nonlinear/ISAM2Params.cpp new file mode 100644 index 000000000..c768ce427 --- /dev/null +++ b/gtsam/nonlinear/ISAM2Params.cpp @@ -0,0 +1,89 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file ISAM2Params.cpp + * @brief Parameters for iSAM 2. + * @author Michael Kaess, Richard Roberts, Frank Dellaert + */ + +#include +#include + +using namespace std; + +namespace gtsam { + +/* ************************************************************************* */ +string ISAM2DoglegParams::adaptationModeTranslator( + const DoglegOptimizerImpl::TrustRegionAdaptationMode& adaptationMode) + const { + string s; + switch (adaptationMode) { + case DoglegOptimizerImpl::SEARCH_EACH_ITERATION: + s = "SEARCH_EACH_ITERATION"; + break; + case DoglegOptimizerImpl::ONE_STEP_PER_ITERATION: + s = "ONE_STEP_PER_ITERATION"; + break; + default: + s = "UNDEFINED"; + break; + } + return s; +} + +/* ************************************************************************* */ +DoglegOptimizerImpl::TrustRegionAdaptationMode +ISAM2DoglegParams::adaptationModeTranslator( + const string& adaptationMode) const { + string s = adaptationMode; + boost::algorithm::to_upper(s); + if (s == "SEARCH_EACH_ITERATION") + return DoglegOptimizerImpl::SEARCH_EACH_ITERATION; + if (s == "ONE_STEP_PER_ITERATION") + return DoglegOptimizerImpl::ONE_STEP_PER_ITERATION; + + /* default is SEARCH_EACH_ITERATION */ + return DoglegOptimizerImpl::SEARCH_EACH_ITERATION; +} + +/* ************************************************************************* */ +ISAM2Params::Factorization ISAM2Params::factorizationTranslator( + const string& str) { + string s = str; + boost::algorithm::to_upper(s); + if (s == "QR") return ISAM2Params::QR; + if (s == "CHOLESKY") return ISAM2Params::CHOLESKY; + + /* default is CHOLESKY */ + return ISAM2Params::CHOLESKY; +} + +/* ************************************************************************* */ +string ISAM2Params::factorizationTranslator( + const ISAM2Params::Factorization& value) { + string s; + switch (value) { + case ISAM2Params::QR: + s = "QR"; + break; + case ISAM2Params::CHOLESKY: + s = "CHOLESKY"; + break; + default: + s = "UNDEFINED"; + break; + } + return s; +} + +} // namespace gtsam diff --git a/gtsam/nonlinear/ISAM2Params.h b/gtsam/nonlinear/ISAM2Params.h new file mode 100644 index 000000000..afddd1f8e --- /dev/null +++ b/gtsam/nonlinear/ISAM2Params.h @@ -0,0 +1,365 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file ISAM2Params.h + * @brief Parameters for iSAM 2. + * @author Michael Kaess, Richard Roberts, Frank Dellaert + */ + +// \callgraph + +#pragma once + +#include +#include +#include +#include + +namespace gtsam { + +/** + * @addtogroup ISAM2 + * Parameters for ISAM2 using Gauss-Newton optimization. Either this class or + * ISAM2DoglegParams should be specified as the optimizationParams in + * ISAM2Params, which should in turn be passed to ISAM2(const ISAM2Params&). + */ +struct GTSAM_EXPORT ISAM2GaussNewtonParams { + double + wildfireThreshold; ///< Continue updating the linear delta only when + ///< changes are above this threshold (default: 0.001) + + /** Specify parameters as constructor arguments */ + ISAM2GaussNewtonParams( + double _wildfireThreshold = + 0.001 ///< see ISAM2GaussNewtonParams public variables, + ///< ISAM2GaussNewtonParams::wildfireThreshold + ) + : wildfireThreshold(_wildfireThreshold) {} + + void print(const std::string str = "") const { + using std::cout; + cout << str << "type: ISAM2GaussNewtonParams\n"; + cout << str << "wildfireThreshold: " << wildfireThreshold << "\n"; + cout.flush(); + } + + double getWildfireThreshold() const { return wildfireThreshold; } + void setWildfireThreshold(double wildfireThreshold) { + this->wildfireThreshold = wildfireThreshold; + } +}; + +/** + * @addtogroup ISAM2 + * Parameters for ISAM2 using Dogleg optimization. Either this class or + * ISAM2GaussNewtonParams should be specified as the optimizationParams in + * ISAM2Params, which should in turn be passed to ISAM2(const ISAM2Params&). + */ +struct GTSAM_EXPORT ISAM2DoglegParams { + double initialDelta; ///< The initial trust region radius for Dogleg + double + wildfireThreshold; ///< Continue updating the linear delta only when + ///< changes are above this threshold (default: 1e-5) + DoglegOptimizerImpl::TrustRegionAdaptationMode + adaptationMode; ///< See description in + ///< DoglegOptimizerImpl::TrustRegionAdaptationMode + bool + verbose; ///< Whether Dogleg prints iteration and convergence information + + /** Specify parameters as constructor arguments */ + ISAM2DoglegParams( + double _initialDelta = 1.0, ///< see ISAM2DoglegParams::initialDelta + double _wildfireThreshold = + 1e-5, ///< see ISAM2DoglegParams::wildfireThreshold + DoglegOptimizerImpl::TrustRegionAdaptationMode _adaptationMode = + DoglegOptimizerImpl:: + SEARCH_EACH_ITERATION, ///< see ISAM2DoglegParams::adaptationMode + bool _verbose = false ///< see ISAM2DoglegParams::verbose + ) + : initialDelta(_initialDelta), + wildfireThreshold(_wildfireThreshold), + adaptationMode(_adaptationMode), + verbose(_verbose) {} + + void print(const std::string str = "") const { + using std::cout; + cout << str << "type: ISAM2DoglegParams\n"; + cout << str << "initialDelta: " << initialDelta << "\n"; + cout << str << "wildfireThreshold: " << wildfireThreshold << "\n"; + cout << str + << "adaptationMode: " << adaptationModeTranslator(adaptationMode) + << "\n"; + cout.flush(); + } + + double getInitialDelta() const { return initialDelta; } + double getWildfireThreshold() const { return wildfireThreshold; } + std::string getAdaptationMode() const { + return adaptationModeTranslator(adaptationMode); + } + bool isVerbose() const { return verbose; } + void setInitialDelta(double initialDelta) { + this->initialDelta = initialDelta; + } + void setWildfireThreshold(double wildfireThreshold) { + this->wildfireThreshold = wildfireThreshold; + } + void setAdaptationMode(const std::string& adaptationMode) { + this->adaptationMode = adaptationModeTranslator(adaptationMode); + } + void setVerbose(bool verbose) { this->verbose = verbose; } + + std::string adaptationModeTranslator( + const DoglegOptimizerImpl::TrustRegionAdaptationMode& adaptationMode) + const; + DoglegOptimizerImpl::TrustRegionAdaptationMode adaptationModeTranslator( + const std::string& adaptationMode) const; +}; + +/** + * @addtogroup ISAM2 + * Parameters for the ISAM2 algorithm. Default parameter values are listed + * below. + */ +typedef FastMap ISAM2ThresholdMap; +typedef ISAM2ThresholdMap::value_type ISAM2ThresholdMapValue; +struct GTSAM_EXPORT ISAM2Params { + typedef boost::variant + OptimizationParams; ///< Either ISAM2GaussNewtonParams or + ///< ISAM2DoglegParams + typedef boost::variant > + RelinearizationThreshold; ///< Either a constant relinearization + ///< threshold or a per-variable-type set of + ///< thresholds + + /** Optimization parameters, this both selects the nonlinear optimization + * method and specifies its parameters, either ISAM2GaussNewtonParams or + * ISAM2DoglegParams. In the former, Gauss-Newton optimization will be used + * with the specified parameters, and in the latter Powell's dog-leg + * algorithm will be used with the specified parameters. + */ + OptimizationParams optimizationParams; + + /** Only relinearize variables whose linear delta magnitude is greater than + * this threshold (default: 0.1). If this is a FastMap instead + * of a double, then the threshold is specified for each dimension of each + * variable type. This parameter then maps from a character indicating the + * variable type to a Vector of thresholds for each dimension of that + * variable. For example, if Pose keys are of type TypedSymbol<'x',Pose3>, + * and landmark keys are of type TypedSymbol<'l',Point3>, then appropriate + * entries would be added with: + * \code + FastMap thresholds; + thresholds['x'] = (Vector(6) << 0.1, 0.1, 0.1, 0.5, 0.5, 0.5).finished(); + // 0.1 rad rotation threshold, 0.5 m translation threshold thresholds['l'] = + Vector3(1.0, 1.0, 1.0); // 1.0 m landmark position threshold + params.relinearizeThreshold = thresholds; + \endcode + */ + RelinearizationThreshold relinearizeThreshold; + + int relinearizeSkip; ///< Only relinearize any variables every + ///< relinearizeSkip calls to ISAM2::update (default: + ///< 10) + + bool enableRelinearization; ///< Controls whether ISAM2 will ever relinearize + ///< any variables (default: true) + + bool evaluateNonlinearError; ///< Whether to evaluate the nonlinear error + ///< before and after the update, to return in + ///< ISAM2Result from update() + + enum Factorization { CHOLESKY, QR }; + /** Specifies whether to use QR or CHOESKY numerical factorization (default: + * CHOLESKY). Cholesky is faster but potentially numerically unstable for + * poorly-conditioned problems, which can occur when uncertainty is very low + * in some variables (or dimensions of variables) and very high in others. QR + * is slower but more numerically stable in poorly-conditioned problems. We + * suggest using the default of Cholesky unless gtsam sometimes throws + * IndefiniteLinearSystemException when your problem's Hessian is actually + * positive definite. For positive definite problems, numerical error + * accumulation can cause the problem to become numerically negative or + * indefinite as solving proceeds, especially when using Cholesky. + */ + Factorization factorization; + + /** Whether to cache linear factors (default: true). + * This can improve performance if linearization is expensive, but can hurt + * performance if linearization is very cleap due to computation to look up + * additional keys. + */ + bool cacheLinearizedFactors; + + KeyFormatter + keyFormatter; ///< A KeyFormatter for when keys are printed during + ///< debugging (default: DefaultKeyFormatter) + + bool enableDetailedResults; ///< Whether to compute and return + ///< ISAM2Result::detailedResults, this can + ///< increase running time (default: false) + + /** Check variables for relinearization in tree-order, stopping the check once + * a variable does not need to be relinearized (default: false). This can + * improve speed by only checking a small part of the top of the tree. + * However, variables below the check cut-off can accumulate significant + * deltas without triggering relinearization. This is particularly useful in + * exploration scenarios where real-time performance is desired over + * correctness. Use with caution. + */ + bool enablePartialRelinearizationCheck; + + /// When you will be removing many factors, e.g. when using ISAM2 as a + /// fixed-lag smoother, enable this option to add factors in the first + /// available factor slots, to avoid accumulating NULL factor slots, at the + /// cost of having to search for slots every time a factor is added. + bool findUnusedFactorSlots; + + /** + * Specify parameters as constructor arguments + * See the documentation of member variables above. + */ + ISAM2Params(OptimizationParams _optimizationParams = ISAM2GaussNewtonParams(), + RelinearizationThreshold _relinearizeThreshold = 0.1, + int _relinearizeSkip = 10, bool _enableRelinearization = true, + bool _evaluateNonlinearError = false, + Factorization _factorization = ISAM2Params::CHOLESKY, + bool _cacheLinearizedFactors = true, + const KeyFormatter& _keyFormatter = + DefaultKeyFormatter ///< see ISAM2::Params::keyFormatter + ) + : optimizationParams(_optimizationParams), + relinearizeThreshold(_relinearizeThreshold), + relinearizeSkip(_relinearizeSkip), + enableRelinearization(_enableRelinearization), + evaluateNonlinearError(_evaluateNonlinearError), + factorization(_factorization), + cacheLinearizedFactors(_cacheLinearizedFactors), + keyFormatter(_keyFormatter), + enableDetailedResults(false), + enablePartialRelinearizationCheck(false), + findUnusedFactorSlots(false) {} + + /// print iSAM2 parameters + void print(const std::string& str = "") const { + using std::cout; + cout << str << "\n"; + + static const std::string kStr("optimizationParams: "); + if (optimizationParams.type() == typeid(ISAM2GaussNewtonParams)) + boost::get(optimizationParams).print(); + else if (optimizationParams.type() == typeid(ISAM2DoglegParams)) + boost::get(optimizationParams).print(kStr); + else + cout << kStr << "{unknown type}\n"; + + cout << "relinearizeThreshold: "; + if (relinearizeThreshold.type() == typeid(double)) { + cout << boost::get(relinearizeThreshold) << "\n"; + } else { + cout << "{mapped}\n"; + for (const ISAM2ThresholdMapValue& value : + boost::get(relinearizeThreshold)) { + cout << " '" << value.first + << "' -> [" << value.second.transpose() << " ]\n"; + } + } + + cout << "relinearizeSkip: " << relinearizeSkip << "\n"; + cout << "enableRelinearization: " << enableRelinearization + << "\n"; + cout << "evaluateNonlinearError: " << evaluateNonlinearError + << "\n"; + cout << "factorization: " + << factorizationTranslator(factorization) << "\n"; + cout << "cacheLinearizedFactors: " << cacheLinearizedFactors + << "\n"; + cout << "enableDetailedResults: " << enableDetailedResults + << "\n"; + cout << "enablePartialRelinearizationCheck: " + << enablePartialRelinearizationCheck << "\n"; + cout << "findUnusedFactorSlots: " << findUnusedFactorSlots + << "\n"; + cout.flush(); + } + + /// @name Getters and Setters for all properties + /// @{ + + OptimizationParams getOptimizationParams() const { + return this->optimizationParams; + } + RelinearizationThreshold getRelinearizeThreshold() const { + return relinearizeThreshold; + } + int getRelinearizeSkip() const { return relinearizeSkip; } + bool isEnableRelinearization() const { return enableRelinearization; } + bool isEvaluateNonlinearError() const { return evaluateNonlinearError; } + std::string getFactorization() const { + return factorizationTranslator(factorization); + } + bool isCacheLinearizedFactors() const { return cacheLinearizedFactors; } + KeyFormatter getKeyFormatter() const { return keyFormatter; } + bool isEnableDetailedResults() const { return enableDetailedResults; } + bool isEnablePartialRelinearizationCheck() const { + return enablePartialRelinearizationCheck; + } + + void setOptimizationParams(OptimizationParams optimizationParams) { + this->optimizationParams = optimizationParams; + } + void setRelinearizeThreshold(RelinearizationThreshold relinearizeThreshold) { + this->relinearizeThreshold = relinearizeThreshold; + } + void setRelinearizeSkip(int relinearizeSkip) { + this->relinearizeSkip = relinearizeSkip; + } + void setEnableRelinearization(bool enableRelinearization) { + this->enableRelinearization = enableRelinearization; + } + void setEvaluateNonlinearError(bool evaluateNonlinearError) { + this->evaluateNonlinearError = evaluateNonlinearError; + } + void setFactorization(const std::string& factorization) { + this->factorization = factorizationTranslator(factorization); + } + void setCacheLinearizedFactors(bool cacheLinearizedFactors) { + this->cacheLinearizedFactors = cacheLinearizedFactors; + } + void setKeyFormatter(KeyFormatter keyFormatter) { + this->keyFormatter = keyFormatter; + } + void setEnableDetailedResults(bool enableDetailedResults) { + this->enableDetailedResults = enableDetailedResults; + } + void setEnablePartialRelinearizationCheck( + bool enablePartialRelinearizationCheck) { + this->enablePartialRelinearizationCheck = enablePartialRelinearizationCheck; + } + + GaussianFactorGraph::Eliminate getEliminationFunction() const { + return factorization == CHOLESKY + ? (GaussianFactorGraph::Eliminate)EliminatePreferCholesky + : (GaussianFactorGraph::Eliminate)EliminateQR; + } + + /// @} + + /// @name Some utilities + /// @{ + + static Factorization factorizationTranslator(const std::string& str); + static std::string factorizationTranslator(const Factorization& value); + + /// @} +}; + +} // namespace gtsam diff --git a/gtsam/nonlinear/ISAM2Result.h b/gtsam/nonlinear/ISAM2Result.h new file mode 100644 index 000000000..1539d90c4 --- /dev/null +++ b/gtsam/nonlinear/ISAM2Result.h @@ -0,0 +1,159 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file ISAM2Result.h + * @brief Class that stores detailed iSAM2 result. + * @author Michael Kaess, Richard Roberts, Frank Dellaert + */ + +// \callgraph + +#pragma once + +#include +#include + +#include +#include +#include +#include + +#include + +namespace gtsam { + +typedef FastVector FactorIndices; + +/** + * @addtogroup ISAM2 + * This struct is returned from ISAM2::update() and contains information about + * the update that is useful for determining whether the solution is + * converging, and about how much work was required for the update. See member + * variables for details and information about each entry. + */ +struct GTSAM_EXPORT ISAM2Result { + /** The nonlinear error of all of the factors, \a including new factors and + * variables added during the current call to ISAM2::update(). This error is + * calculated using the following variable values: + * \li Pre-existing variables will be evaluated by combining their + * linearization point before this call to update, with their partial linear + * delta, as computed by ISAM2::calculateEstimate(). + * \li New variables will be evaluated at their initialization points passed + * into the current call to update. + * \par Note: This will only be computed if + * ISAM2Params::evaluateNonlinearError is set to \c true, because there is + * some cost to this computation. + */ + boost::optional errorBefore; + + /** The nonlinear error of all of the factors computed after the current + * update, meaning that variables above the relinearization threshold + * (ISAM2Params::relinearizeThreshold) have been relinearized and new + * variables have undergone one linear update. Variable values are + * again computed by combining their linearization points with their + * partial linear deltas, by ISAM2::calculateEstimate(). + * \par Note: This will only be computed if + * ISAM2Params::evaluateNonlinearError is set to \c true, because there is + * some cost to this computation. + */ + boost::optional errorAfter; + + /** The number of variables that were relinearized because their linear + * deltas exceeded the reslinearization threshold + * (ISAM2Params::relinearizeThreshold), combined with any additional + * variables that had to be relinearized because they were involved in + * the same factor as a variable above the relinearization threshold. + * On steps where no relinearization is considered + * (see ISAM2Params::relinearizeSkip), this count will be zero. + */ + size_t variablesRelinearized; + + /** The number of variables that were reeliminated as parts of the Bayes' + * Tree were recalculated, due to new factors. When loop closures occur, + * this count will be large as the new loop-closing factors will tend to + * involve variables far away from the root, and everything up to the root + * will be reeliminated. + */ + size_t variablesReeliminated; + + /** The number of factors that were included in reelimination of the Bayes' + * tree. */ + size_t factorsRecalculated; + + /** The number of cliques in the Bayes' Tree */ + size_t cliques; + + /** The indices of the newly-added factors, in 1-to-1 correspondence with the + * factors passed as \c newFactors to ISAM2::update(). These indices may be + * used later to refer to the factors in order to remove them. + */ + FactorIndices newFactorsIndices; + + /** A struct holding detailed results, which must be enabled with + * ISAM2Params::enableDetailedResults. + */ + struct DetailedResults { + /** The status of a single variable, this struct is stored in + * DetailedResults::variableStatus */ + struct VariableStatus { + /** Whether the variable was just reeliminated, due to being relinearized, + * observed, new, or on the path up to the root clique from another + * reeliminated variable. */ + bool isReeliminated; + bool isAboveRelinThreshold; ///< Whether the variable was just + ///< relinearized due to being above the + ///< relinearization threshold + bool isRelinearizeInvolved; ///< Whether the variable was below the + ///< relinearization threshold but was + ///< relinearized by being involved in a + ///< factor with a variable above the + ///< relinearization threshold + bool isRelinearized; /// Whether the variable was relinearized, either by + /// being above the relinearization threshold or by + /// involvement. + bool isObserved; ///< Whether the variable was just involved in new + ///< factors + bool isNew; ///< Whether the variable itself was just added + bool inRootClique; ///< Whether the variable is in the root clique + VariableStatus() + : isReeliminated(false), + isAboveRelinThreshold(false), + isRelinearizeInvolved(false), + isRelinearized(false), + isObserved(false), + isNew(false), + inRootClique(false) {} + }; + + /** The status of each variable during this update, see VariableStatus. + */ + FastMap variableStatus; + }; + + /** Detailed results, if enabled by ISAM2Params::enableDetailedResults. See + * Detail for information about the results data stored here. */ + boost::optional detail; + + void print(const std::string str = "") const { + using std::cout; + cout << str << " Reelimintated: " << variablesReeliminated + << " Relinearized: " << variablesRelinearized + << " Cliques: " << cliques << std::endl; + } + + /** Getters and Setters */ + size_t getVariablesRelinearized() const { return variablesRelinearized; } + size_t getVariablesReeliminated() const { return variablesReeliminated; } + size_t getCliques() const { return cliques; } +}; + +} // namespace gtsam From 63b7d64fea2febfabfe2909e151ec82140950836 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 30 Sep 2018 14:09:52 -0400 Subject: [PATCH 18/51] Fixed headers --- gtsam/inference/BayesTreeCliqueBase.h | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/gtsam/inference/BayesTreeCliqueBase.h b/gtsam/inference/BayesTreeCliqueBase.h index eae886785..055a03939 100644 --- a/gtsam/inference/BayesTreeCliqueBase.h +++ b/gtsam/inference/BayesTreeCliqueBase.h @@ -17,9 +17,13 @@ #pragma once -#include +#include +#include #include #include +#include + +#include namespace gtsam { From d6edc217abb64ac9e76323d69c38d483c7722649 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 30 Sep 2018 14:11:00 -0400 Subject: [PATCH 19/51] Changed several Impl functions to methods in ISAM2 and ISAM2Clique --- gtsam/nonlinear/ISAM2-impl.cpp | 131 ++++---------------------------- gtsam/nonlinear/ISAM2-impl.h | 66 ++-------------- gtsam/nonlinear/ISAM2.cpp | 76 +++++++++++++++--- gtsam/nonlinear/ISAM2.h | 24 ++++++ gtsam/nonlinear/ISAM2Clique.cpp | 27 ++++++- gtsam/nonlinear/ISAM2Clique.h | 21 ++++- tests/testGaussianISAM2.cpp | 2 +- 7 files changed, 154 insertions(+), 193 deletions(-) diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index bc43c9b9d..8a8813af5 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -31,27 +31,12 @@ using namespace std; namespace gtsam { -/* ************************************************************************* */ -void ISAM2::Impl::AddVariables(const Values& newTheta, Values& theta, - VectorValues& delta, VectorValues& deltaNewton, - VectorValues& RgProd, - const KeyFormatter& keyFormatter) { - const bool debug = ISDEBUG("ISAM2 AddVariables"); - - theta.insert(newTheta); - if (debug) newTheta.print("The new variables are: "); - // Add zeros into the VectorValues - delta.insert(newTheta.zeroVectors()); - deltaNewton.insert(newTheta.zeroVectors()); - RgProd.insert(newTheta.zeroVectors()); -} - /* ************************************************************************* */ void ISAM2::Impl::AddFactorsStep1(const NonlinearFactorGraph& newFactors, bool useUnusedSlots, - NonlinearFactorGraph& nonlinearFactors, - FactorIndices& newFactorIndices) { - newFactorIndices.resize(newFactors.size()); + NonlinearFactorGraph* nonlinearFactors, + FactorIndices* newFactorIndices) { + newFactorIndices->resize(newFactors.size()); if (useUnusedSlots) { size_t globalFactorIndex = 0; @@ -65,44 +50,24 @@ void ISAM2::Impl::AddFactorsStep1(const NonlinearFactorGraph& newFactors, // if so, increase globalFactorIndex. If the current factor in // nonlinearFactors is unused, break out of the loop and use the current // slot. - if (globalFactorIndex >= nonlinearFactors.size()) - nonlinearFactors.resize(nonlinearFactors.size() + newFactors.size() - - newFactorIndex); - else if (nonlinearFactors[globalFactorIndex]) + if (globalFactorIndex >= nonlinearFactors->size()) + nonlinearFactors->resize(nonlinearFactors->size() + + newFactors.size() - newFactorIndex); + else if ((*nonlinearFactors)[globalFactorIndex]) ++globalFactorIndex; else break; } while (true); // Use the current slot, updating nonlinearFactors and newFactorSlots. - nonlinearFactors[globalFactorIndex] = newFactors[newFactorIndex]; - newFactorIndices[newFactorIndex] = globalFactorIndex; + (*nonlinearFactors)[globalFactorIndex] = newFactors[newFactorIndex]; + (*newFactorIndices)[newFactorIndex] = globalFactorIndex; } } else { // We're not looking for unused slots, so just add the factors at the end. for (size_t i = 0; i < newFactors.size(); ++i) - newFactorIndices[i] = i + nonlinearFactors.size(); - nonlinearFactors.push_back(newFactors); - } -} - -/* ************************************************************************* */ -void ISAM2::Impl::RemoveVariables(const KeySet& unusedKeys, - const FastVector& roots, - Values& theta, VariableIndex& variableIndex, - VectorValues& delta, - VectorValues& deltaNewton, - VectorValues& RgProd, KeySet& replacedKeys, - Base::Nodes& nodes, KeySet& fixedVariables) { - variableIndex.removeUnusedVariables(unusedKeys.begin(), unusedKeys.end()); - for (Key key : unusedKeys) { - delta.erase(key); - deltaNewton.erase(key); - RgProd.erase(key); - replacedKeys.erase(key); - nodes.unsafe_erase(key); - theta.erase(key); - fixedVariables.erase(key); + (*newFactorIndices)[i] = i + nonlinearFactors->size(); + nonlinearFactors->push_back(newFactors); } } @@ -195,7 +160,7 @@ static void CheckRelinearizationRecursiveMap( /* ************************************************************************* */ KeySet ISAM2::Impl::CheckRelinearizationPartial( - const FastVector& roots, const VectorValues& delta, + const ISAM2::Roots& roots, const VectorValues& delta, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) { KeySet relinKeys; for (const ISAM2::sharedClique& root : roots) { @@ -210,71 +175,6 @@ KeySet ISAM2::Impl::CheckRelinearizationPartial( return relinKeys; } -/* ************************************************************************* */ -void ISAM2::Impl::FindAll(ISAM2::sharedClique clique, KeySet& keys, - const KeySet& markedMask) { - static const bool debug = false; - // does the separator contain any of the variables? - bool found = false; - for (Key key : clique->conditional()->parents()) { - if (markedMask.exists(key)) { - found = true; - break; - } - } - if (found) { - // then add this clique - keys.insert(clique->conditional()->beginFrontals(), - clique->conditional()->endFrontals()); - if (debug) clique->print("Key(s) marked in clique "); - if (debug) - cout << "so marking key " << clique->conditional()->front() << endl; - } - for (const ISAM2::sharedClique& child : clique->children) { - FindAll(child, keys, markedMask); - } -} - -/* ************************************************************************* */ -void ISAM2::Impl::ExpmapMasked(const VectorValues& delta, const KeySet& mask, - Values* values, - boost::optional invalidateIfDebug, - const KeyFormatter& keyFormatter) { - // If debugging, invalidate if requested, otherwise do not invalidate. - // Invalidating means setting expmapped entries to Inf, to trigger assertions - // if we try to re-use them. -#ifdef NDEBUG - invalidateIfDebug = boost::none; -#endif - - assert(values->size() == delta.size()); - Values::iterator key_value; - VectorValues::const_iterator key_delta; -#ifdef GTSAM_USE_TBB - for (key_value = values->begin(); key_value != values->end(); ++key_value) { - key_delta = delta.find(key_value->key); -#else - for (key_value = values->begin(), key_delta = delta.begin(); - key_value != values->end(); ++key_value, ++key_delta) { - assert(key_value->key == key_delta->first); -#endif - Key var = key_value->key; - assert(static_cast(delta[var].size()) == key_value->value.dim()); - assert(delta[var].allFinite()); - if (mask.exists(var)) { - Value* retracted = key_value->value.retract_(delta[var]); - key_value->value = *retracted; - retracted->deallocate_(); - if (invalidateIfDebug) - (*invalidateIfDebug)[var].operator=(Vector::Constant( - delta[var].rows(), - numeric_limits::infinity())); // Strange syntax to work - // with clang++ (bug in - // clang?) - } - } -} - /* ************************************************************************* */ namespace internal { inline static void optimizeInPlace(const ISAM2::sharedClique& clique, @@ -289,9 +189,10 @@ inline static void optimizeInPlace(const ISAM2::sharedClique& clique, } // namespace internal /* ************************************************************************* */ -size_t ISAM2::Impl::UpdateGaussNewtonDelta( - const FastVector& roots, const KeySet& replacedKeys, - double wildfireThreshold, VectorValues* delta) { +size_t ISAM2::Impl::UpdateGaussNewtonDelta(const ISAM2::Roots& roots, + const KeySet& replacedKeys, + double wildfireThreshold, + VectorValues* delta) { size_t lastBacksubVariableCount; if (wildfireThreshold <= 0.0) { diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index 0e05ab453..8a30fb8cd 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -23,7 +23,6 @@ namespace gtsam { struct GTSAM_EXPORT ISAM2::Impl { - struct GTSAM_EXPORT PartialSolveResult { ISAM2::sharedClique bayesTree; }; @@ -32,35 +31,14 @@ struct GTSAM_EXPORT ISAM2::Impl { size_t nFullSystemVars; enum { /*AS_ADDED,*/ COLAMD } algorithm; enum { NO_CONSTRAINT, CONSTRAIN_LAST } constrain; - boost::optional > constrainedKeys; + boost::optional > constrainedKeys; }; - /** - * Add new variables to the ISAM2 system. - * @param newTheta Initial values for new variables - * @param theta Current solution to be augmented with new initialization - * @param delta Current linear delta to be augmented with zeros - * @param ordering Current ordering to be augmented with new variables - * @param nodes Current BayesTree::Nodes index to be augmented with slots for new variables - * @param keyFormatter Formatter for printing nonlinear keys during debugging - */ - static void AddVariables(const Values& newTheta, Values& theta, VectorValues& delta, - VectorValues& deltaNewton, VectorValues& RgProd, - const KeyFormatter& keyFormatter = DefaultKeyFormatter); - /// Perform the first part of the bookkeeping updates for adding new factors. Adds them to the /// complete list of nonlinear factors, and populates the list of new factor indices, both /// optionally finding and reusing empty factor slots. static void AddFactorsStep1(const NonlinearFactorGraph& newFactors, bool useUnusedSlots, - NonlinearFactorGraph& nonlinearFactors, FactorIndices& newFactorIndices); - - /** - * Remove variables from the ISAM2 system. - */ - static void RemoveVariables(const KeySet& unusedKeys, const FastVector& roots, - Values& theta, VariableIndex& variableIndex, VectorValues& delta, VectorValues& deltaNewton, - VectorValues& RgProd, KeySet& replacedKeys, Base::Nodes& nodes, - KeySet& fixedVariables); + NonlinearFactorGraph* nonlinearFactors, FactorIndices* newFactorIndices); /** * Find the set of variables to be relinearized according to relinearizeThreshold. @@ -85,47 +63,13 @@ struct GTSAM_EXPORT ISAM2::Impl { * @return The set of variable indices in delta whose magnitude is greater than or * equal to relinearizeThreshold */ - static KeySet CheckRelinearizationPartial(const FastVector& roots, + static KeySet CheckRelinearizationPartial(const ISAM2::Roots& roots, const VectorValues& delta, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold); - /** - * Recursively search this clique and its children for marked keys appearing - * in the separator, and add the *frontal* keys of any cliques whose - * separator contains any marked keys to the set \c keys. The purpose of - * this is to discover the cliques that need to be redone due to information - * propagating to them from cliques that directly contain factors being - * relinearized. - * - * The original comment says this finds all variables directly connected to - * the marked ones by measurements. Is this true, because it seems like this - * would also pull in variables indirectly connected through other frontal or - * separator variables? - * - * Alternatively could we trace up towards the root for each variable here? - */ - static void FindAll(ISAM2Clique::shared_ptr clique, KeySet& keys, const KeySet& markedMask); - - /** - * Apply expmap to the given values, but only for indices appearing in - * \c markedRelinMask. Values are expmapped in-place. - * \param delta The linear delta with which to expmap - * \param mask Mask on linear indices, only \c true entries are expmapped - * \param [in, out] values The value to expmap in-place - * \param invalidateIfDebug If this is true, *and* NDEBUG is not defined, - * expmapped deltas will be set to an invalid value (infinity) to catch bugs - * where we might expmap something twice, or expmap it but then not - * recalculate its delta. - * @param keyFormatter Formatter for printing nonlinear keys during debugging - */ - static void ExpmapMasked( - const VectorValues& delta, const KeySet& mask, Values* values, - boost::optional invalidateIfDebug = boost::none, - const KeyFormatter& keyFormatter = DefaultKeyFormatter); - /** * Update the Newton's method step point, using wildfire */ - static size_t UpdateGaussNewtonDelta(const FastVector& roots, + static size_t UpdateGaussNewtonDelta(const ISAM2::Roots& roots, const KeySet& replacedKeys, double wildfireThreshold, VectorValues* delta); /** @@ -134,7 +78,7 @@ struct GTSAM_EXPORT ISAM2::Impl { */ static size_t UpdateRgProd(const ISAM2::Roots& roots, const KeySet& replacedKeys, const VectorValues& gradAtZero, VectorValues* RgProd); - + /** * Compute the gradient-search point. Only used in Dogleg. */ diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index e543508b2..2f44063ac 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -33,6 +33,7 @@ using namespace boost::adaptors; } // namespace br #include +#include #include #include @@ -484,6 +485,62 @@ boost::shared_ptr ISAM2::recalculate( return affectedKeysSet; } +/* ************************************************************************* */ +void ISAM2::addVariables(const Values& newTheta) { + const bool debug = ISDEBUG("ISAM2 AddVariables"); + + theta_.insert(newTheta); + if (debug) newTheta.print("The new variables are: "); + // Add zeros into the VectorValues + delta_.insert(newTheta.zeroVectors()); + deltaNewton_.insert(newTheta.zeroVectors()); + RgProd_.insert(newTheta.zeroVectors()); +} + +/* ************************************************************************* */ +void ISAM2::removeVariables(const KeySet& unusedKeys) { + variableIndex_.removeUnusedVariables(unusedKeys.begin(), unusedKeys.end()); + for (Key key : unusedKeys) { + delta_.erase(key); + deltaNewton_.erase(key); + RgProd_.erase(key); + deltaReplacedMask_.erase(key); + Base::nodes_.unsafe_erase(key); + theta_.erase(key); + fixedVariables_.erase(key); + } +} + +/* ************************************************************************* */ +void ISAM2::expmapMasked(const KeySet& mask) { + assert(theta_.size() == delta_.size()); + Values::iterator key_value; + VectorValues::const_iterator key_delta; +#ifdef GTSAM_USE_TBB + for (key_value = theta_.begin(); key_value != theta_.end(); ++key_value) { + key_delta = delta_.find(key_value->key); +#else + for (key_value = theta_.begin(), key_delta = delta_.begin(); + key_value != theta_.end(); ++key_value, ++key_delta) { + assert(key_value->key == key_delta->first); +#endif + Key var = key_value->key; + assert(static_cast(delta_[var].size()) == key_value->value.dim()); + assert(delta_[var].allFinite()); + if (mask.exists(var)) { + Value* retracted = key_value->value.retract_(delta_[var]); + key_value->value = *retracted; + retracted->deallocate_(); +#ifndef NDEBUG + // If debugging, invalidate delta_ entries to Inf, to trigger assertions + // if we try to re-use them. + delta_[var] = Vector::Constant(delta_[var].rows(), + numeric_limits::infinity()); +#endif + } + } +} + /* ************************************************************************* */ ISAM2Result ISAM2::update( const NonlinearFactorGraph& newFactors, const Values& newTheta, @@ -529,7 +586,7 @@ ISAM2Result ISAM2::update( // Add the new factor indices to the result struct if (debug || verbose) newFactors.print("The new factors are: "); Impl::AddFactorsStep1(newFactors, params_.findUnusedFactorSlots, - nonlinearFactors_, result.newFactorsIndices); + &nonlinearFactors_, &result.newFactorsIndices); // Remove the removed factors NonlinearFactorGraph removeFactors; @@ -571,7 +628,7 @@ ISAM2Result ISAM2::update( gttic(add_new_variables); // 2. Initialize any new variables \Theta_{new} and add // \Theta:=\Theta\cup\Theta_{new}. - Impl::AddVariables(newTheta, theta_, delta_, deltaNewton_, RgProd_); + addVariables(newTheta); // New keys for detailed results if (params_.enableDetailedResults) { for (Key key : newTheta.keys()) { @@ -669,13 +726,13 @@ ISAM2Result ISAM2::update( if (!relinKeys.empty()) { for (const sharedClique& root : roots_) // add other cliques that have the marked ones in the separator - Impl::FindAll(root, markedKeys, markedRelinMask); + root->findAll(markedRelinMask, &markedKeys); // Relin involved keys for detailed results if (params_.enableDetailedResults) { KeySet involvedRelinKeys; for (const sharedClique& root : roots_) - Impl::FindAll(root, involvedRelinKeys, markedRelinMask); + root->findAll(markedRelinMask, &involvedRelinKeys); for (Key key : involvedRelinKeys) { if (!result.detail->variableStatus[key].isAboveRelinThreshold) { result.detail->variableStatus[key].isRelinearizeInvolved = true; @@ -689,8 +746,7 @@ ISAM2Result ISAM2::update( gttic(expmap); // 6. Update linearization point for marked variables: // \Theta_{J}:=\Theta_{J}+\Delta_{J}. - if (!relinKeys.empty()) - Impl::ExpmapMasked(delta_, markedRelinMask, &theta_, delta_); + if (!relinKeys.empty()) expmapMasked(markedRelinMask); gttoc(expmap); result.variablesRelinearized = markedKeys.size(); @@ -740,9 +796,7 @@ ISAM2Result ISAM2::update( // Update data structures to remove unused keys if (!unusedKeys.empty()) { gttic(remove_variables); - Impl::RemoveVariables(unusedKeys, roots_, theta_, variableIndex_, delta_, - deltaNewton_, RgProd_, deltaReplacedMask_, - Base::nodes_, fixedVariables_); + removeVariables(unusedKeys); gttoc(remove_variables); } result.cliques = this->nodes().size(); @@ -998,9 +1052,7 @@ void ISAM2::marginalizeLeaves( factorIndicesToRemove.end()); // Remove the marginalized variables - Impl::RemoveVariables(KeySet(leafKeys.begin(), leafKeys.end()), roots_, - theta_, variableIndex_, delta_, deltaNewton_, RgProd_, - deltaReplacedMask_, nodes_, fixedVariables_); + removeVariables(KeySet(leafKeys.begin(), leafKeys.end())); } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 069d90cc8..2de8eb608 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -269,6 +269,29 @@ class GTSAM_EXPORT ISAM2 : public BayesTree { /// @} protected: + /** + * Add new variables to the ISAM2 system. + * @param newTheta Initial values for new variables + * @param theta Current solution to be augmented with new initialization + * @param delta Current linear delta to be augmented with zeros + * @param deltaNewton + * @param RgProd + * @param keyFormatter Formatter for printing nonlinear keys during debugging + */ + void addVariables(const Values& newTheta); + + /** + * Remove variables from the ISAM2 system. + */ + void removeVariables(const KeySet& unusedKeys); + + /** + * Apply expmap to the given values, but only for indices appearing in + * \c mask. Values are expmapped in-place. + * \param mask Mask on linear indices, only \c true entries are expmapped + */ + void expmapMasked(const KeySet& mask); + FastSet getAffectedFactors(const FastList& keys) const; GaussianFactorGraph::shared_ptr relinearizeAffectedFactors( const FastList& affectedKeys, const KeySet& relinKeys) const; @@ -279,6 +302,7 @@ class GTSAM_EXPORT ISAM2 : public BayesTree { const std::vector& observedKeys, const KeySet& unusedIndices, const boost::optional >& constrainKeys, ISAM2Result& result); + void updateDelta(bool forceFullSolve = false) const; }; // ISAM2 diff --git a/gtsam/nonlinear/ISAM2Clique.cpp b/gtsam/nonlinear/ISAM2Clique.cpp index 6e03b0680..c55ca7959 100644 --- a/gtsam/nonlinear/ISAM2Clique.cpp +++ b/gtsam/nonlinear/ISAM2Clique.cpp @@ -15,10 +15,10 @@ * @author Michael Kaess, Richard Roberts, Frank Dellaert */ -#include +#include #include #include -#include +#include #include using namespace std; @@ -299,4 +299,27 @@ size_t ISAM2Clique::calculate_nnz() const { return result; } +/* ************************************************************************* */ +void ISAM2Clique::findAll(const KeySet& markedMask, KeySet* keys) const { + static const bool debug = false; + // does the separator contain any of the variables? + bool found = false; + for (Key key : conditional()->parents()) { + if (markedMask.exists(key)) { + found = true; + break; + } + } + if (found) { + // then add this clique + keys->insert(conditional()->beginFrontals(), conditional()->endFrontals()); + if (debug) print("Key(s) marked in clique "); + if (debug) cout << "so marking key " << conditional()->front() << endl; + } + for (const auto& child : children) { + child->findAll(markedMask, keys); + } +} + +/* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/nonlinear/ISAM2Clique.h b/gtsam/nonlinear/ISAM2Clique.h index fecc8d335..3c53e3d72 100644 --- a/gtsam/nonlinear/ISAM2Clique.h +++ b/gtsam/nonlinear/ISAM2Clique.h @@ -19,11 +19,11 @@ #pragma once +#include +#include #include #include #include -#include -#include #include namespace gtsam { @@ -99,6 +99,23 @@ class GTSAM_EXPORT ISAM2Clique void nnz_internal(size_t* result) const; size_t calculate_nnz() const; + /** + * Recursively search this clique and its children for marked keys appearing + * in the separator, and add the *frontal* keys of any cliques whose + * separator contains any marked keys to the set \c keys. The purpose of + * this is to discover the cliques that need to be redone due to information + * propagating to them from cliques that directly contain factors being + * relinearized. + * + * The original comment says this finds all variables directly connected to + * the marked ones by measurements. Is this true, because it seems like this + * would also pull in variables indirectly connected through other frontal or + * separator variables? + * + * Alternatively could we trace up towards the root for each variable here? + */ + void findAll(const KeySet& markedMask, KeySet* keys) const; + private: /** * Check if clique was replaced, or if any parents were changed above the diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 681c30587..d6ca896dd 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -304,7 +304,7 @@ TEST(ISAM2, AddFactorsStep1) FactorIndices actualNewFactorIndices; - ISAM2::Impl::AddFactorsStep1(newFactors, true, nonlinearFactors, actualNewFactorIndices); + ISAM2::Impl::AddFactorsStep1(newFactors, true, &nonlinearFactors, &actualNewFactorIndices); EXPECT(assert_equal(expectedNonlinearFactors, nonlinearFactors)); EXPECT(assert_container_equality(expectedNewFactorIndices, actualNewFactorIndices)); From 2fe2f1ad190614904256068b2da1d7a7f2a35b14 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 30 Sep 2018 14:22:20 -0400 Subject: [PATCH 20/51] input/output convention --- gtsam/nonlinear/ISAM2.cpp | 18 +++++++++--------- gtsam/nonlinear/ISAM2.h | 2 +- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 2f44063ac..1c6da94a7 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -199,7 +199,7 @@ boost::shared_ptr ISAM2::recalculate( const KeySet& markedKeys, const KeySet& relinKeys, const vector& observedKeys, const KeySet& unusedIndices, const boost::optional >& constrainKeys, - ISAM2Result& result) { + ISAM2Result* result) { // TODO(dellaert): new factors are linearized twice, // the newFactors passed in are not used. @@ -333,8 +333,8 @@ boost::shared_ptr ISAM2::recalculate( this->nodes_.insert(bayesTree->nodes().begin(), bayesTree->nodes().end()); gttoc(insert); - result.variablesReeliminated = affectedKeysSet->size(); - result.factorsRecalculated = nonlinearFactors_.size(); + result->variablesReeliminated = affectedKeysSet->size(); + result->factorsRecalculated = nonlinearFactors_.size(); lastAffectedMarkedCount = markedKeys.size(); lastAffectedVariableCount = affectedKeysSet->size(); @@ -343,7 +343,7 @@ boost::shared_ptr ISAM2::recalculate( // Reeliminated keys for detailed results if (params_.enableDetailedResults) { for (Key key : theta_.keys()) { - result.detail->variableStatus[key].isReeliminated = true; + result->detail->variableStatus[key].isReeliminated = true; } } @@ -375,12 +375,12 @@ boost::shared_ptr ISAM2::recalculate( // Reeliminated keys for detailed results if (params_.enableDetailedResults) { for (Key key : affectedAndNewKeys) { - result.detail->variableStatus[key].isReeliminated = true; + result->detail->variableStatus[key].isReeliminated = true; } } - result.variablesReeliminated = affectedAndNewKeys.size(); - result.factorsRecalculated = factors.size(); + result->variablesReeliminated = affectedAndNewKeys.size(); + result->factorsRecalculated = factors.size(); lastAffectedMarkedCount = markedKeys.size(); lastAffectedVariableCount = affectedKeys.size(); lastAffectedFactorCount = factors.size(); @@ -479,7 +479,7 @@ boost::shared_ptr ISAM2::recalculate( if (params_.enableDetailedResults) { for (const sharedNode& root : this->roots()) for (Key var : *root->conditional()) - result.detail->variableStatus[var].inRootClique = true; + result->detail->variableStatus[var].inRootClique = true; } return affectedKeysSet; @@ -786,7 +786,7 @@ ISAM2Result ISAM2::update( boost::shared_ptr replacedKeys; if (!markedKeys.empty() || !observedKeys.empty()) replacedKeys = recalculate(markedKeys, relinKeys, observedKeys, - unusedIndices, constrainedKeys, result); + unusedIndices, constrainedKeys, &result); // Update replaced keys mask (accumulates until back-substitution takes place) if (replacedKeys) diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 2de8eb608..5d448d786 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -301,7 +301,7 @@ class GTSAM_EXPORT ISAM2 : public BayesTree { const KeySet& markedKeys, const KeySet& relinKeys, const std::vector& observedKeys, const KeySet& unusedIndices, const boost::optional >& constrainKeys, - ISAM2Result& result); + ISAM2Result* result); void updateDelta(bool forceFullSolve = false) const; }; // ISAM2 From c50b3cd6beb7c695fc067cb3e01087748e8452df Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 30 Sep 2018 15:13:47 -0400 Subject: [PATCH 21/51] Eliminated some copypasta with lambda --- gtsam/nonlinear/ISAM2.cpp | 112 +++++++++++++++----------------------- 1 file changed, 44 insertions(+), 68 deletions(-) diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 1c6da94a7..cad9add9f 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -141,8 +141,7 @@ GaussianFactorGraph::shared_ptr ISAM2::relinearizeAffectedFactors( gttoc(affectedKeysSet); gttic(check_candidates_and_linearize); - GaussianFactorGraph::shared_ptr linearized = - boost::make_shared(); + auto linearized = boost::make_shared(); for (Key idx : candidates) { bool inside = true; bool useCachedLinear = params_.cacheLinearizedFactors; @@ -162,8 +161,7 @@ GaussianFactorGraph::shared_ptr ISAM2::relinearizeAffectedFactors( #endif linearized->push_back(linearFactors_[idx]); } else { - GaussianFactor::shared_ptr linearFactor = - nonlinearFactors_[idx]->linearize(theta_); + auto linearFactor = nonlinearFactors_[idx]->linearize(theta_); linearized->push_back(linearFactor); if (params_.cacheLinearizedFactors) { #ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS @@ -268,7 +266,7 @@ boost::shared_ptr ISAM2::recalculate( // path to root included gttic(affectedKeys); FastList affectedKeys; - for (const ConditionalType::shared_ptr& conditional : affectedBayesNet) + for (const auto& conditional : affectedBayesNet) affectedKeys.insert(affectedKeys.end(), conditional->beginFrontals(), conditional->endFrontals()); gttoc(affectedKeys); @@ -758,8 +756,7 @@ ISAM2Result ISAM2::update( // 7. Linearize new factors if (params_.cacheLinearizedFactors) { gttic(linearize); - GaussianFactorGraph::shared_ptr linearFactors = - newFactors.linearize(theta_); + auto linearFactors = newFactors.linearize(theta_); if (params_.findUnusedFactorSlots) { linearFactors_.resize(nonlinearFactors_.size()); for (size_t newFactorI = 0; newFactorI < newFactors.size(); ++newFactorI) @@ -822,11 +819,35 @@ void ISAM2::marginalizeLeaves( // multimap marginalFactors; map > marginalFactors; + // Keep track of variables removed in subtrees + KeySet leafKeysRemoved; + // Keep track of factors that get summarized by removing cliques KeySet factorIndicesToRemove; - // Keep track of variables removed in subtrees - KeySet leafKeysRemoved; + // Remove the subtree and throw away the cliques + auto trackingRemoveSubtree = [&](const sharedClique& subtreeRoot) { + const Cliques removedCliques = this->removeSubtree(subtreeRoot); + for (const sharedClique& removedClique : removedCliques) { + auto cg = removedClique->conditional(); + marginalFactors.erase(cg->front()); + leafKeysRemoved.insert(cg->beginFrontals(), cg->endFrontals()); + for (Key frontal : cg->frontals()) { + // Add to factors to remove + const auto& involved = variableIndex_[frontal]; + factorIndicesToRemove.insert(involved.begin(), involved.end()); +#if !defined(NDEBUG) + // Check for non-leaf keys + if (!leafKeys.exists(frontal)) + throw std::runtime_error( + "Requesting to marginalize variables that are not leaves, " + "the ISAM2 object is now in an inconsistent state so should " + "no longer be used."); +#endif + } + } + return removedCliques; + }; // Remove each variable and its subtrees for (Key j : leafKeys) { @@ -858,7 +879,7 @@ void ISAM2::marginalizeLeaves( if (marginalizeEntireClique) { // Remove the whole clique and its subtree, and keep the marginal // factor. - GaussianFactor::shared_ptr marginalFactor = clique->cachedFactor(); + auto marginalFactor = clique->cachedFactor(); // 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 @@ -867,27 +888,7 @@ void ISAM2::marginalizeLeaves( marginalFactor); // Now remove this clique and its subtree - all of its marginal // information has been stored in marginalFactors. - const Cliques removedCliques = this->removeSubtree( - clique); // Remove the subtree and throw away the cliques - for (const sharedClique& removedClique : removedCliques) { - marginalFactors.erase(removedClique->conditional()->front()); - leafKeysRemoved.insert(removedClique->conditional()->beginFrontals(), - removedClique->conditional()->endFrontals()); - for (Key frontal : removedClique->conditional()->frontals()) { - // Add to factors to remove - const VariableIndex::Factors& involvedFactors = - variableIndex_[frontal]; - factorIndicesToRemove.insert(involvedFactors.begin(), - involvedFactors.end()); - - // Check for non-leaf keys - if (!leafKeys.exists(frontal)) - throw runtime_error( - "Requesting to marginalize variables that are not leaves, " - "the ISAM2 object is now in an inconsistent state so should " - "no longer be used."); - } - } + trackingRemoveSubtree(clique); } else { // Reeliminate the current clique and the marginals from its children, // then keep only the marginal on the non-marginalized variables. We @@ -910,31 +911,10 @@ void ISAM2::marginalizeLeaves( } } Cliques childrenRemoved; - for (const sharedClique& childToRemove : subtreesToRemove) { - const Cliques removedCliques = this->removeSubtree( - childToRemove); // Remove the subtree and throw away the cliques - childrenRemoved.insert(childrenRemoved.end(), removedCliques.begin(), - removedCliques.end()); - for (const sharedClique& removedClique : removedCliques) { - marginalFactors.erase(removedClique->conditional()->front()); - leafKeysRemoved.insert( - removedClique->conditional()->beginFrontals(), - removedClique->conditional()->endFrontals()); - for (Key frontal : removedClique->conditional()->frontals()) { - // Add to factors to remove - const VariableIndex::Factors& involvedFactors = - variableIndex_[frontal]; - factorIndicesToRemove.insert(involvedFactors.begin(), - involvedFactors.end()); - - // Check for non-leaf keys - if (!leafKeys.exists(frontal)) - throw runtime_error( - "Requesting to marginalize variables that are not leaves, " - "the ISAM2 object is now in an inconsistent state so " - "should no longer be used."); - } - } + for (const sharedClique& subtree : subtreesToRemove) { + const Cliques removed = trackingRemoveSubtree(subtree); + childrenRemoved.insert(childrenRemoved.end(), removed.begin(), + removed.end()); } // Add the factors that are pulled into the current clique by the @@ -969,9 +949,8 @@ void ISAM2::marginalizeLeaves( std::set_intersection(cliqueFrontals.begin(), cliqueFrontals.end(), leafKeys.begin(), leafKeys.end(), std::back_inserter(cliqueFrontalsToEliminate)); - pair - eliminationResult1 = params_.getEliminationFunction()( - graph, Ordering(cliqueFrontalsToEliminate)); + auto eliminationResult1 = params_.getEliminationFunction()( + graph, Ordering(cliqueFrontalsToEliminate)); // Add the resulting marginal if (eliminationResult1.second) @@ -997,13 +976,11 @@ void ISAM2::marginalizeLeaves( originalKeys.end()); clique->conditional()->nrFrontals() -= nToRemove; - // Add to factors to remove factors involved in frontals of current - // clique + // Add to factorIndicesToRemove any factors involved in frontals of + // current clique for (Key frontal : cliqueFrontalsToEliminate) { - const VariableIndex::Factors& involvedFactors = - variableIndex_[frontal]; - factorIndicesToRemove.insert(involvedFactors.begin(), - involvedFactors.end()); + const auto& involved = variableIndex_[frontal]; + factorIndicesToRemove.insert(involved.begin(), involved.end()); } // Add removed keys @@ -1018,9 +995,8 @@ void ISAM2::marginalizeLeaves( // Gather factors to add - the new marginal factors GaussianFactorGraph factorsToAdd; - typedef pair > Key_Factors; - for (const Key_Factors& key_factors : marginalFactors) { - for (const GaussianFactor::shared_ptr& factor : key_factors.second) { + for (const auto& key_factors : marginalFactors) { + for (const auto& factor : key_factors.second) { if (factor) { factorsToAdd.push_back(factor); if (marginalFactorsIndices) From 3e0ee2052d0c333f004179e85cefce97cbafc4f3 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 30 Sep 2018 15:26:36 -0400 Subject: [PATCH 22/51] Small cleanup of copy/paste call --- gtsam/nonlinear/ISAM2.cpp | 24 ++++++++++-------------- 1 file changed, 10 insertions(+), 14 deletions(-) diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index cad9add9f..df07050de 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -943,8 +943,8 @@ void ISAM2::marginalizeLeaves( // Reeliminate the linear graph to get the marginal and discard the // conditional - const KeySet cliqueFrontals(clique->conditional()->beginFrontals(), - clique->conditional()->endFrontals()); + auto cg = clique->conditional(); + const KeySet cliqueFrontals(cg->beginFrontals(), cg->endFrontals()); FastVector cliqueFrontalsToEliminate; std::set_intersection(cliqueFrontals.begin(), cliqueFrontals.end(), leafKeys.begin(), leafKeys.end(), @@ -954,27 +954,23 @@ void ISAM2::marginalizeLeaves( // Add the resulting marginal if (eliminationResult1.second) - marginalFactors[clique->conditional()->front()].push_back( - eliminationResult1.second); + marginalFactors[cg->front()].push_back(eliminationResult1.second); // Split the current clique // Find the position of the last leaf key in this clique DenseIndex nToRemove = 0; - while (leafKeys.exists(clique->conditional()->keys()[nToRemove])) - ++nToRemove; + while (leafKeys.exists(cg->keys()[nToRemove])) ++nToRemove; // Make the clique's matrix appear as a subset - const DenseIndex dimToRemove = - clique->conditional()->matrixObject().offset(nToRemove); - clique->conditional()->matrixObject().firstBlock() = nToRemove; - clique->conditional()->matrixObject().rowStart() = dimToRemove; + const DenseIndex dimToRemove = cg->matrixObject().offset(nToRemove); + cg->matrixObject().firstBlock() = nToRemove; + cg->matrixObject().rowStart() = dimToRemove; // Change the keys in the clique FastVector originalKeys; - originalKeys.swap(clique->conditional()->keys()); - clique->conditional()->keys().assign(originalKeys.begin() + nToRemove, - originalKeys.end()); - clique->conditional()->nrFrontals() -= nToRemove; + originalKeys.swap(cg->keys()); + cg->keys().assign(originalKeys.begin() + nToRemove, originalKeys.end()); + cg->nrFrontals() -= nToRemove; // Add to factorIndicesToRemove any factors involved in frontals of // current clique From efa35e6a82aed346bff5e2aed6851f1f089d14b6 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 30 Sep 2018 17:22:53 -0400 Subject: [PATCH 23/51] Cleaned up example --- examples/VisualISAM2Example.cpp | 88 ++++++++++++++++++--------------- 1 file changed, 49 insertions(+), 39 deletions(-) diff --git a/examples/VisualISAM2Example.cpp b/examples/VisualISAM2Example.cpp index 157768be7..751b776f6 100644 --- a/examples/VisualISAM2Example.cpp +++ b/examples/VisualISAM2Example.cpp @@ -11,8 +11,8 @@ /** * @file VisualISAM2Example.cpp - * @brief A visualSLAM example for the structure-from-motion problem on a simulated dataset - * This version uses iSAM2 to solve the problem incrementally + * @brief A visualSLAM example for the structure-from-motion problem on a + * simulated dataset This version uses iSAM2 to solve the problem incrementally * @author Duy-Nguyen Ta */ @@ -25,27 +25,28 @@ // For loading the data #include "SFMdata.h" -// Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y). +// Camera observations of landmarks will be stored as Point2 (x, y). #include -// Each variable in the system (poses and landmarks) must be identified with a unique key. -// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1). -// Here we will use Symbols +// Each variable in the system (poses and landmarks) must be identified with a +// unique key. We can either use simple integer keys (1, 2, 3, ...) or symbols +// (X1, X2, L1). Here we will use Symbols #include -// We want to use iSAM2 to solve the structure-from-motion problem incrementally, so -// include iSAM2 here +// We want to use iSAM2 to solve the structure-from-motion problem +// incrementally, so include iSAM2 here #include -// iSAM2 requires as input a set set of new factors to be added stored in a factor graph, -// and initial guesses for any new variables used in the added factors +// iSAM2 requires as input a set of new factors to be added stored in a factor +// graph, and initial guesses for any new variables used in the added factors #include #include -// In GTSAM, measurement functions are represented as 'factors'. Several common factors -// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems. -// Here we will use Projection factors to model the camera's landmark observations. -// Also, we will initialize the robot at some location using a Prior factor. +// In GTSAM, measurement functions are represented as 'factors'. Several common +// factors have been provided with the library for solving robotics/SLAM/Bundle +// Adjustment problems. Here we will use Projection factors to model the +// camera's landmark observations. Also, we will initialize the robot at some +// location using a Prior factor. #include #include @@ -56,12 +57,11 @@ using namespace gtsam; /* ************************************************************************* */ int main(int argc, char* argv[]) { - // Define the camera calibration parameters Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)); - // Define the camera observation noise model - noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v + // Define the camera observation noise model, 1 pixel stddev + auto measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); // Create the set of ground-truth landmarks vector points = createPoints(); @@ -69,10 +69,12 @@ int main(int argc, char* argv[]) { // Create the set of ground-truth poses vector poses = createPoses(); - // Create an iSAM2 object. Unlike iSAM1, which performs periodic batch steps to maintain proper linearization - // and efficient variable ordering, iSAM2 performs partial relinearization/reordering at each step. A parameter - // structure is available that allows the user to set various properties, such as the relinearization threshold - // and type of linear solver. For this example, we we set the relinearization threshold small so the iSAM2 result + // Create an iSAM2 object. Unlike iSAM1, which performs periodic batch steps + // to maintain proper linearization and efficient variable ordering, iSAM2 + // performs partial relinearization/reordering at each step. A parameter + // structure is available that allows the user to set various properties, such + // as the relinearization threshold and type of linear solver. For this + // example, we we set the relinearization threshold small so the iSAM2 result // will approach the batch result. ISAM2Params parameters; parameters.relinearizeThreshold = 0.01; @@ -83,44 +85,52 @@ int main(int argc, char* argv[]) { NonlinearFactorGraph graph; Values initialEstimate; - // Loop over the different poses, adding the observations to iSAM incrementally + // Loop over the poses, adding the observations to iSAM incrementally for (size_t i = 0; i < poses.size(); ++i) { - // Add factors for each landmark observation for (size_t j = 0; j < points.size(); ++j) { SimpleCamera camera(poses[i], *K); Point2 measurement = camera.project(points[j]); - graph.emplace_shared >(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K); + graph.emplace_shared >( + measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K); } // Add an initial guess for the current pose // Intentionally initialize the variables off from the ground truth - initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)))); + static Pose3 kDeltaPose(Rot3::Rodrigues(-0.1, 0.2, 0.25), + Point3(0.05, -0.10, 0.20)); + initialEstimate.insert(Symbol('x', i), poses[i] * kDeltaPose); - // If this is the first iteration, add a prior on the first pose to set the coordinate frame - // and a prior on the first landmark to set the scale - // Also, as iSAM solves incrementally, we must wait until each is observed at least twice before - // adding it to iSAM. - if( i == 0) { - // Add a prior on pose x0 - noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3),Vector3::Constant(0.1)).finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw - graph.emplace_shared >(Symbol('x', 0), poses[0], poseNoise); + // If this is the first iteration, add a prior on the first pose to set the + // coordinate frame and a prior on the first landmark to set the scale Also, + // as iSAM solves incrementally, we must wait until each is observed at + // least twice before adding it to iSAM. + if (i == 0) { + // Add a prior on pose x0, 30cm std on x,y,z and 0.1 rad on roll,pitch,yaw + static auto kPosePrior = noiseModel::Diagonal::Sigmas( + (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)) + .finished()); + graph.emplace_shared >(Symbol('x', 0), poses[0], + kPosePrior); // Add a prior on landmark l0 - noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1); - graph.emplace_shared >(Symbol('l', 0), points[0], pointNoise); // add directly to graph + static auto kPointPrior = noiseModel::Isotropic::Sigma(3, 0.1); + graph.emplace_shared >(Symbol('l', 0), points[0], + kPointPrior); // Add initial guesses to all observed landmarks // Intentionally initialize the variables off from the ground truth + static Point3 kDeltaPoint(-0.25, 0.20, 0.15); for (size_t j = 0; j < points.size(); ++j) - initialEstimate.insert(Symbol('l', j), points[j] +Point3(-0.25, 0.20, 0.15)); + initialEstimate.insert(Symbol('l', j), points[j] + kDeltaPoint); } else { // Update iSAM with the new factors isam.update(graph, initialEstimate); - // Each call to iSAM2 update(*) performs one iteration of the iterative nonlinear solver. - // If accuracy is desired at the expense of time, update(*) can be called additional times - // to perform multiple optimizer iterations every step. + // Each call to iSAM2 update(*) performs one iteration of the iterative + // nonlinear solver. If accuracy is desired at the expense of time, + // update(*) can be called additional times to perform multiple optimizer + // iterations every step. isam.update(); Values currentEstimate = isam.calculateEstimate(); cout << "****************************************************" << endl; From 3116fd30b9e4a9530dca07d9c95ea6ff3dc2a9c3 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 2 Oct 2018 21:53:49 -0400 Subject: [PATCH 24/51] Fixed lint errors --- gtsam/linear/GaussianConditional.cpp | 62 +++++++++++++--------------- 1 file changed, 28 insertions(+), 34 deletions(-) diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index eefb6302f..60187d129 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -12,11 +12,13 @@ /** * @file GaussianConditional.cpp * @brief Conditional Gaussian Base class - * @author Christian Potthast + * @author Christian Potthast, Frank Dellaert */ -#include -#include +#include +#include +#include + #include #ifdef __GNUC__ #pragma GCC diagnostic push @@ -28,9 +30,9 @@ #pragma GCC diagnostic pop #endif -#include -#include -#include +#include +#include +#include using namespace std; @@ -54,38 +56,36 @@ namespace gtsam { BaseFactor(key, R, name1, S, name2, T, d, sigmas), BaseConditional(1) {} /* ************************************************************************* */ - void GaussianConditional::print(const string &s, const KeyFormatter& formatter) const - { + void GaussianConditional::print(const string &s, const KeyFormatter& formatter) const { cout << s << " Conditional density "; - for(const_iterator it = beginFrontals(); it != endFrontals(); ++it) { + for (const_iterator it = beginFrontals(); it != endFrontals(); ++it) { cout << (boost::format("[%1%]")%(formatter(*it))).str() << " "; } cout << endl; cout << formatMatrixIndented(" R = ", get_R()) << endl; - for(const_iterator it = beginParents() ; it != endParents() ; ++it ) { + for (const_iterator it = beginParents() ; it != endParents() ; ++it) { cout << formatMatrixIndented((boost::format(" S[%1%] = ")%(formatter(*it))).str(), getA(it)) << endl; } cout << formatMatrixIndented(" d = ", getb(), true) << "\n"; - if(model_) + if (model_) model_->print(" Noise model: "); else cout << " No noise model" << endl; } /* ************************************************************************* */ - bool GaussianConditional::equals(const GaussianFactor& f, double tol) const - { - if (const GaussianConditional* c = dynamic_cast(&f)) - { + bool GaussianConditional::equals(const GaussianFactor& f, double tol) const { + if (const GaussianConditional* c = dynamic_cast(&f)) { // check if the size of the parents_ map is the same if (parents().size() != c->parents().size()) return false; // check if R_ and d_ are linear independent for (DenseIndex i = 0; i < Ab_.rows(); i++) { - list rows1; rows1.push_back(Vector(get_R().row(i))); - list rows2; rows2.push_back(Vector(c->get_R().row(i))); + list rows1, rows2; + rows1.push_back(Vector(get_R().row(i))); + rows2.push_back(Vector(c->get_R().row(i))); // check if the matrices are the same // iterate over the parents_ map @@ -109,16 +109,13 @@ namespace gtsam { return false; return true; - } - else - { + } else { return false; } } /* ************************************************************************* */ - VectorValues GaussianConditional::solve(const VectorValues& x) const - { + VectorValues GaussianConditional::solve(const VectorValues& x) const { // Concatenate all vector values that correspond to parent variables const Vector xS = x.vector(FastVector(beginParents(), endParents())); @@ -146,8 +143,7 @@ namespace gtsam { /* ************************************************************************* */ VectorValues GaussianConditional::solveOtherRHS( - const VectorValues& parents, const VectorValues& rhs) const - { + const VectorValues& parents, const VectorValues& rhs) const { // Concatenate all vector values that correspond to parent variables Vector xS = parents.vector(FastVector(beginParents(), endParents())); @@ -159,13 +155,13 @@ namespace gtsam { Vector soln = get_R().triangularView().solve(xS); // Scale by sigmas - if(model_) + if (model_) soln.array() *= model_->sigmas().array(); // Insert solution into a VectorValues VectorValues result; DenseIndex vectorPosition = 0; - for(const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) { + for (const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) { result.insert(*frontal, soln.segment(vectorPosition, getDim(frontal))); vectorPosition += getDim(frontal); } @@ -174,8 +170,7 @@ namespace gtsam { } /* ************************************************************************* */ - void GaussianConditional::solveTransposeInPlace(VectorValues& gy) const - { + void GaussianConditional::solveTransposeInPlace(VectorValues& gy) const { Vector frontalVec = gy.vector(FastVector(beginFrontals(), endFrontals())); frontalVec = gtsam::backSubstituteUpper(frontalVec, Matrix(get_R())); @@ -186,25 +181,24 @@ namespace gtsam { gy[*it] += -1.0 * Matrix(getA(it)).transpose() * frontalVec; // Scale by sigmas - if(model_) + if (model_) frontalVec.array() *= model_->sigmas().array(); // Write frontal solution into a VectorValues DenseIndex vectorPosition = 0; - for(const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) { + for (const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) { gy[*frontal] = frontalVec.segment(vectorPosition, getDim(frontal)); vectorPosition += getDim(frontal); } } /* ************************************************************************* */ - void GaussianConditional::scaleFrontalsBySigma(VectorValues& gy) const - { + void GaussianConditional::scaleFrontalsBySigma(VectorValues& gy) const { DenseIndex vectorPosition = 0; - for(const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) { + for (const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) { gy[*frontal].array() *= model_->sigmas().segment(vectorPosition, getDim(frontal)).array(); vectorPosition += getDim(frontal); } } -} +} // namespace gtsam From 566315f47da97a5735d7fdb3c4cbb0388047fba2 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 2 Oct 2018 23:25:27 -0400 Subject: [PATCH 25/51] Solved overloading --- tests/testNonlinearOptimizer.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index 9ddbbb1b2..3d30d6880 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -20,13 +20,12 @@ #include #include #include -#include -#include #include #include #include #include #include +#include #include #include @@ -394,19 +393,20 @@ class IterativeLM: public LevenbergMarquardtOptimizer { /// Solver specific parameters ConjugateGradientParameters cgParams_; + Values initial_; public: /// Constructor IterativeLM(const NonlinearFactorGraph& graph, const Values& initialValues, const ConjugateGradientParameters &p, const LevenbergMarquardtParams& params = LevenbergMarquardtParams::LegacyDefaults()) : - LevenbergMarquardtOptimizer(graph, initialValues, params), cgParams_(p) { + LevenbergMarquardtOptimizer(graph, initialValues, params), cgParams_(p), initial_(initialValues) { } /// Solve that uses conjugate gradient virtual VectorValues solve(const GaussianFactorGraph &gfg, - const Values& initial, const NonlinearOptimizerParams& params) const { - VectorValues zeros = initial.zeroVectors(); + const NonlinearOptimizerParams& params) const { + VectorValues zeros = initial_.zeroVectors(); return conjugateGradientDescent(gfg, zeros, cgParams_); } }; From 366b2485c1d53c21d887a08357d6383c4dfe8b21 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 8 Oct 2018 21:00:22 -0400 Subject: [PATCH 26/51] Fixed warnings flagged by Ubuntu g++ --- gtsam/3rdparty/CCOLAMD/Source/ccolamd.c | 11 ++-- gtsam/3rdparty/metis/GKlib/csr.c | 32 ++++++----- gtsam/3rdparty/metis/GKlib/getopt.c | 71 +++++++++++++------------ gtsam/inference/BayesTree-inst.h | 14 ++--- 4 files changed, 66 insertions(+), 62 deletions(-) diff --git a/gtsam/3rdparty/CCOLAMD/Source/ccolamd.c b/gtsam/3rdparty/CCOLAMD/Source/ccolamd.c index 9a08e3ea8..d5e8da2f0 100644 --- a/gtsam/3rdparty/CCOLAMD/Source/ccolamd.c +++ b/gtsam/3rdparty/CCOLAMD/Source/ccolamd.c @@ -1560,8 +1560,9 @@ PUBLIC Int CCOLAMD_2 /* returns TRUE if successful, FALSE otherwise */ Int *dead_cols ; Int set1 ; Int set2 ; +#ifndef NDEBUG Int cs ; - +#endif int ok ; #ifndef NDEBUG @@ -1909,8 +1910,10 @@ PUBLIC Int CCOLAMD_2 /* returns TRUE if successful, FALSE otherwise */ p [k] = col ; ASSERT (A [col] == EMPTY) ; - cs = CMEMBER (col) ; +#ifndef NDEBUG + cs = CMEMBER (col) ; ASSERT (k >= cset_start [cs] && k < cset_start [cs+1]) ; +#endif A [col] = k ; k++ ; @@ -1926,11 +1929,11 @@ PUBLIC Int CCOLAMD_2 /* returns TRUE if successful, FALSE otherwise */ if (A [col] == EMPTY) { k = Col [col].shared2.order ; - cs = CMEMBER (col) ; #ifndef NDEBUG + cs = CMEMBER (col) ; dead_cols [cs]-- ; -#endif ASSERT (k >= cset_start [cs] && k < cset_start [cs+1]) ; +#endif DEBUG1 (("ccolamd output ordering: k "ID" col "ID " (dense or null col)\n", k, col)) ; p [k] = col ; diff --git a/gtsam/3rdparty/metis/GKlib/csr.c b/gtsam/3rdparty/metis/GKlib/csr.c index a19d793bd..57934ee9d 100644 --- a/gtsam/3rdparty/metis/GKlib/csr.c +++ b/gtsam/3rdparty/metis/GKlib/csr.c @@ -1323,29 +1323,27 @@ void gk_csr_Normalize(gk_csr_t *mat, int what, int norm) ssize_t *ptr; float *val, sum; - if (what&GK_CSR_ROW && mat->rowval) { - n = mat->nrows; + if (what & GK_CSR_ROW && mat->rowval) { + n = mat->nrows; ptr = mat->rowptr; val = mat->rowval; - #pragma omp parallel if (ptr[n] > OMPMINOPS) +#pragma omp parallel if (ptr[n] > OMPMINOPS) { - #pragma omp for private(j,sum) schedule(static) - for (i=0; i 0 */ +#pragma omp for private(j, sum) schedule(static) + for (i = 0; i < n; i++) { + for (sum = 0.0, j = ptr[i]; j < ptr[i + 1]; j++) { + if (norm == 2) + sum += val[j] * val[j]; + else if (norm == 1) + sum += val[j]; /* assume val[j] > 0 */ } if (sum > 0) { - if (norm == 2) - sum=1.0/sqrt(sum); - else if (norm == 1) - sum=1.0/sum; - for (j=ptr[i]; jconditional_->frontals()) { - if(!first) parent += ","; first = false; + for (Key index : clique->conditional_->frontals()) { + if (!first) parent += ","; + first = false; parent += indexFormatter(index); } - if(clique->parent()){ + if (clique->parent()) { parent += " : "; s << parentnum << "->" << num << "\n"; } first = true; - for(Key sep: clique->conditional_->parents()) { - if(!first) parent += ","; first = false; + for (Key sep : clique->conditional_->parents()) { + if (!first) parent += ","; + first = false; parent += indexFormatter(sep); } parent += "\"];\n"; s << parent; parentnum = num; - for(sharedClique c: clique->children) { + for (sharedClique c : clique->children) { num++; saveGraph(s, c, indexFormatter, parentnum); } From 4faac98f512314da1d5e0033b3897d43e2d5addd Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 8 Oct 2018 21:00:49 -0400 Subject: [PATCH 27/51] Pointed pipeline to new docker image, using latest system Eigen3 --- bitbucket-pipelines.yml | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/bitbucket-pipelines.yml b/bitbucket-pipelines.yml index 2d8b7f061..288a40b14 100644 --- a/bitbucket-pipelines.yml +++ b/bitbucket-pipelines.yml @@ -3,14 +3,16 @@ # Only use spaces to indent your .yml configuration. # ----- # You can specify a custom docker image from Docker Hub as your build environment. -image: ilssim/cmake-boost-qt5 +image: dellaert/ubuntu-boost-tbb-eigen3:latest pipelines: default: - step: + caches: + - docker script: # Modify the commands below to build your repository. - mkdir build - cd build - - cmake .. + - cmake -DGTSAM_USE_SYSTEM_EIGEN=ON -DGTSAM_USE_EIGEN_MKL=OFF .. - make - make check \ No newline at end of file From e742cde2c8a53ba905f0f9ba44ae447a5d02d2a2 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 8 Oct 2018 21:03:45 -0400 Subject: [PATCH 28/51] Fixed pipeline config --- bitbucket-pipelines.yml | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/bitbucket-pipelines.yml b/bitbucket-pipelines.yml index 288a40b14..33204aea2 100644 --- a/bitbucket-pipelines.yml +++ b/bitbucket-pipelines.yml @@ -1,15 +1,12 @@ -# This is a sample build configuration for C++ – Make. -# Check our guides at https://confluence.atlassian.com/x/5Q4SMw for more examples. -# Only use spaces to indent your .yml configuration. +# Built from sample configuration for C++ – Make. +# Check https://confluence.atlassian.com/x/5Q4SMw for more examples. # ----- -# You can specify a custom docker image from Docker Hub as your build environment. +# Our custom docker image from Docker Hub as the build environment. image: dellaert/ubuntu-boost-tbb-eigen3:latest pipelines: default: - step: - caches: - - docker script: # Modify the commands below to build your repository. - mkdir build - cd build From 5d49177c8a50f8b0d394f93dfee4400150d45fe3 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 8 Oct 2018 21:38:11 -0400 Subject: [PATCH 29/51] Added docker file to build pipeline docker image --- docker/ubuntu-boost-tbb-eigen3/Dockerfile | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) create mode 100644 docker/ubuntu-boost-tbb-eigen3/Dockerfile diff --git a/docker/ubuntu-boost-tbb-eigen3/Dockerfile b/docker/ubuntu-boost-tbb-eigen3/Dockerfile new file mode 100644 index 000000000..b400c0538 --- /dev/null +++ b/docker/ubuntu-boost-tbb-eigen3/Dockerfile @@ -0,0 +1,18 @@ +# Get the base Ubuntu image from Docker Hub +FROM ubuntu:latest + +# Update apps on the base image +RUN apt-get -y update && apt-get install -y + +# Install C++ +RUN apt-get -y install build-essential + +# Install boost and cmake +RUN apt-get -y install libboost-all-dev cmake + +# Install TBB +RUN apt-get -y install libtbb-dev + +# Install latest Eigen +RUN apt-get install -y libeigen3-dev + From eb4659bcb66d7f7a55dd21e7f66cf10c449f0b62 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 8 Oct 2018 21:38:32 -0400 Subject: [PATCH 30/51] Added -j4 flags --- bitbucket-pipelines.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/bitbucket-pipelines.yml b/bitbucket-pipelines.yml index 33204aea2..8a8d5dd14 100644 --- a/bitbucket-pipelines.yml +++ b/bitbucket-pipelines.yml @@ -11,5 +11,5 @@ pipelines: - mkdir build - cd build - cmake -DGTSAM_USE_SYSTEM_EIGEN=ON -DGTSAM_USE_EIGEN_MKL=OFF .. - - make - - make check \ No newline at end of file + - make -j4 + - make -j4 check \ No newline at end of file From ca80678ffcdb069e0301e8c49a1efaf8ebabc9a3 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 8 Oct 2018 21:38:50 -0400 Subject: [PATCH 31/51] Fixed more warnings --- examples/SolverComparer.cpp | 2 +- gtsam/3rdparty/metis/GKlib/csr.c | 31 +++++++++++++++---------------- gtsam/base/cholesky.cpp | 5 +++-- 3 files changed, 19 insertions(+), 19 deletions(-) diff --git a/examples/SolverComparer.cpp b/examples/SolverComparer.cpp index deaf3b781..63c512edb 100644 --- a/examples/SolverComparer.cpp +++ b/examples/SolverComparer.cpp @@ -206,7 +206,7 @@ int main(int argc, char *argv[]) { } #ifdef GTSAM_USE_TBB - std::auto_ptr init; + std::unique_ptr init; if(nThreads > 0) { cout << "Using " << nThreads << " threads" << endl; init.reset(new tbb::task_scheduler_init(nThreads)); diff --git a/gtsam/3rdparty/metis/GKlib/csr.c b/gtsam/3rdparty/metis/GKlib/csr.c index 57934ee9d..4f6213c49 100644 --- a/gtsam/3rdparty/metis/GKlib/csr.c +++ b/gtsam/3rdparty/metis/GKlib/csr.c @@ -1349,27 +1349,26 @@ void gk_csr_Normalize(gk_csr_t *mat, int what, int norm) } } - if (what&GK_CSR_COL && mat->colval) { - n = mat->ncols; + if (what & GK_CSR_COL && mat->colval) { + n = mat->ncols; ptr = mat->colptr; val = mat->colval; - #pragma omp parallel if (ptr[n] > OMPMINOPS) +#pragma omp parallel if (ptr[n] > OMPMINOPS) { - #pragma omp for private(j,sum) schedule(static) - for (i=0; i 0) { - if (norm == 2) - sum=1.0/sqrt(sum); - else if (norm == 1) - sum=1.0/sum; - for (j=ptr[i]; j= 0 && nFrontal <= size_t(n)); + assert(ABC.rows() >= topleft); + const size_t n = static_cast(ABC.rows() - topleft); + assert(nFrontal <= size_t(n)); // Create views on blocks auto A = ABC.block(topleft, topleft, nFrontal, nFrontal); From 90373d878666e3e5719077512e965456c4b62720 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 8 Oct 2018 21:51:20 -0400 Subject: [PATCH 32/51] Solved overloading, cherry-picked from fix/isam2 branch --- tests/testNonlinearOptimizer.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index 9ddbbb1b2..3d30d6880 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -20,13 +20,12 @@ #include #include #include -#include -#include #include #include #include #include #include +#include #include #include @@ -394,19 +393,20 @@ class IterativeLM: public LevenbergMarquardtOptimizer { /// Solver specific parameters ConjugateGradientParameters cgParams_; + Values initial_; public: /// Constructor IterativeLM(const NonlinearFactorGraph& graph, const Values& initialValues, const ConjugateGradientParameters &p, const LevenbergMarquardtParams& params = LevenbergMarquardtParams::LegacyDefaults()) : - LevenbergMarquardtOptimizer(graph, initialValues, params), cgParams_(p) { + LevenbergMarquardtOptimizer(graph, initialValues, params), cgParams_(p), initial_(initialValues) { } /// Solve that uses conjugate gradient virtual VectorValues solve(const GaussianFactorGraph &gfg, - const Values& initial, const NonlinearOptimizerParams& params) const { - VectorValues zeros = initial.zeroVectors(); + const NonlinearOptimizerParams& params) const { + VectorValues zeros = initial_.zeroVectors(); return conjugateGradientDescent(gfg, zeros, cgParams_); } }; From f60e3332a4a3fb1a0519f1a832de23ad9fa1b5c8 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 8 Oct 2018 21:51:42 -0400 Subject: [PATCH 33/51] Made explicit float to avoid ambiguous overload. --- gtsam/geometry/tests/testPoint2.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/gtsam/geometry/tests/testPoint2.cpp b/gtsam/geometry/tests/testPoint2.cpp index e2a5bcdea..8b9e8a7e6 100644 --- a/gtsam/geometry/tests/testPoint2.cpp +++ b/gtsam/geometry/tests/testPoint2.cpp @@ -100,12 +100,12 @@ TEST( Point2, expmap) { /* ************************************************************************* */ TEST( Point2, arithmetic) { - EXPECT(assert_equal( Point2(-5,-6), -Point2(5,6) )); - EXPECT(assert_equal( Point2(5,6), Point2(4,5)+Point2(1,1))); - EXPECT(assert_equal( Point2(3,4), Point2(4,5)-Point2(1,1) )); - EXPECT(assert_equal( Point2(8,6), Point2(4,3)*2)); - EXPECT(assert_equal( Point2(4,6), 2*Point2(2,3))); - EXPECT(assert_equal( Point2(2,3), Point2(4,6)/2)); + EXPECT(assert_equal(Point2(-5, -6), -Point2(5, 6))); + EXPECT(assert_equal(Point2(5, 6), Point2(4, 5) + Point2(1, 1))); + EXPECT(assert_equal(Point2(3, 4), Point2(4, 5) - Point2(1, 1))); + EXPECT(assert_equal(Point2(8, 6), Point2(4, 3) * 2)); + EXPECT(assert_equal(Point2(4, 6), 2.0 * Point2(2, 3))); + EXPECT(assert_equal(Point2(2, 3), Point2(4, 6) / 2)); } /* ************************************************************************* */ From 1bc479fc3c992b8009579ee8a1eaf61bfabb375d Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 8 Oct 2018 22:54:36 -0400 Subject: [PATCH 34/51] Fixed some lint errors --- gtsam/geometry/Unit3.cpp | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index dacb5c3fd..00edc1ad4 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -21,7 +21,7 @@ #include #include -#include // for GTSAM_USE_TBB +#include // for GTSAM_USE_TBB #ifdef __clang__ # pragma clang diagnostic push @@ -36,13 +36,14 @@ #include #include #include +#include using namespace std; namespace gtsam { /* ************************************************************************* */ -Unit3 Unit3::FromPoint3(const Point3& point, OptionalJacobian<2,3> H) { +Unit3 Unit3::FromPoint3(const Point3& point, OptionalJacobian<2, 3> H) { // 3*3 Derivative of representation with respect to point is 3*3: Matrix3 D_p_point; Unit3 direction; @@ -54,7 +55,7 @@ Unit3 Unit3::FromPoint3(const Point3& point, OptionalJacobian<2,3> H) { /* ************************************************************************* */ Unit3 Unit3::Random(boost::mt19937 & rng) { - // TODO allow any engine without including all of boost :-( + // TODO(dellaert): allow any engine without including all of boost :-( boost::uniform_on_sphere randomDirection(3); // This variate_generator object is required for versions of boost somewhere // around 1.46, instead of drawing directly using boost::uniform_on_sphere(rng). @@ -161,7 +162,8 @@ Matrix3 Unit3::skew() const { } /* ************************************************************************* */ -double Unit3::dot(const Unit3& q, OptionalJacobian<1,2> H_p, OptionalJacobian<1,2> H_q) const { +double Unit3::dot(const Unit3& q, OptionalJacobian<1, 2> H_p, + OptionalJacobian<1, 2> H_q) const { // Get the unit vectors of each, and the derivative. Matrix32 H_pn_p; Point3 pn = point3(H_p ? &H_pn_p : nullptr); @@ -185,7 +187,7 @@ double Unit3::dot(const Unit3& q, OptionalJacobian<1,2> H_p, OptionalJacobian<1, } /* ************************************************************************* */ -Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2,2> H_q) const { +Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2, 2> H_q) const { // 2D error is equal to B'*q, as B is 3x2 matrix and q is 3x1 const Vector2 xi = basis().transpose() * q.p_; if (H_q) { @@ -195,7 +197,8 @@ Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2,2> H_q) const { } /* ************************************************************************* */ -Vector2 Unit3::errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p, OptionalJacobian<2, 2> H_q) const { +Vector2 Unit3::errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p, + OptionalJacobian<2, 2> H_q) const { // Get the point3 of this, and the derivative. Matrix32 H_qn_q; const Point3 qn = q.point3(H_q ? &H_qn_q : nullptr); @@ -230,7 +233,7 @@ Vector2 Unit3::errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p, OptionalJ } /* ************************************************************************* */ -double Unit3::distance(const Unit3& q, OptionalJacobian<1,2> H) const { +double Unit3::distance(const Unit3& q, OptionalJacobian<1, 2> H) const { Matrix2 H_xi_q; const Vector2 xi = error(q, H ? &H_xi_q : nullptr); const double theta = xi.norm(); @@ -277,4 +280,4 @@ Vector2 Unit3::localCoordinates(const Unit3& other) const { } /* ************************************************************************* */ -} +} // namespace gtsam From b9f080456c4164f11812c6ae1f960bd6adb741ca Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 8 Oct 2018 22:54:48 -0400 Subject: [PATCH 35/51] Catch exception by value --- wrap/FileWriter.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/wrap/FileWriter.cpp b/wrap/FileWriter.cpp index 783661819..2c3843b37 100644 --- a/wrap/FileWriter.cpp +++ b/wrap/FileWriter.cpp @@ -29,7 +29,7 @@ void FileWriter::emit(bool add_header, bool force_overwrite) const { bool file_exists = true; try { existing_contents = file_contents(filename_.c_str(), add_header); - } catch (CantOpenFile) { + } catch (const CantOpenFile& e) { file_exists = false; } From 845574860fb7dbbf631de84067036e79ad15ec21 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 8 Oct 2018 22:55:46 -0400 Subject: [PATCH 36/51] Try cosmic rather than latest (which throws internal compiler error) --- docker/ubuntu-boost-tbb-eigen3/Dockerfile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docker/ubuntu-boost-tbb-eigen3/Dockerfile b/docker/ubuntu-boost-tbb-eigen3/Dockerfile index b400c0538..bb2bf4719 100644 --- a/docker/ubuntu-boost-tbb-eigen3/Dockerfile +++ b/docker/ubuntu-boost-tbb-eigen3/Dockerfile @@ -1,5 +1,5 @@ # Get the base Ubuntu image from Docker Hub -FROM ubuntu:latest +FROM ubuntu:cosmic # Update apps on the base image RUN apt-get -y update && apt-get install -y From a932d4bd5e06f9b906a70a592a079669042198c0 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 8 Oct 2018 23:13:15 -0400 Subject: [PATCH 37/51] Trying xenial --- bitbucket-pipelines.yml | 2 +- docker/ubuntu-boost-tbb-eigen3/Dockerfile | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/bitbucket-pipelines.yml b/bitbucket-pipelines.yml index 8a8d5dd14..58e27c79a 100644 --- a/bitbucket-pipelines.yml +++ b/bitbucket-pipelines.yml @@ -2,7 +2,7 @@ # Check https://confluence.atlassian.com/x/5Q4SMw for more examples. # ----- # Our custom docker image from Docker Hub as the build environment. -image: dellaert/ubuntu-boost-tbb-eigen3:latest +image: dellaert/ubuntu-boost-tbb-eigen3:xenial pipelines: default: diff --git a/docker/ubuntu-boost-tbb-eigen3/Dockerfile b/docker/ubuntu-boost-tbb-eigen3/Dockerfile index bb2bf4719..dda4fbbaf 100644 --- a/docker/ubuntu-boost-tbb-eigen3/Dockerfile +++ b/docker/ubuntu-boost-tbb-eigen3/Dockerfile @@ -1,5 +1,5 @@ # Get the base Ubuntu image from Docker Hub -FROM ubuntu:cosmic +FROM ubuntu:xenial # Update apps on the base image RUN apt-get -y update && apt-get install -y From 632dcd0bf2cf60e755bb40216f85706b6222f611 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 8 Oct 2018 23:24:19 -0400 Subject: [PATCH 38/51] Trying bionic, again, with -j2 flag --- bitbucket-pipelines.yml | 6 +++--- docker/ubuntu-boost-tbb-eigen3/Dockerfile | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/bitbucket-pipelines.yml b/bitbucket-pipelines.yml index 58e27c79a..d41ba7f26 100644 --- a/bitbucket-pipelines.yml +++ b/bitbucket-pipelines.yml @@ -2,7 +2,7 @@ # Check https://confluence.atlassian.com/x/5Q4SMw for more examples. # ----- # Our custom docker image from Docker Hub as the build environment. -image: dellaert/ubuntu-boost-tbb-eigen3:xenial +image: dellaert/ubuntu-boost-tbb-eigen3:bionic pipelines: default: @@ -11,5 +11,5 @@ pipelines: - mkdir build - cd build - cmake -DGTSAM_USE_SYSTEM_EIGEN=ON -DGTSAM_USE_EIGEN_MKL=OFF .. - - make -j4 - - make -j4 check \ No newline at end of file + - make -j2 + - make -j2 check \ No newline at end of file diff --git a/docker/ubuntu-boost-tbb-eigen3/Dockerfile b/docker/ubuntu-boost-tbb-eigen3/Dockerfile index dda4fbbaf..33aa1ab96 100644 --- a/docker/ubuntu-boost-tbb-eigen3/Dockerfile +++ b/docker/ubuntu-boost-tbb-eigen3/Dockerfile @@ -1,5 +1,5 @@ # Get the base Ubuntu image from Docker Hub -FROM ubuntu:xenial +FROM ubuntu:bionic # Update apps on the base image RUN apt-get -y update && apt-get install -y From 285e2da5a808c6b88d2d0f389d0ba3d0bab6d159 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 8 Oct 2018 23:55:49 -0400 Subject: [PATCH 39/51] Fixed lint errors --- gtsam/geometry/Unit3.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index 00edc1ad4..e0e0ecb56 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -257,7 +257,7 @@ Unit3 Unit3::retract(const Vector2& v) const { std::cos(theta) * p_ + xi_hat * (sin(theta) / theta); return Unit3(exp_p_xi_hat); } - + /* ************************************************************************* */ Vector2 Unit3::localCoordinates(const Unit3& other) const { const double x = p_.dot(other.p_); From e1466b2609daa9ca06bfef9e1732a6f8a9dde540 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 9 Oct 2018 08:45:42 -0400 Subject: [PATCH 40/51] Fixed uninitialized problem --- gtsam_unstable/slam/SmartRangeFactor.h | 18 ++++++++++++++---- 1 file changed, 14 insertions(+), 4 deletions(-) diff --git a/gtsam_unstable/slam/SmartRangeFactor.h b/gtsam_unstable/slam/SmartRangeFactor.h index 5e107ea58..2170c8599 100644 --- a/gtsam_unstable/slam/SmartRangeFactor.h +++ b/gtsam_unstable/slam/SmartRangeFactor.h @@ -14,7 +14,12 @@ #include #include #include + +#include #include +#include +#include +#include namespace gtsam { @@ -83,6 +88,7 @@ public: /** * Triangulate a point from at least three pose-range pairs * Checks for best pair that includes first point + * Raise runtime_error if not well defined. */ Point2 triangulate(const Values& x) const { //gttic_(triangulate); @@ -96,7 +102,7 @@ public: Circle2 circle1 = circles.front(); boost::optional best_fh; - boost::optional best_circle; + boost::optional bestCircle2; // loop over all circles for(const Circle2& it: circles) { @@ -111,13 +117,14 @@ public: // if h is large, the intersections are well defined. if (fh && (!best_fh || fh->y() > best_fh->y())) { best_fh = fh; - best_circle = it; + bestCircle2 = it; } } // use best fh to find actual intersection points + if (bestCircle2 && best_fh) { std::list intersections = circleCircleIntersection( - circle1.center, best_circle->center, best_fh); + circle1.center, bestCircle2->center, best_fh); // pick winner based on other measurements double error1 = 0, error2 = 0; @@ -127,7 +134,10 @@ public: error2 += distance2(it.center, p2); } return (error1 < error2) ? p1 : p2; - //gttoc_(triangulate); + } else { + throw std::runtime_error("triangulate failed"); + } + // gttoc_(triangulate); } /** From a34a9b8ff1512e62f738c6a276cd38204ec32f49 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 9 Oct 2018 08:46:30 -0400 Subject: [PATCH 41/51] Fixed remaining lint errors --- gtsam_unstable/slam/SmartRangeFactor.h | 31 ++++++++++++-------------- 1 file changed, 14 insertions(+), 17 deletions(-) diff --git a/gtsam_unstable/slam/SmartRangeFactor.h b/gtsam_unstable/slam/SmartRangeFactor.h index 2170c8599..3164b360e 100644 --- a/gtsam_unstable/slam/SmartRangeFactor.h +++ b/gtsam_unstable/slam/SmartRangeFactor.h @@ -28,8 +28,7 @@ namespace gtsam { * @addtogroup SLAM */ class SmartRangeFactor: public NoiseModelFactor { -protected: - + protected: struct Circle2 { Circle2(const Point2& p, double r) : center(p), radius(r) { @@ -40,11 +39,10 @@ protected: typedef SmartRangeFactor This; - std::vector measurements_; ///< Range measurements - double variance_; ///< variance on noise - -public: + std::vector measurements_; ///< Range measurements + double variance_; ///< variance on noise + public: /** Default constructor: don't use directly */ SmartRangeFactor() { } @@ -53,7 +51,7 @@ public: * Constructor * @param s standard deviation of range measurement noise */ - SmartRangeFactor(double s) : + explicit SmartRangeFactor(double s) : NoiseModelFactor(noiseModel::Isotropic::Sigma(1, s)), variance_(s * s) { } @@ -91,7 +89,7 @@ public: * Raise runtime_error if not well defined. */ Point2 triangulate(const Values& x) const { - //gttic_(triangulate); + // gttic_(triangulate); // create n circles corresponding to measured range around each pose std::list circles; size_t n = size(); @@ -105,11 +103,11 @@ public: boost::optional bestCircle2; // loop over all circles - for(const Circle2& it: circles) { + for (const Circle2& it : circles) { // distance between circle centers. double d = distance2(circle1.center, it.center); if (d < 1e-9) - continue; // skip circles that are in the same location + continue; // skip circles that are in the same location // Find f and h, the intersection points in normalized circles boost::optional fh = circleCircleIntersection( circle1.radius / d, it.radius / d); @@ -129,7 +127,7 @@ public: // pick winner based on other measurements double error1 = 0, error2 = 0; Point2 p1 = intersections.front(), p2 = intersections.back(); - for(const Circle2& it: circles) { + for (const Circle2& it : circles) { error1 += distance2(it.center, p1); error2 += distance2(it.center, p2); } @@ -147,19 +145,20 @@ public: boost::optional&> H = boost::none) const { size_t n = size(); if (n < 3) { - if (H) + if (H) { // set Jacobians to zero for n<3 for (size_t j = 0; j < n; j++) (*H)[j] = Matrix::Zero(3, 1); + } return Z_1x1; } else { Vector error = Z_1x1; // triangulate to get the optimized point - // TODO: Should we have a (better?) variant that does this in relative coordinates ? + // TODO(dellaert): Should we have a (better?) variant that does this in relative coordinates ? Point2 optimizedPoint = triangulate(x); - // TODO: triangulation should be followed by an optimization given poses + // TODO(dellaert): triangulation should be followed by an optimization given poses // now evaluate the errors between predicted and measured range for (size_t j = 0; j < n; j++) { const Pose2& pose = x.at(keys_[j]); @@ -178,8 +177,6 @@ public: return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } - }; - -} // \namespace gtsam +} // \namespace gtsam From 767c5d41ee8ef3914ce855997a984849702988ae Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 9 Oct 2018 09:11:50 -0400 Subject: [PATCH 42/51] Showed compiler that B_ is always initialized --- gtsam/geometry/Unit3.cpp | 100 +++++++++++++++++++-------------------- 1 file changed, 49 insertions(+), 51 deletions(-) diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index e0e0ecb56..2c49f72e7 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -74,61 +74,59 @@ const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const { tbb::mutex::scoped_lock lock(B_mutex_); #endif - // Return cached basis if available and the Jacobian isn't needed. if (B_ && !H) { + // Return cached basis if available and the Jacobian isn't needed. return *B_; - } - - // Return cached basis and derivatives if available. - if (B_ && H && H_B_) { + } else if (B_ && H && H_B_) { + // Return cached basis and derivatives if available. *H = *H_B_; + return *B_; + } else { + B_.reset(Matrix32()); + // Get the unit vector and derivative wrt this. + // NOTE(hayk): We can't call point3(), because it would recursively call basis(). + const Point3 n(p_); + + // Get the axis of rotation with the minimum projected length of the point + Point3 axis(0, 0, 1); + double mx = fabs(n.x()), my = fabs(n.y()), mz = fabs(n.z()); + if ((mx <= my) && (mx <= mz)) { + axis = Point3(1.0, 0.0, 0.0); + } else if ((my <= mx) && (my <= mz)) { + axis = Point3(0.0, 1.0, 0.0); + } + + // Choose the direction of the first basis vector b1 in the tangent plane by crossing n with + // the chosen axis. + Matrix33 H_B1_n; + Point3 B1 = gtsam::cross(n, axis, H ? &H_B1_n : nullptr); + + // Normalize result to get a unit vector: b1 = B1 / |B1|. + Matrix33 H_b1_B1; + Point3 b1 = normalize(B1, H ? &H_b1_B1 : nullptr); + + // Get the second basis vector b2, which is orthogonal to n and b1, by crossing them. + // No need to normalize this, p and b1 are orthogonal unit vectors. + Matrix33 H_b2_n, H_b2_b1; + Point3 b2 = gtsam::cross(n, b1, H ? &H_b2_n : nullptr, H ? &H_b2_b1 : nullptr); + + // Create the basis by stacking b1 and b2. + (*B_) << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z(); + + if (H) { + // Chain rule tomfoolery to compute the derivative. + const Matrix32& H_n_p = *B_; + const Matrix32 H_b1_p = H_b1_B1 * H_B1_n * H_n_p; + const Matrix32 H_b2_p = H_b2_n * H_n_p + H_b2_b1 * H_b1_p; + + // Cache the derivative and fill the result. + H_B_.reset(Matrix62()); + (*H_B_) << H_b1_p, H_b2_p; + *H = *H_B_; + } + return *B_; } - - // Get the unit vector and derivative wrt this. - // NOTE(hayk): We can't call point3(), because it would recursively call basis(). - const Point3 n(p_); - - // Get the axis of rotation with the minimum projected length of the point - Point3 axis(0, 0, 1); - double mx = fabs(n.x()), my = fabs(n.y()), mz = fabs(n.z()); - if ((mx <= my) && (mx <= mz)) { - axis = Point3(1.0, 0.0, 0.0); - } else if ((my <= mx) && (my <= mz)) { - axis = Point3(0.0, 1.0, 0.0); - } - - // Choose the direction of the first basis vector b1 in the tangent plane by crossing n with - // the chosen axis. - Matrix33 H_B1_n; - Point3 B1 = gtsam::cross(n, axis, H ? &H_B1_n : nullptr); - - // Normalize result to get a unit vector: b1 = B1 / |B1|. - Matrix33 H_b1_B1; - Point3 b1 = normalize(B1, H ? &H_b1_B1 : nullptr); - - // Get the second basis vector b2, which is orthogonal to n and b1, by crossing them. - // No need to normalize this, p and b1 are orthogonal unit vectors. - Matrix33 H_b2_n, H_b2_b1; - Point3 b2 = gtsam::cross(n, b1, H ? &H_b2_n : nullptr, H ? &H_b2_b1 : nullptr); - - // Create the basis by stacking b1 and b2. - B_.reset(Matrix32()); - (*B_) << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z(); - - if (H) { - // Chain rule tomfoolery to compute the derivative. - const Matrix32& H_n_p = *B_; - const Matrix32 H_b1_p = H_b1_B1 * H_B1_n * H_n_p; - const Matrix32 H_b2_p = H_b2_n * H_n_p + H_b2_b1 * H_b1_p; - - // Cache the derivative and fill the result. - H_B_.reset(Matrix62()); - (*H_B_) << H_b1_p, H_b2_p; - *H = *H_B_; - } - - return *B_; } /* ************************************************************************* */ @@ -257,7 +255,7 @@ Unit3 Unit3::retract(const Vector2& v) const { std::cos(theta) * p_ + xi_hat * (sin(theta) / theta); return Unit3(exp_p_xi_hat); } - + /* ************************************************************************* */ Vector2 Unit3::localCoordinates(const Unit3& other) const { const double x = p_.dot(other.p_); From 1cf5a2a80b7ea367ca116a468afee93a9dfe38af Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 8 Oct 2018 23:55:49 -0400 Subject: [PATCH 43/51] Fixed lint errors --- gtsam/geometry/Unit3.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index 00edc1ad4..e0e0ecb56 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -257,7 +257,7 @@ Unit3 Unit3::retract(const Vector2& v) const { std::cos(theta) * p_ + xi_hat * (sin(theta) / theta); return Unit3(exp_p_xi_hat); } - + /* ************************************************************************* */ Vector2 Unit3::localCoordinates(const Unit3& other) const { const double x = p_.dot(other.p_); From 560ee010c2c13a09036c7eb22189ccab7ee50030 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 9 Oct 2018 08:45:42 -0400 Subject: [PATCH 44/51] Fixed uninitialized problem --- gtsam_unstable/slam/SmartRangeFactor.h | 18 ++++++++++++++---- 1 file changed, 14 insertions(+), 4 deletions(-) diff --git a/gtsam_unstable/slam/SmartRangeFactor.h b/gtsam_unstable/slam/SmartRangeFactor.h index 5e107ea58..2170c8599 100644 --- a/gtsam_unstable/slam/SmartRangeFactor.h +++ b/gtsam_unstable/slam/SmartRangeFactor.h @@ -14,7 +14,12 @@ #include #include #include + +#include #include +#include +#include +#include namespace gtsam { @@ -83,6 +88,7 @@ public: /** * Triangulate a point from at least three pose-range pairs * Checks for best pair that includes first point + * Raise runtime_error if not well defined. */ Point2 triangulate(const Values& x) const { //gttic_(triangulate); @@ -96,7 +102,7 @@ public: Circle2 circle1 = circles.front(); boost::optional best_fh; - boost::optional best_circle; + boost::optional bestCircle2; // loop over all circles for(const Circle2& it: circles) { @@ -111,13 +117,14 @@ public: // if h is large, the intersections are well defined. if (fh && (!best_fh || fh->y() > best_fh->y())) { best_fh = fh; - best_circle = it; + bestCircle2 = it; } } // use best fh to find actual intersection points + if (bestCircle2 && best_fh) { std::list intersections = circleCircleIntersection( - circle1.center, best_circle->center, best_fh); + circle1.center, bestCircle2->center, best_fh); // pick winner based on other measurements double error1 = 0, error2 = 0; @@ -127,7 +134,10 @@ public: error2 += distance2(it.center, p2); } return (error1 < error2) ? p1 : p2; - //gttoc_(triangulate); + } else { + throw std::runtime_error("triangulate failed"); + } + // gttoc_(triangulate); } /** From 7da4824568071d6b42d56bd33aa39aad840b20f5 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 9 Oct 2018 08:46:30 -0400 Subject: [PATCH 45/51] Fixed remaining lint errors --- gtsam_unstable/slam/SmartRangeFactor.h | 31 ++++++++++++-------------- 1 file changed, 14 insertions(+), 17 deletions(-) diff --git a/gtsam_unstable/slam/SmartRangeFactor.h b/gtsam_unstable/slam/SmartRangeFactor.h index 2170c8599..3164b360e 100644 --- a/gtsam_unstable/slam/SmartRangeFactor.h +++ b/gtsam_unstable/slam/SmartRangeFactor.h @@ -28,8 +28,7 @@ namespace gtsam { * @addtogroup SLAM */ class SmartRangeFactor: public NoiseModelFactor { -protected: - + protected: struct Circle2 { Circle2(const Point2& p, double r) : center(p), radius(r) { @@ -40,11 +39,10 @@ protected: typedef SmartRangeFactor This; - std::vector measurements_; ///< Range measurements - double variance_; ///< variance on noise - -public: + std::vector measurements_; ///< Range measurements + double variance_; ///< variance on noise + public: /** Default constructor: don't use directly */ SmartRangeFactor() { } @@ -53,7 +51,7 @@ public: * Constructor * @param s standard deviation of range measurement noise */ - SmartRangeFactor(double s) : + explicit SmartRangeFactor(double s) : NoiseModelFactor(noiseModel::Isotropic::Sigma(1, s)), variance_(s * s) { } @@ -91,7 +89,7 @@ public: * Raise runtime_error if not well defined. */ Point2 triangulate(const Values& x) const { - //gttic_(triangulate); + // gttic_(triangulate); // create n circles corresponding to measured range around each pose std::list circles; size_t n = size(); @@ -105,11 +103,11 @@ public: boost::optional bestCircle2; // loop over all circles - for(const Circle2& it: circles) { + for (const Circle2& it : circles) { // distance between circle centers. double d = distance2(circle1.center, it.center); if (d < 1e-9) - continue; // skip circles that are in the same location + continue; // skip circles that are in the same location // Find f and h, the intersection points in normalized circles boost::optional fh = circleCircleIntersection( circle1.radius / d, it.radius / d); @@ -129,7 +127,7 @@ public: // pick winner based on other measurements double error1 = 0, error2 = 0; Point2 p1 = intersections.front(), p2 = intersections.back(); - for(const Circle2& it: circles) { + for (const Circle2& it : circles) { error1 += distance2(it.center, p1); error2 += distance2(it.center, p2); } @@ -147,19 +145,20 @@ public: boost::optional&> H = boost::none) const { size_t n = size(); if (n < 3) { - if (H) + if (H) { // set Jacobians to zero for n<3 for (size_t j = 0; j < n; j++) (*H)[j] = Matrix::Zero(3, 1); + } return Z_1x1; } else { Vector error = Z_1x1; // triangulate to get the optimized point - // TODO: Should we have a (better?) variant that does this in relative coordinates ? + // TODO(dellaert): Should we have a (better?) variant that does this in relative coordinates ? Point2 optimizedPoint = triangulate(x); - // TODO: triangulation should be followed by an optimization given poses + // TODO(dellaert): triangulation should be followed by an optimization given poses // now evaluate the errors between predicted and measured range for (size_t j = 0; j < n; j++) { const Pose2& pose = x.at(keys_[j]); @@ -178,8 +177,6 @@ public: return boost::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new This(*this))); } - }; - -} // \namespace gtsam +} // \namespace gtsam From b84f7ed22d221f5fd71fa382145bcd0cd7c692d8 Mon Sep 17 00:00:00 2001 From: Cyril Roussillon Date: Tue, 9 Oct 2018 17:30:41 +0200 Subject: [PATCH 46/51] (#378) Allow zero dimension factors, as it was in gtsam 3.x --- gtsam/linear/Scatter.cpp | 3 --- gtsam/nonlinear/internal/LevenbergMarquardtState.h | 6 +++--- 2 files changed, 3 insertions(+), 6 deletions(-) diff --git a/gtsam/linear/Scatter.cpp b/gtsam/linear/Scatter.cpp index db5c25442..5312da34b 100644 --- a/gtsam/linear/Scatter.cpp +++ b/gtsam/linear/Scatter.cpp @@ -70,9 +70,6 @@ Scatter::Scatter(const GaussianFactorGraph& gfg, iterator first = begin(); if (ordering) first += ordering->size(); if (first != end()) std::sort(first, end()); - - // Filter out keys with zero dimensions (if ordering had more keys) - erase(std::remove_if(begin(), end(), SlotEntry::Zero), end()); } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/internal/LevenbergMarquardtState.h b/gtsam/nonlinear/internal/LevenbergMarquardtState.h index 19a390c45..bd5465dda 100644 --- a/gtsam/nonlinear/internal/LevenbergMarquardtState.h +++ b/gtsam/nonlinear/internal/LevenbergMarquardtState.h @@ -113,9 +113,9 @@ struct LevenbergMarquardtState : public NonlinearOptimizerState { // Small cache of A|b|model indexed by dimension. Can save many mallocs. mutable std::vector noiseModelCache; CachedModel* getCachedModel(size_t dim) const { - if (dim > noiseModelCache.size()) - noiseModelCache.resize(dim); - CachedModel* item = &noiseModelCache[dim - 1]; + if (dim >= noiseModelCache.size()) + noiseModelCache.resize(dim+1); + CachedModel* item = &noiseModelCache[dim]; if (!item->model) *item = CachedModel(dim, 1.0 / std::sqrt(lambda)); return item; From 6ec0ca798299a001f6cb8c87066c4275d27d71d4 Mon Sep 17 00:00:00 2001 From: Andrei Costinescu Date: Fri, 12 Oct 2018 10:32:41 +0000 Subject: [PATCH 47/51] Fixed maybe-uninitialized warnings in Unit3::basis function --- gtsam/geometry/Unit3.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index 2c49f72e7..21f66604e 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -82,7 +82,6 @@ const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const { *H = *H_B_; return *B_; } else { - B_.reset(Matrix32()); // Get the unit vector and derivative wrt this. // NOTE(hayk): We can't call point3(), because it would recursively call basis(). const Point3 n(p_); @@ -111,7 +110,9 @@ const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const { Point3 b2 = gtsam::cross(n, b1, H ? &H_b2_n : nullptr, H ? &H_b2_b1 : nullptr); // Create the basis by stacking b1 and b2. - (*B_) << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z(); + Matrix32 stacked; + stacked << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z(); + B_.reset(stacked); if (H) { // Chain rule tomfoolery to compute the derivative. @@ -120,8 +121,9 @@ const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const { const Matrix32 H_b2_p = H_b2_n * H_n_p + H_b2_b1 * H_b1_p; // Cache the derivative and fill the result. - H_B_.reset(Matrix62()); - (*H_B_) << H_b1_p, H_b2_p; + Matrix62 derivative; + derivative << H_b1_p, H_b2_p; + H_B_.reset(derivative); *H = *H_B_; } From 6c09d8681c73cd3b21ab1bb8e587bd1633e777b9 Mon Sep 17 00:00:00 2001 From: AndreiCostinescu Date: Fri, 12 Oct 2018 19:10:18 -0400 Subject: [PATCH 48/51] Fixed warning in SmartRangeFactor.h --- gtsam_unstable/slam/SmartRangeFactor.h | 21 +++++++++++---------- 1 file changed, 11 insertions(+), 10 deletions(-) diff --git a/gtsam_unstable/slam/SmartRangeFactor.h b/gtsam_unstable/slam/SmartRangeFactor.h index 3164b360e..08f6bf6ce 100644 --- a/gtsam_unstable/slam/SmartRangeFactor.h +++ b/gtsam_unstable/slam/SmartRangeFactor.h @@ -121,17 +121,18 @@ class SmartRangeFactor: public NoiseModelFactor { // use best fh to find actual intersection points if (bestCircle2 && best_fh) { - std::list intersections = circleCircleIntersection( - circle1.center, bestCircle2->center, best_fh); + auto bestCircleCenter = bestCircle2->center; + std::list intersections = circleCircleIntersection( + circle1.center, bestCircleCenter, best_fh); - // pick winner based on other measurements - double error1 = 0, error2 = 0; - Point2 p1 = intersections.front(), p2 = intersections.back(); - for (const Circle2& it : circles) { - error1 += distance2(it.center, p1); - error2 += distance2(it.center, p2); - } - return (error1 < error2) ? p1 : p2; + // pick winner based on other measurements + double error1 = 0, error2 = 0; + Point2 p1 = intersections.front(), p2 = intersections.back(); + for (const Circle2& it : circles) { + error1 += distance2(it.center, p1); + error2 += distance2(it.center, p2); + } + return (error1 < error2) ? p1 : p2; } else { throw std::runtime_error("triangulate failed"); } From 540be68b803aac74be823933075d9d09571fd429 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 12 Oct 2018 23:30:47 -0400 Subject: [PATCH 49/51] Refactored code paths to cover all 8 cases of H, B_, H_B_ with minimal calculation. Previous version was a bit hard to parse. Assign directly to B (formerly stacked) and jacobian (formerly derivative). --- gtsam/geometry/Unit3.cpp | 89 +++++++++++++++++++++------------------- 1 file changed, 47 insertions(+), 42 deletions(-) diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index 21f66604e..df1152487 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -68,67 +68,72 @@ Unit3 Unit3::Random(boost::mt19937 & rng) { /* ************************************************************************* */ const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const { #ifdef GTSAM_USE_TBB - // NOTE(hayk): At some point it seemed like this reproducably resulted in deadlock. However, I - // can't see the reason why and I can no longer reproduce it. It may have been a red herring, or - // there is still a latent bug to watch out for. + // NOTE(hayk): At some point it seemed like this reproducably resulted in + // deadlock. However, I can't see reason why and I can no longer reproduce it. + // It may have been a red herring, or there is still a latent bug. tbb::mutex::scoped_lock lock(B_mutex_); #endif - - if (B_ && !H) { - // Return cached basis if available and the Jacobian isn't needed. - return *B_; - } else if (B_ && H && H_B_) { - // Return cached basis and derivatives if available. - *H = *H_B_; - return *B_; - } else { - // Get the unit vector and derivative wrt this. - // NOTE(hayk): We can't call point3(), because it would recursively call basis(). - const Point3 n(p_); + Point3 n, axis; + if (!B_ || (H && !H_B_)) { + // Get the unit vector + // NOTE(hayk): can't call point3(), because would recursively call basis(). + n = Point3(p_); // Get the axis of rotation with the minimum projected length of the point - Point3 axis(0, 0, 1); + axis = Point3(0, 0, 1); double mx = fabs(n.x()), my = fabs(n.y()), mz = fabs(n.z()); if ((mx <= my) && (mx <= mz)) { axis = Point3(1.0, 0.0, 0.0); } else if ((my <= mx) && (my <= mz)) { axis = Point3(0.0, 1.0, 0.0); } + } - // Choose the direction of the first basis vector b1 in the tangent plane by crossing n with - // the chosen axis. - Matrix33 H_B1_n; - Point3 B1 = gtsam::cross(n, axis, H ? &H_B1_n : nullptr); + if (H) { + if (!H_B_) { + // Compute Jacobian. Possibly recomputes B_ - // Normalize result to get a unit vector: b1 = B1 / |B1|. - Matrix33 H_b1_B1; - Point3 b1 = normalize(B1, H ? &H_b1_B1 : nullptr); + // Choose the direction of the first basis vector b1 in the tangent plane + // by crossing n with the chosen axis. + Matrix33 H_B1_n; + const Point3 B1 = gtsam::cross(n, axis, &H_B1_n); - // Get the second basis vector b2, which is orthogonal to n and b1, by crossing them. - // No need to normalize this, p and b1 are orthogonal unit vectors. - Matrix33 H_b2_n, H_b2_b1; - Point3 b2 = gtsam::cross(n, b1, H ? &H_b2_n : nullptr, H ? &H_b2_b1 : nullptr); + // Normalize result to get a unit vector: b1 = B1 / |B1|. + Matrix32 B; + Matrix33 H_b1_B1; + B.col(0) = normalize(B1, &H_b1_B1); - // Create the basis by stacking b1 and b2. - Matrix32 stacked; - stacked << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z(); - B_.reset(stacked); + // Get the second basis vector b2, which is orthogonal to n and b1. + Matrix33 H_b2_n, H_b2_b1; + B.col(1) = gtsam::cross(n, B.col(0), &H_b2_n, &H_b2_b1); - if (H) { - // Chain rule tomfoolery to compute the derivative. - const Matrix32& H_n_p = *B_; - const Matrix32 H_b1_p = H_b1_B1 * H_B1_n * H_n_p; - const Matrix32 H_b2_p = H_b2_n * H_n_p + H_b2_b1 * H_b1_p; + // Chain rule tomfoolery to compute the jacobian. + Matrix62 jacobian; + const Matrix32& H_n_p = B; + jacobian.block<3, 2>(0, 0) = H_b1_B1 * H_B1_n * H_n_p; + auto H_b1_p = jacobian.block<3, 2>(0, 0); + jacobian.block<3, 2>(3, 0) = H_b2_n * H_n_p + H_b2_b1 * H_b1_p; - // Cache the derivative and fill the result. - Matrix62 derivative; - derivative << H_b1_p, H_b2_p; - H_B_.reset(derivative); - *H = *H_B_; + // Cache the result and jacobian + B_.reset(B); + H_B_.reset(jacobian); } - return *B_; + // Return cached jacobian, possibly computed just above + *H = *H_B_; } + + if (!B_) { + // Same calculation as above, without derivatives. + // Done after H block, as that possibly computes B_ for the first time + Matrix32 B; + const Point3 B1 = gtsam::cross(n, axis); + B.col(0) = normalize(B1); + B.col(1) = gtsam::cross(n, B.col(0)); + B_.reset(B); + } + + return *B_; } /* ************************************************************************* */ From 88c4bd0a330043f4c4b3f50ced55fd793b6f3c48 Mon Sep 17 00:00:00 2001 From: Andrei Costinescu Date: Sat, 13 Oct 2018 06:56:04 -0400 Subject: [PATCH 50/51] Second attempt at a logical refactor of Unit3::basis method --- gtsam/geometry/Unit3.cpp | 83 ++++++++++++++++++---------------------- 1 file changed, 38 insertions(+), 45 deletions(-) mode change 100644 => 100755 gtsam/geometry/Unit3.cpp diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp old mode 100644 new mode 100755 index 61bcbc457..d5ddc115b --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -69,70 +69,63 @@ Unit3 Unit3::Random(boost::mt19937 & rng) { const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const { #ifdef GTSAM_USE_TBB // NOTE(hayk): At some point it seemed like this reproducably resulted in - // deadlock. However, I can't see reason why and I can no longer reproduce it. - // It may have been a red herring, or there is still a latent bug. + // deadlock. However, I don't know why and I can no longer reproduce it. + // It either was a red herring or there is still a latent bug left to debug. tbb::mutex::scoped_lock lock(B_mutex_); #endif - Point3 n, axis; - if (!B_ || (H && !H_B_)) { + + bool cachedBasis = static_cast(B_); + Matrix33 H_B1_n, H_b1_B1, H_b2_n, H_b2_b1; + + if (!cachedBasis) { // Get the unit vector - // NOTE(hayk): can't call point3(), because would recursively call basis(). - n = Point3(p_); + // NOTE(hayk): We can't call point3(), due to the recursive call of basis(). + const Point3 n(p_); // Get the axis of rotation with the minimum projected length of the point - axis = Point3(0, 0, 1); + Point3 axis(0, 0, 1); double mx = fabs(n.x()), my = fabs(n.y()), mz = fabs(n.z()); if ((mx <= my) && (mx <= mz)) { axis = Point3(1.0, 0.0, 0.0); } else if ((my <= mx) && (my <= mz)) { axis = Point3(0.0, 1.0, 0.0); } + + // Choose the direction of the first basis vector b1 in the tangent plane + // by crossing n with the chosen axis. + Point3 B1 = gtsam::cross(n, axis, H ? &H_B1_n : nullptr); + + // Normalize result to get a unit vector: b1 = B1 / |B1|. + Point3 b1 = normalize(B1, H ? &H_b1_B1 : nullptr); + + // Get the second basis vector b2, through the cross-product of n and b1. + // No need to normalize this, p and b1 are orthogonal unit vectors. + Point3 b2 = + gtsam::cross(n, b1, H ? &H_b2_n : nullptr, H ? &H_b2_b1 : nullptr); + + // Create the basis by stacking b1 and b2. + Matrix32 stacked; + stacked << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z(); + B_.reset(stacked); } if (H) { - if (!H_B_) { - // Compute Jacobian. Possibly recomputes B_ + if (!cachedBasis || !H_B_) { + // If Jacobian not cached or the basis was not cached, recompute it. + // Chain rule tomfoolery to compute the derivative. + const Matrix32& H_n_p = *B_; + const Matrix32 H_b1_p = H_b1_B1 * H_B1_n * H_n_p; + const Matrix32 H_b2_p = H_b2_n * H_n_p + H_b2_b1 * H_b1_p; - // Choose the direction of the first basis vector b1 in the tangent plane - // by crossing n with the chosen axis. - Matrix33 H_B1_n; - const Point3 B1 = gtsam::cross(n, axis, &H_B1_n); - - // Normalize result to get a unit vector: b1 = B1 / |B1|. - Matrix32 B; - Matrix33 H_b1_B1; - B.col(0) = normalize(B1, &H_b1_B1); - - // Get the second basis vector b2, which is orthogonal to n and b1. - Matrix33 H_b2_n, H_b2_b1; - B.col(1) = gtsam::cross(n, B.col(0), &H_b2_n, &H_b2_b1); - - // Chain rule tomfoolery to compute the jacobian. - Matrix62 jacobian; - const Matrix32& H_n_p = B; - jacobian.block<3, 2>(0, 0) = H_b1_B1 * H_B1_n * H_n_p; - auto H_b1_p = jacobian.block<3, 2>(0, 0); - jacobian.block<3, 2>(3, 0) = H_b2_n * H_n_p + H_b2_b1 * H_b1_p; - - // Cache the result and jacobian - B_.reset(B); - H_B_.reset(jacobian); + // Cache the derivative and fill the result. + Matrix62 derivative; + derivative << H_b1_p, H_b2_p; + H_B_.reset(derivative); } - // Return cached jacobian, possibly computed just above *H = *H_B_; } - if (!B_) { - // Same calculation as above, without derivatives. - // Done after H block, as that possibly computes B_ for the first time - Matrix32 B; - const Point3 B1 = gtsam::cross(n, axis); - B.col(0) = normalize(B1); - B.col(1) = gtsam::cross(n, B.col(0)); - B_.reset(B); - } - return *B_; } @@ -262,7 +255,7 @@ Unit3 Unit3::retract(const Vector2& v) const { std::cos(theta) * p_ + xi_hat * (sin(theta) / theta); return Unit3(exp_p_xi_hat); } - + /* ************************************************************************* */ Vector2 Unit3::localCoordinates(const Unit3& other) const { const double x = p_.dot(other.p_); From 35d4bb1a7618aa70c7aeccbc31998f85699bdaf3 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 13 Oct 2018 12:18:25 -0400 Subject: [PATCH 51/51] After discussion with Andrei, a final version of logic, and new tests to check more cases. Tested with both typedef and old Point3 configs. --- gtsam/geometry/Unit3.cpp | 97 ++++++++++++++++-------------- gtsam/geometry/tests/testUnit3.cpp | 24 +++++--- 2 files changed, 70 insertions(+), 51 deletions(-) diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index d5ddc115b..08f14c829 100755 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -65,6 +65,19 @@ Unit3 Unit3::Random(boost::mt19937 & rng) { return Unit3(d[0], d[1], d[2]); } +/* ************************************************************************* */ +// Get the axis of rotation with the minimum projected length of the point +static Point3 CalculateBestAxis(const Point3& n) { + double mx = fabs(n.x()), my = fabs(n.y()), mz = fabs(n.z()); + if ((mx <= my) && (mx <= mz)) { + return Point3(1.0, 0.0, 0.0); + } else if ((my <= mx) && (my <= mz)) { + return Point3(0.0, 1.0, 0.0); + } else { + return Point3(0, 0, 1); + } +} + /* ************************************************************************* */ const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const { #ifdef GTSAM_USE_TBB @@ -74,58 +87,54 @@ const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const { tbb::mutex::scoped_lock lock(B_mutex_); #endif - bool cachedBasis = static_cast(B_); - Matrix33 H_B1_n, H_b1_B1, H_b2_n, H_b2_b1; - - if (!cachedBasis) { - // Get the unit vector - // NOTE(hayk): We can't call point3(), due to the recursive call of basis(). - const Point3 n(p_); - - // Get the axis of rotation with the minimum projected length of the point - Point3 axis(0, 0, 1); - double mx = fabs(n.x()), my = fabs(n.y()), mz = fabs(n.z()); - if ((mx <= my) && (mx <= mz)) { - axis = Point3(1.0, 0.0, 0.0); - } else if ((my <= mx) && (my <= mz)) { - axis = Point3(0.0, 1.0, 0.0); - } - - // Choose the direction of the first basis vector b1 in the tangent plane - // by crossing n with the chosen axis. - Point3 B1 = gtsam::cross(n, axis, H ? &H_B1_n : nullptr); - - // Normalize result to get a unit vector: b1 = B1 / |B1|. - Point3 b1 = normalize(B1, H ? &H_b1_B1 : nullptr); - - // Get the second basis vector b2, through the cross-product of n and b1. - // No need to normalize this, p and b1 are orthogonal unit vectors. - Point3 b2 = - gtsam::cross(n, b1, H ? &H_b2_n : nullptr, H ? &H_b2_b1 : nullptr); - - // Create the basis by stacking b1 and b2. - Matrix32 stacked; - stacked << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z(); - B_.reset(stacked); - } + const bool cachedBasis = static_cast(B_); + const bool cachedJacobian = static_cast(H_B_); if (H) { - if (!cachedBasis || !H_B_) { - // If Jacobian not cached or the basis was not cached, recompute it. - // Chain rule tomfoolery to compute the derivative. - const Matrix32& H_n_p = *B_; - const Matrix32 H_b1_p = H_b1_B1 * H_B1_n * H_n_p; - const Matrix32 H_b2_p = H_b2_n * H_n_p + H_b2_b1 * H_b1_p; + if (!cachedJacobian) { + // Compute Jacobian. Recomputes B_ + Matrix32 B; + Matrix62 jacobian; + Matrix33 H_B1_n, H_b1_B1, H_b2_n, H_b2_b1; - // Cache the derivative and fill the result. - Matrix62 derivative; - derivative << H_b1_p, H_b2_p; - H_B_.reset(derivative); + // Choose the direction of the first basis vector b1 in the tangent plane + // by crossing n with the chosen axis. + const Point3 n(p_), axis = CalculateBestAxis(n); + const Point3 B1 = gtsam::cross(n, axis, &H_B1_n); + + // Normalize result to get a unit vector: b1 = B1 / |B1|. + B.col(0) = normalize(B1, &H_b1_B1); + + // Get the second basis vector b2, which is orthogonal to n and b1. + B.col(1) = gtsam::cross(n, B.col(0), &H_b2_n, &H_b2_b1); + + // Chain rule tomfoolery to compute the jacobian. + const Matrix32& H_n_p = B; + jacobian.block<3, 2>(0, 0) = H_b1_B1 * H_B1_n * H_n_p; + auto H_b1_p = jacobian.block<3, 2>(0, 0); + jacobian.block<3, 2>(3, 0) = H_b2_n * H_n_p + H_b2_b1 * H_b1_p; + + // Cache the result and jacobian + H_B_.reset(jacobian); + B_.reset(B); } + // Return cached jacobian, possibly computed just above *H = *H_B_; } + if (!cachedBasis) { + // Same calculation as above, without derivatives. + // Done after H block, as that possibly computes B_ for the first time + Matrix32 B; + + const Point3 n(p_), axis = CalculateBestAxis(n); + const Point3 B1 = gtsam::cross(n, axis); + B.col(0) = normalize(B1); + B.col(1) = gtsam::cross(n, B.col(0)); + B_.reset(B); + } + return *B_; } diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index 71093d668..542aca038 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -314,15 +314,24 @@ TEST(Unit3, basis) { Unit3 p(0.1, -0.2, 0.9); Matrix expected(3, 2); - expected << 0.0, -0.994169047, 0.97618706, - -0.0233922129, 0.216930458, 0.105264958; + expected << 0.0, -0.994169047, 0.97618706, -0.0233922129, 0.216930458, 0.105264958; Matrix62 actualH; - Matrix actual = p.basis(actualH); - EXPECT(assert_equal(expected, actual, 1e-6)); - Matrix62 expectedH = numericalDerivative11( - boost::bind(BasisTest, _1, boost::none), p); + boost::bind(BasisTest, _1, boost::none), p); + + // without H, first time + EXPECT(assert_equal(expected, p.basis(), 1e-6)); + + // without H, cached + EXPECT(assert_equal(expected, p.basis(), 1e-6)); + + // with H, first time + EXPECT(assert_equal(expected, p.basis(actualH), 1e-6)); + EXPECT(assert_equal(expectedH, actualH, 1e-8)); + + // with H, cached + EXPECT(assert_equal(expected, p.basis(actualH), 1e-6)); EXPECT(assert_equal(expectedH, actualH, 1e-8)); } @@ -432,7 +441,8 @@ TEST(Unit3, ErrorBetweenFactor) { // Add process factors using the dot product error function. SharedNoiseModel R_process = noiseModel::Isotropic::Sigma(2, 0.01); for (size_t i = 0; i < data.size() - 1; i++) { - Expression exp(Expression(U(i)), &Unit3::errorVector, Expression(U(i + 1))); + Expression exp(Expression(U(i)), &Unit3::errorVector, + Expression(U(i + 1))); graph.addExpressionFactor(R_process, Vector2::Zero(), exp); }