From 32044eaac879ad482d457b858983a415a63ebee6 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 16 Jun 2015 22:24:39 -0700 Subject: [PATCH 01/46] Added a named constructor to mimick Ceres defaults --- .../nonlinear/LevenbergMarquardtOptimizer.cpp | 12 +++--- gtsam/nonlinear/LevenbergMarquardtOptimizer.h | 42 +++++++++++++++---- timing/timeSFMBAL.cpp | 5 +-- 3 files changed, 42 insertions(+), 17 deletions(-) diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index 5fb51a243..398ccda75 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -105,8 +105,8 @@ void LevenbergMarquardtParams::print(const std::string& str) const { std::cout << " lambdaLowerBound: " << lambdaLowerBound << "\n"; std::cout << " minModelFidelity: " << minModelFidelity << "\n"; std::cout << " diagonalDamping: " << diagonalDamping << "\n"; - std::cout << " min_diagonal: " << min_diagonal_ << "\n"; - std::cout << " max_diagonal: " << max_diagonal_ << "\n"; + std::cout << " min_diagonal: " << min_diagonal << "\n"; + std::cout << " max_diagonal: " << max_diagonal << "\n"; std::cout << " verbosityLM: " << verbosityLMTranslator(verbosityLM) << "\n"; std::cout.flush(); @@ -119,7 +119,7 @@ GaussianFactorGraph::shared_ptr LevenbergMarquardtOptimizer::linearize() const { /* ************************************************************************* */ void LevenbergMarquardtOptimizer::increaseLambda() { - if (params_.useFixedLambdaFactor_) { + if (params_.useFixedLambdaFactor) { state_.lambda *= params_.lambdaFactor; } else { state_.lambda *= params_.lambdaFactor; @@ -131,7 +131,7 @@ void LevenbergMarquardtOptimizer::increaseLambda() { /* ************************************************************************* */ void LevenbergMarquardtOptimizer::decreaseLambda(double stepQuality) { - if (params_.useFixedLambdaFactor_) { + if (params_.useFixedLambdaFactor) { state_.lambda /= params_.lambdaFactor; } else { // CHECK_GT(step_quality, 0.0); @@ -156,8 +156,8 @@ GaussianFactorGraph::shared_ptr LevenbergMarquardtOptimizer::buildDampedSystem( state_.hessianDiagonal = linear.hessianDiagonal(); BOOST_FOREACH(Vector& v, state_.hessianDiagonal | map_values) { for (int aa = 0; aa < v.size(); aa++) { - v(aa) = std::min(std::max(v(aa), params_.min_diagonal_), - params_.max_diagonal_); + v(aa) = std::min(std::max(v(aa), params_.min_diagonal), + params_.max_diagonal); v(aa) = sqrt(v(aa)); } } diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h index e5561af48..632a7ac0c 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h @@ -55,9 +55,9 @@ public: std::string logFile; ///< an optional CSV log file, with [iteration, time, error, labda] bool diagonalDamping; ///< if true, use diagonal of Hessian bool reuse_diagonal_; ///< an additional option in Ceres for diagonalDamping (TODO: should be in state?) - bool useFixedLambdaFactor_; ///< if true applies constant increase (or decrease) to lambda according to lambdaFactor - double min_diagonal_; ///< when using diagonal damping saturates the minimum diagonal entries (default: 1e-6) - double max_diagonal_; ///< when using diagonal damping saturates the maximum diagonal entries (default: 1e32) + bool useFixedLambdaFactor; ///< if true applies constant increase (or decrease) to lambda according to lambdaFactor + double min_diagonal; ///< when using diagonal damping saturates the minimum diagonal entries (default: 1e-6) + double max_diagonal; ///< when using diagonal damping saturates the maximum diagonal entries (default: 1e32) LevenbergMarquardtParams() : lambdaInitial(1e-5), @@ -68,9 +68,37 @@ public: minModelFidelity(1e-3), diagonalDamping(false), reuse_diagonal_(false), - useFixedLambdaFactor_(true), - min_diagonal_(1e-6), - max_diagonal_(1e32) {} + useFixedLambdaFactor(true), + min_diagonal(1e-6), + max_diagonal(1e32) {} + + static LevenbergMarquardtParams CeresDefaults() { + LevenbergMarquardtParams p; + + // Termination condition, same as options.max_num_iterations + p.maxIterations = 50; + + // Termination condition, turn off because no corresponding option in CERES + p.absoluteErrorTol = 0; // Frank thinks this is not tolerance (was 1e-6) + + // Termination condition, turn off because no corresponding option in CERES + p.errorTol = 0; // 1e-6; + + // Termination condition, same as options.function_tolerance + p.relativeErrorTol = 1e-6; // This is function_tolerance (was 1e-03) + + // Change lambda parameters to be the same as Ceres + p.lambdaUpperBound = 1e32; + p.lambdaLowerBound = 1e-16; + p.lambdaInitial = 1e-04; + p.lambdaFactor = 2.0; + p.useFixedLambdaFactor = false; // Luca says this is important + + p.diagonalDamping = true; + p.minModelFidelity = 1e-3; // options.min_relative_decrease in CERES + + return p; + } virtual ~LevenbergMarquardtParams() {} virtual void print(const std::string& str = "") const; @@ -94,7 +122,7 @@ public: inline void setLogFile(const std::string& s) { logFile = s; } inline void setDiagonalDamping(bool flag) { diagonalDamping = flag; } inline void setUseFixedLambdaFactor(bool flag) { - useFixedLambdaFactor_ = flag; + useFixedLambdaFactor = flag; } }; diff --git a/timing/timeSFMBAL.cpp b/timing/timeSFMBAL.cpp index 154a72dc9..45a1cae81 100644 --- a/timing/timeSFMBAL.cpp +++ b/timing/timeSFMBAL.cpp @@ -72,13 +72,10 @@ int main(int argc, char* argv[]) { // Optimize // Set parameters to be similar to ceres - LevenbergMarquardtParams params; + LevenbergMarquardtParams params = LevenbergMarquardtParams::CeresDefaults(); params.setOrdering(ordering); params.setVerbosity("ERROR"); params.setVerbosityLM("TRYLAMBDA"); - params.setDiagonalDamping(true); - params.setlambdaInitial(1e-4); - params.setlambdaFactor(2.0); LevenbergMarquardtOptimizer lm(graph, initial, params); Values actual = lm.optimize(); From 178c1aec8880801d6ebc0b8669fec0d7414ffff3 Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 17 Jun 2015 09:05:02 -0700 Subject: [PATCH 02/46] Better comment --- gtsam/nonlinear/AdaptAutoDiff.h | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/gtsam/nonlinear/AdaptAutoDiff.h b/gtsam/nonlinear/AdaptAutoDiff.h index 7295f3160..adb2031df 100644 --- a/gtsam/nonlinear/AdaptAutoDiff.h +++ b/gtsam/nonlinear/AdaptAutoDiff.h @@ -65,7 +65,12 @@ struct Canonical { } }; -/// Adapt ceres-style autodiff +/** + * The AdaptAutoDiff class uses ceres-style autodiff to adapt a ceres-style + * Function evaluation, i.e., a function F that defines an operator + * template bool operator()(const T* const, const T* const, T* predicted) const; + * For now only binary operators are supported. + */ template class AdaptAutoDiff { From 6efc708c5ebfae6b3fc386540994380b23473033 Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 17 Jun 2015 09:06:21 -0700 Subject: [PATCH 03/46] For exact comparison with Ceres, use exact same AutoDiff model --- timing/timeSFMBAL.cpp | 67 +++++++++++++++++++++++++++++++++++++++---- 1 file changed, 61 insertions(+), 6 deletions(-) diff --git a/timing/timeSFMBAL.cpp b/timing/timeSFMBAL.cpp index 45a1cae81..77bf4a708 100644 --- a/timing/timeSFMBAL.cpp +++ b/timing/timeSFMBAL.cpp @@ -16,11 +16,14 @@ * @date June 6, 2015 */ +#include #include #include #include #include #include +#include +#include #include #include #include @@ -39,6 +42,32 @@ using namespace gtsam; //#define TERNARY +// Special version of Cal3Bundler so that default constructor = 0,0,0 +struct CeresCalibration: public Cal3Bundler { + CeresCalibration(double f = 0, double k1 = 0, double k2 = 0, double u0 = 0, + double v0 = 0) : + Cal3Bundler(f, k1, k2, u0, v0) { + } + CeresCalibration(const Cal3Bundler& cal) : + Cal3Bundler(cal) { + } + CeresCalibration retract(const Vector& d) const { + return CeresCalibration(fx() + d(0), k1() + d(1), k2() + d(2), u0(), v0()); + } + Vector3 localCoordinates(const CeresCalibration& T2) const { + return T2.vector() - vector(); + } +}; + +namespace gtsam { +template<> +struct traits : public internal::Manifold { +}; +} + +// With that, camera below behaves like Snavely's 9-dim vector +typedef PinholeCamera CeresCamera; + int main(int argc, char* argv[]) { typedef GeneralSFMFactor, Point3> sfmFactor; using symbol_shorthand::P; @@ -47,17 +76,41 @@ int main(int argc, char* argv[]) { string defaultFilename = findExampleDataFile("dubrovnik-3-7-pre"); SfM_data db; bool success = readBAL(argc > 1 ? argv[1] : defaultFilename, db); - if (!success) throw runtime_error("Could not access file!"); + if (!success) + throw runtime_error("Could not access file!"); + + typedef AdaptAutoDiff Adaptor; // Build graph SharedNoiseModel unit2 = noiseModel::Unit::Create(2); NonlinearFactorGraph graph; for (size_t j = 0; j < db.number_tracks(); j++) { - BOOST_FOREACH (const SfM_Measurement& m, db.tracks[j].measurements) - graph.push_back(sfmFactor(m.second, unit2, m.first, P(j))); + BOOST_FOREACH (const SfM_Measurement& m, db.tracks[j].measurements) { + size_t i = m.first; + Point2 measurement = m.second; +#ifdef USE_GTSAM_FACTOR + graph.push_back(sfmFactor(measurement, unit2, i, P(j))); +#else + Expression camera_(i); + Expression point_(P(j)); + graph.addExpressionFactor(unit2, measurement, + Expression(Adaptor(), camera_, point_)); +#endif + } } - Values initial = initialCamerasAndPointsEstimate(db); + Values initial; + size_t i = 0, j = 0; + BOOST_FOREACH(const SfM_Camera& camera, db.cameras) { +#ifdef USE_GTSAM_FACTOR + initial.insert((i++), camera); +#else + CeresCamera ceresCamera(camera.pose(), camera.calibration()); + initial.insert((i++), ceresCamera); +#endif + } + BOOST_FOREACH(const SfM_Track& track, db.tracks) + initial.insert(P(j++), track.p); // Create Schur-complement ordering #ifdef CCOLAMD @@ -66,8 +119,10 @@ int main(int argc, char* argv[]) { Ordering ordering = Ordering::colamdConstrainedFirst(graph, pointKeys, true); #else Ordering ordering; - for (size_t j = 0; j < db.number_tracks(); j++) ordering.push_back(P(j)); - for (size_t i = 0; i < db.number_cameras(); i++) ordering.push_back(i); + for (size_t j = 0; j < db.number_tracks(); j++) + ordering.push_back(P(j)); + for (size_t i = 0; i < db.number_cameras(); i++) + ordering.push_back(i); #endif // Optimize From d71e66ea48d5cb8cb78fb94ef63bb982b90aa0b2 Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 17 Jun 2015 09:20:46 -0700 Subject: [PATCH 04/46] Moved reuse_diagnal_ to reuseDiagonal in state --- .../nonlinear/LevenbergMarquardtOptimizer.cpp | 8 ++++---- gtsam/nonlinear/LevenbergMarquardtOptimizer.h | 20 +++++++++---------- 2 files changed, 14 insertions(+), 14 deletions(-) diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index 398ccda75..a691506e2 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -125,7 +125,7 @@ void LevenbergMarquardtOptimizer::increaseLambda() { state_.lambda *= params_.lambdaFactor; params_.lambdaFactor *= 2.0; } - params_.reuse_diagonal_ = true; + state_.reuseDiagonal = true; } /* ************************************************************************* */ @@ -139,7 +139,7 @@ void LevenbergMarquardtOptimizer::decreaseLambda(double stepQuality) { params_.lambdaFactor = 2.0; } state_.lambda = std::max(params_.lambdaLowerBound, state_.lambda); - params_.reuse_diagonal_ = false; + state_.reuseDiagonal = false; } @@ -152,7 +152,7 @@ GaussianFactorGraph::shared_ptr LevenbergMarquardtOptimizer::buildDampedSystem( cout << "building damped system with lambda " << state_.lambda << endl; // Only retrieve diagonal vector when reuse_diagonal = false - if (params_.diagonalDamping && params_.reuse_diagonal_ == false) { + if (params_.diagonalDamping && state_.reuseDiagonal == false) { state_.hessianDiagonal = linear.hessianDiagonal(); BOOST_FOREACH(Vector& v, state_.hessianDiagonal | map_values) { for (int aa = 0; aa < v.size(); aa++) { @@ -263,7 +263,7 @@ void LevenbergMarquardtOptimizer::iterate() { double linearizedCostChange = 0, newlinearizedError = 0; if (systemSolvedSuccessfully) { - params_.reuse_diagonal_ = true; + state_.reuseDiagonal = true; if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) cout << "linear delta norm = " << delta.norm() << endl; diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h index 632a7ac0c..d185bca0e 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h @@ -54,7 +54,6 @@ public: double minModelFidelity; ///< Lower bound for the modelFidelity to accept the result of an LM iteration std::string logFile; ///< an optional CSV log file, with [iteration, time, error, labda] bool diagonalDamping; ///< if true, use diagonal of Hessian - bool reuse_diagonal_; ///< an additional option in Ceres for diagonalDamping (TODO: should be in state?) bool useFixedLambdaFactor; ///< if true applies constant increase (or decrease) to lambda according to lambdaFactor double min_diagonal; ///< when using diagonal damping saturates the minimum diagonal entries (default: 1e-6) double max_diagonal; ///< when using diagonal damping saturates the maximum diagonal entries (default: 1e32) @@ -67,7 +66,6 @@ public: verbosityLM(SILENT), minModelFidelity(1e-3), diagonalDamping(false), - reuse_diagonal_(false), useFixedLambdaFactor(true), min_diagonal(1e-6), max_diagonal(1e32) {} @@ -79,23 +77,23 @@ public: p.maxIterations = 50; // Termination condition, turn off because no corresponding option in CERES - p.absoluteErrorTol = 0; // Frank thinks this is not tolerance (was 1e-6) + p.absoluteErrorTol = 0; // Frank thinks this is not tolerance (was 1e-6) // Termination condition, turn off because no corresponding option in CERES - p.errorTol = 0; // 1e-6; + p.errorTol = 0; // 1e-6; // Termination condition, same as options.function_tolerance - p.relativeErrorTol = 1e-6; // This is function_tolerance (was 1e-03) + p.relativeErrorTol = 1e-6; // This is function_tolerance (was 1e-03) // Change lambda parameters to be the same as Ceres p.lambdaUpperBound = 1e32; p.lambdaLowerBound = 1e-16; p.lambdaInitial = 1e-04; p.lambdaFactor = 2.0; - p.useFixedLambdaFactor = false; // Luca says this is important + p.useFixedLambdaFactor = false; // Luca says this is important p.diagonalDamping = true; - p.minModelFidelity = 1e-3; // options.min_relative_decrease in CERES + p.minModelFidelity = 1e-3; // options.min_relative_decrease in CERES return p; } @@ -133,11 +131,13 @@ class GTSAM_EXPORT LevenbergMarquardtState: public NonlinearOptimizerState { public: double lambda; - int totalNumberInnerIterations; // The total number of inner iterations in the optimization (for each iteration, LM may try multiple iterations with different lambdas) boost::posix_time::ptime startTime; - VectorValues hessianDiagonal; //only update hessianDiagonal when reuse_diagonal_ = false + int totalNumberInnerIterations; //< The total number of inner iterations in the optimization (for each iteration, LM may try multiple iterations with different lambdas) + VectorValues hessianDiagonal; //< we only update hessianDiagonal when reuseDiagonal = false + bool reuseDiagonal; ///< an additional option in Ceres for diagonalDamping - LevenbergMarquardtState() { + LevenbergMarquardtState() : + reuseDiagonal(false) { initTime(); } From 1269785c0589e5347e35ea0b1e8fb67727899b0e Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 17 Jun 2015 09:23:24 -0700 Subject: [PATCH 05/46] Fixed naming convention --- gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp | 8 ++++---- gtsam/nonlinear/LevenbergMarquardtOptimizer.h | 8 ++++---- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index a691506e2..928b27167 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -105,8 +105,8 @@ void LevenbergMarquardtParams::print(const std::string& str) const { std::cout << " lambdaLowerBound: " << lambdaLowerBound << "\n"; std::cout << " minModelFidelity: " << minModelFidelity << "\n"; std::cout << " diagonalDamping: " << diagonalDamping << "\n"; - std::cout << " min_diagonal: " << min_diagonal << "\n"; - std::cout << " max_diagonal: " << max_diagonal << "\n"; + std::cout << " minDiagonal: " << minDiagonal << "\n"; + std::cout << " maxDiagonal: " << maxDiagonal << "\n"; std::cout << " verbosityLM: " << verbosityLMTranslator(verbosityLM) << "\n"; std::cout.flush(); @@ -156,8 +156,8 @@ GaussianFactorGraph::shared_ptr LevenbergMarquardtOptimizer::buildDampedSystem( state_.hessianDiagonal = linear.hessianDiagonal(); BOOST_FOREACH(Vector& v, state_.hessianDiagonal | map_values) { for (int aa = 0; aa < v.size(); aa++) { - v(aa) = std::min(std::max(v(aa), params_.min_diagonal), - params_.max_diagonal); + v(aa) = std::min(std::max(v(aa), params_.minDiagonal), + params_.maxDiagonal); v(aa) = sqrt(v(aa)); } } diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h index d185bca0e..bc4a55e26 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h @@ -55,8 +55,8 @@ public: std::string logFile; ///< an optional CSV log file, with [iteration, time, error, labda] bool diagonalDamping; ///< if true, use diagonal of Hessian bool useFixedLambdaFactor; ///< if true applies constant increase (or decrease) to lambda according to lambdaFactor - double min_diagonal; ///< when using diagonal damping saturates the minimum diagonal entries (default: 1e-6) - double max_diagonal; ///< when using diagonal damping saturates the maximum diagonal entries (default: 1e32) + double minDiagonal; ///< when using diagonal damping saturates the minimum diagonal entries (default: 1e-6) + double maxDiagonal; ///< when using diagonal damping saturates the maximum diagonal entries (default: 1e32) LevenbergMarquardtParams() : lambdaInitial(1e-5), @@ -67,8 +67,8 @@ public: minModelFidelity(1e-3), diagonalDamping(false), useFixedLambdaFactor(true), - min_diagonal(1e-6), - max_diagonal(1e32) {} + minDiagonal(1e-6), + maxDiagonal(1e32) {} static LevenbergMarquardtParams CeresDefaults() { LevenbergMarquardtParams p; From ba932cafae108189ea4eccb188ffe7bd799250c8 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 17 Jun 2015 19:43:04 -0700 Subject: [PATCH 06/46] Spelling --- gtsam/nonlinear/AdaptAutoDiff.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/nonlinear/AdaptAutoDiff.h b/gtsam/nonlinear/AdaptAutoDiff.h index adb2031df..341bb7d10 100644 --- a/gtsam/nonlinear/AdaptAutoDiff.h +++ b/gtsam/nonlinear/AdaptAutoDiff.h @@ -33,7 +33,7 @@ namespace detail { template struct Origin { T operator()() { return traits::Identity();} }; -// but dimple manifolds don't have one, so we just use the default constructor +// but simple manifolds don't have one, so we just use the default constructor template struct Origin { T operator()() { return T();} }; @@ -109,7 +109,7 @@ public: // Get derivatives with AutoDiff double *parameters[] = { v1.data(), v2.data() }; - double rowMajor1[N * M1], rowMajor2[N * M2]; // om the stack + double rowMajor1[N * M1], rowMajor2[N * M2]; // on the stack double *jacobians[] = { rowMajor1, rowMajor2 }; success = AutoDiff::Differentiate(f, parameters, 2, result.data(), jacobians); From 62db9370ca926e6e1a5b0d64d41c532fca6997dc Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 17 Jun 2015 19:43:26 -0700 Subject: [PATCH 07/46] Better separation of cers/gtam paths --- timing/timeSFMBAL.cpp | 52 ++++++++++++++++++++++++++++--------------- 1 file changed, 34 insertions(+), 18 deletions(-) diff --git a/timing/timeSFMBAL.cpp b/timing/timeSFMBAL.cpp index 77bf4a708..743d98284 100644 --- a/timing/timeSFMBAL.cpp +++ b/timing/timeSFMBAL.cpp @@ -16,14 +16,8 @@ * @date June 6, 2015 */ -#include #include -#include -#include -#include #include -#include -#include #include #include #include @@ -40,8 +34,17 @@ using namespace std; using namespace gtsam; -//#define TERNARY - +#define USE_GTSAM_FACTOR +#ifdef USE_GTSAM_FACTOR +#include +#include +#include +typedef PinholeCamera Camera; +typedef GeneralSFMFactor SfmFactor; +#else +#include +#include +#include // Special version of Cal3Bundler so that default constructor = 0,0,0 struct CeresCalibration: public Cal3Bundler { CeresCalibration(double f = 0, double k1 = 0, double k2 = 0, double u0 = 0, @@ -66,10 +69,10 @@ struct traits : public internal::Manifold { } // With that, camera below behaves like Snavely's 9-dim vector -typedef PinholeCamera CeresCamera; +typedef PinholeCamera Camera; +#endif int main(int argc, char* argv[]) { - typedef GeneralSFMFactor, Point3> sfmFactor; using symbol_shorthand::P; // Load BAL file (default is tiny) @@ -79,7 +82,9 @@ int main(int argc, char* argv[]) { if (!success) throw runtime_error("Could not access file!"); - typedef AdaptAutoDiff Adaptor; +#ifndef USE_GTSAM_FACTOR + AdaptAutoDiff snavely; +#endif // Build graph SharedNoiseModel unit2 = noiseModel::Unit::Create(2); @@ -89,12 +94,12 @@ int main(int argc, char* argv[]) { size_t i = m.first; Point2 measurement = m.second; #ifdef USE_GTSAM_FACTOR - graph.push_back(sfmFactor(measurement, unit2, i, P(j))); + graph.push_back(SfmFactor(measurement, unit2, i, P(j))); #else - Expression camera_(i); + Expression camera_(i); Expression point_(P(j)); graph.addExpressionFactor(unit2, measurement, - Expression(Adaptor(), camera_, point_)); + Expression(snavely, camera_, point_)); #endif } } @@ -105,14 +110,25 @@ int main(int argc, char* argv[]) { #ifdef USE_GTSAM_FACTOR initial.insert((i++), camera); #else - CeresCamera ceresCamera(camera.pose(), camera.calibration()); + Camera ceresCamera(camera.pose(), camera.calibration()); initial.insert((i++), ceresCamera); #endif } BOOST_FOREACH(const SfM_Track& track, db.tracks) initial.insert(P(j++), track.p); -// Create Schur-complement ordering + // Check projection + Point2 expected = db.tracks.front().measurements.front().second; + Camera camera = initial.at(0); + Point3 point = initial.at(P(0)); +#ifdef USE_GTSAM_FACTOR + Point2 actual = camera.project(point); +#else + Point2 actual = snavely(camera, point); +#endif + assert_equal(expected,actual,10); + + // Create Schur-complement ordering #ifdef CCOLAMD vector pointKeys; for (size_t j = 0; j < db.number_tracks(); j++) pointKeys.push_back(P(j)); @@ -127,12 +143,12 @@ int main(int argc, char* argv[]) { // Optimize // Set parameters to be similar to ceres - LevenbergMarquardtParams params = LevenbergMarquardtParams::CeresDefaults(); + LevenbergMarquardtParams params;// = LevenbergMarquardtParams::CeresDefaults(); params.setOrdering(ordering); params.setVerbosity("ERROR"); params.setVerbosityLM("TRYLAMBDA"); LevenbergMarquardtOptimizer lm(graph, initial, params); - Values actual = lm.optimize(); + Values result = lm.optimize(); tictoc_finishedIteration_(); tictoc_print_(); From 62bfce7358bdcd9aa6b0763d2ab5eb7c8e0a3371 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 17 Jun 2015 19:54:21 -0700 Subject: [PATCH 08/46] Fixed state constructor to set reuseDiagonal properly --- gtsam/nonlinear/LevenbergMarquardtOptimizer.h | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h index d185bca0e..bd02592fc 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h @@ -136,10 +136,7 @@ public: VectorValues hessianDiagonal; //< we only update hessianDiagonal when reuseDiagonal = false bool reuseDiagonal; ///< an additional option in Ceres for diagonalDamping - LevenbergMarquardtState() : - reuseDiagonal(false) { - initTime(); - } + LevenbergMarquardtState() {} // used in LM constructor but immediately overwritten void initTime() { startTime = boost::posix_time::microsec_clock::universal_time(); @@ -153,7 +150,7 @@ protected: const Values& initialValues, const LevenbergMarquardtParams& params, unsigned int iterations = 0) : NonlinearOptimizerState(graph, initialValues, iterations), lambda( - params.lambdaInitial), totalNumberInnerIterations(0) { + params.lambdaInitial), totalNumberInnerIterations(0),reuseDiagonal(false) { initTime(); } From 879e66a63a503984daab26725afce17e0ccc4ce6 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 18 Jun 2015 09:51:22 -0700 Subject: [PATCH 09/46] TWo default param sets --- gtsam/nonlinear/LevenbergMarquardtOptimizer.h | 134 ++++++++++-------- tests/testNonlinearOptimizer.cpp | 25 ++-- 2 files changed, 85 insertions(+), 74 deletions(-) diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h index 9fe2471a8..1e395d550 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h @@ -52,76 +52,84 @@ public: double lambdaLowerBound; ///< The minimum lambda used in LM (default: 0) VerbosityLM verbosityLM; ///< The verbosity level for Levenberg-Marquardt (default: SILENT), see also NonlinearOptimizerParams::verbosity double minModelFidelity; ///< Lower bound for the modelFidelity to accept the result of an LM iteration - std::string logFile; ///< an optional CSV log file, with [iteration, time, error, labda] + std::string logFile; ///< an optional CSV log file, with [iteration, time, error, lambda] bool diagonalDamping; ///< if true, use diagonal of Hessian bool useFixedLambdaFactor; ///< if true applies constant increase (or decrease) to lambda according to lambdaFactor double minDiagonal; ///< when using diagonal damping saturates the minimum diagonal entries (default: 1e-6) double maxDiagonal; ///< when using diagonal damping saturates the maximum diagonal entries (default: 1e32) LevenbergMarquardtParams() - : lambdaInitial(1e-5), - lambdaFactor(10.0), - lambdaUpperBound(1e5), - lambdaLowerBound(0.0), - verbosityLM(SILENT), - minModelFidelity(1e-3), + : verbosityLM(SILENT), diagonalDamping(false), - useFixedLambdaFactor(true), minDiagonal(1e-6), - maxDiagonal(1e32) {} + maxDiagonal(1e32) { + SetLegacyDefaults(this); + } + + static void SetLegacyDefaults(LevenbergMarquardtParams* p) { + // Relevant NonlinearOptimizerParams: + p->maxIterations = 100; + p->relativeErrorTol = 1e-5; + p->absoluteErrorTol = 1e-5; + // LM-specific: + p->lambdaInitial = 1e-5; + p->lambdaFactor = 10.0; + p->lambdaUpperBound = 1e5; + p->lambdaLowerBound = 0.0; + p->minModelFidelity = 1e-3; + p->diagonalDamping = false; + p->useFixedLambdaFactor = true; + } + + // these do seem to work better for SFM + static void SetCeresDefaults(LevenbergMarquardtParams* p) { + // Relevant NonlinearOptimizerParams: + p->maxIterations = 50; + p->absoluteErrorTol = 0; // No corresponding option in CERES + p->relativeErrorTol = 1e-6; // This is function_tolerance + // LM-specific: + p->lambdaUpperBound = 1e32; + p->lambdaLowerBound = 1e-16; + p->lambdaInitial = 1e-04; + p->lambdaFactor = 2.0; + p->minModelFidelity = 1e-3; // options.min_relative_decrease in CERES + p->diagonalDamping = true; + p->useFixedLambdaFactor = false; // This is important + } + + static LevenbergMarquardtParams LegacyDefaults() { + LevenbergMarquardtParams p; + SetLegacyDefaults(&p); + return p; + } static LevenbergMarquardtParams CeresDefaults() { LevenbergMarquardtParams p; - - // Termination condition, same as options.max_num_iterations - p.maxIterations = 50; - - // Termination condition, turn off because no corresponding option in CERES - p.absoluteErrorTol = 0; // Frank thinks this is not tolerance (was 1e-6) - - // Termination condition, turn off because no corresponding option in CERES - p.errorTol = 0; // 1e-6; - - // Termination condition, same as options.function_tolerance - p.relativeErrorTol = 1e-6; // This is function_tolerance (was 1e-03) - - // Change lambda parameters to be the same as Ceres - p.lambdaUpperBound = 1e32; - p.lambdaLowerBound = 1e-16; - p.lambdaInitial = 1e-04; - p.lambdaFactor = 2.0; - p.useFixedLambdaFactor = false; // Luca says this is important - - p.diagonalDamping = true; - p.minModelFidelity = 1e-3; // options.min_relative_decrease in CERES - + SetCeresDefaults(&p); return p; } virtual ~LevenbergMarquardtParams() {} virtual void print(const std::string& str = "") const; - inline double getlambdaInitial() const { return lambdaInitial; } - inline double getlambdaFactor() const { return lambdaFactor; } - inline double getlambdaUpperBound() const { return lambdaUpperBound; } - inline double getlambdaLowerBound() const { return lambdaLowerBound; } - inline std::string getVerbosityLM() const { - return verbosityLMTranslator(verbosityLM); - } - inline std::string getLogFile() const { return logFile; } - inline bool getDiagonalDamping() const { return diagonalDamping; } + std::string getVerbosityLM() const { return verbosityLMTranslator(verbosityLM);} + void setVerbosityLM(const std::string& s) { verbosityLM = verbosityLMTranslator(s);} - inline void setlambdaInitial(double value) { lambdaInitial = value; } - inline void setlambdaFactor(double value) { lambdaFactor = value; } - inline void setlambdaUpperBound(double value) { lambdaUpperBound = value; } - inline void setlambdaLowerBound(double value) { lambdaLowerBound = value; } - inline void setVerbosityLM(const std::string& s) { - verbosityLM = verbosityLMTranslator(s); - } - inline void setLogFile(const std::string& s) { logFile = s; } - inline void setDiagonalDamping(bool flag) { diagonalDamping = flag; } - inline void setUseFixedLambdaFactor(bool flag) { - useFixedLambdaFactor = flag; - } + // @deprecated (just use fields) +#ifdef GTSAM_ALLOW_DEPRECATED + bool getDiagonalDamping() const { return diagonalDamping; } + double getlambdaFactor() const { return lambdaFactor; } + double getlambdaInitial() const { return lambdaInitial; } + double getlambdaLowerBound() const { return lambdaLowerBound; } + double getlambdaUpperBound() const { return lambdaUpperBound; } + std::string getLogFile() const { return logFile; } + void setDiagonalDamping(bool flag) { diagonalDamping = flag; } + void setlambdaFactor(double value) { lambdaFactor = value; } + void setlambdaInitial(double value) { lambdaInitial = value; } + void setlambdaLowerBound(double value) { lambdaLowerBound = value; } + void setlambdaUpperBound(double value) { lambdaUpperBound = value; } + void setLogFile(const std::string& s) { logFile = s; } + void setUseFixedLambdaFactor(bool flag) { useFixedLambdaFactor = flag;} +#endif }; /** @@ -180,12 +188,12 @@ public: * @param initialValues The initial variable assignments * @param params The optimization parameters */ - LevenbergMarquardtOptimizer(const NonlinearFactorGraph& graph, - const Values& initialValues, const LevenbergMarquardtParams& params = - LevenbergMarquardtParams()) : - NonlinearOptimizer(graph), params_(ensureHasOrdering(params, graph)), state_( - graph, initialValues, params_) { - } + LevenbergMarquardtOptimizer( + const NonlinearFactorGraph& graph, const Values& initialValues, + const LevenbergMarquardtParams& params = LevenbergMarquardtParams()) + : NonlinearOptimizer(graph), + params_(ensureHasOrdering(params, graph)), + state_(graph, initialValues, params_) {} /** Standard constructor, requires a nonlinear factor graph, initial * variable assignments, and optimization parameters. For convenience this @@ -194,9 +202,11 @@ public: * @param graph The nonlinear factor graph to optimize * @param initialValues The initial variable assignments */ - LevenbergMarquardtOptimizer(const NonlinearFactorGraph& graph, - const Values& initialValues, const Ordering& ordering) : - NonlinearOptimizer(graph) { + LevenbergMarquardtOptimizer( + const NonlinearFactorGraph& graph, const Values& initialValues, + const Ordering& ordering, + const LevenbergMarquardtParams& params = LevenbergMarquardtParams()) + : NonlinearOptimizer(graph), params_(params) { params_.ordering = ordering; state_ = LevenbergMarquardtState(graph, initialValues, params_); } diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index 6d8a056fc..f927f45ae 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -187,7 +187,9 @@ TEST( NonlinearOptimizer, Factorization ) ordering.push_back(X(1)); ordering.push_back(X(2)); - LevenbergMarquardtOptimizer optimizer(graph, config, ordering); + LevenbergMarquardtParams params; + LevenbergMarquardtParams::SetLegacyDefaults(¶ms); + LevenbergMarquardtOptimizer optimizer(graph, config, ordering, params); optimizer.iterate(); Values expected; @@ -260,13 +262,13 @@ TEST_UNSAFE(NonlinearOptimizer, MoreOptimization) { expectedGradient.insert(2,zero(3)); // Try LM and Dogleg - LevenbergMarquardtParams params; -// params.setVerbosityLM("TRYDELTA"); -// params.setVerbosity("TERMINATION"); - params.setlambdaUpperBound(1e9); -// params.setRelativeErrorTol(0); -// params.setAbsoluteErrorTol(0); - //params.setlambdaInitial(10); + LevenbergMarquardtParams params = LevenbergMarquardtParams::LegacyDefaults(); + // params.setVerbosityLM("TRYDELTA"); + // params.setVerbosity("TERMINATION"); + params.lambdaUpperBound = 1e9; +// params.relativeErrorTol = 0; +// params.absoluteErrorTol = 0; + //params.lambdaInitial = 10; { LevenbergMarquardtOptimizer optimizer(fg, init, params); @@ -290,7 +292,7 @@ TEST_UNSAFE(NonlinearOptimizer, MoreOptimization) { initBetter.insert(2, Pose2(11,7,M_PI/2)); { - params.setDiagonalDamping(true); + params.diagonalDamping = true; LevenbergMarquardtOptimizer optimizer(fg, initBetter, params); // test the diagonal @@ -399,7 +401,7 @@ public: /// Constructor IterativeLM(const NonlinearFactorGraph& graph, const Values& initialValues, const ConjugateGradientParameters &p, - const LevenbergMarquardtParams& params = LevenbergMarquardtParams()) : + const LevenbergMarquardtParams& params = LevenbergMarquardtParams::LegacyDefaults()) : LevenbergMarquardtOptimizer(graph, initialValues, params), cgParams_(p) { } @@ -446,8 +448,7 @@ TEST( NonlinearOptimizer, logfile ) // Levenberg-Marquardt LevenbergMarquardtParams lmParams; static const string filename("testNonlinearOptimizer.log"); - lmParams.setLogFile(filename); - CHECK(lmParams.getLogFile()==filename); + lmParams.logFile = filename; LevenbergMarquardtOptimizer(fg, c0, lmParams).optimize(); // stringstream expected,actual; From c01618a7ffa867f6a7e40a41d20226ab0e07c07b Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 18 Jun 2015 09:51:30 -0700 Subject: [PATCH 10/46] Use Ceres defaults --- .cproject | 3314 +++++++++++++++++++++-------------------- timing/timeSFMBAL.cpp | 3 +- 2 files changed, 1675 insertions(+), 1642 deletions(-) diff --git a/.cproject b/.cproject index ac9b166ec..866092c95 100644 --- a/.cproject +++ b/.cproject @@ -5,13 +5,13 @@ - - + + @@ -60,13 +60,13 @@ - - + + @@ -116,13 +116,13 @@ - - + + @@ -484,341 +484,6 @@ - - make - -j5 - testCombinedImuFactor.run - true - true - true - - - make - -j5 - testImuFactor.run - true - true - true - - - make - -j5 - testAHRSFactor.run - true - true - true - - - make - -j8 - testAttitudeFactor.run - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - testNonlinearConstraint.run - true - true - true - - - make - -j2 - testLieConfig.run - true - true - true - - - make - -j2 - testConstraintOptimizer.run - true - true - true - - - make - -j2 - testGaussianFactor.run - true - true - true - - - make - -j4 - testImuFactor.run - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - testBTree.run - true - true - true - - - make - -j2 - testDSF.run - true - true - true - - - make - -j2 - testDSFVector.run - true - true - true - - - make - -j2 - testMatrix.run - true - true - true - - - make - -j2 - testSPQRUtil.run - true - true - true - - - make - -j2 - testVector.run - true - true - true - - - make - -j2 - timeMatrix.run - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j5 - wrap - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - install - true - true - true - - - make - -j2 - all - true - true - true - - - cmake - .. - true - false - true - - - make - -j2 - check - true - true - true - - - make - -j2 - tests/testLieConfig.run - true - true - true - - - make - -j4 - testSimilarity3.run - true - true - true - - - make - -j5 - testInvDepthCamera3.run - true - true - true - - - make - -j5 - testTriangulation.run - true - true - true - - - make - -j4 - testEvent.run - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j5 - all - true - false - true - - - make - -j5 - check - true - false - true - make -j5 @@ -867,183 +532,39 @@ true true - - make - -j5 - testCal3Bundler.run - true - true - true - - - make - -j5 - testCal3DS2.run - true - true - true - - - make - -j5 - testCalibratedCamera.run - true - true - true - - - make - -j5 - testEssentialMatrix.run - true - true - true - - - make - -j1 VERBOSE=1 - testHomography2.run - true - false - true - - - make - -j5 - testPinholeCamera.run - true - true - true - - - make - -j5 - testPoint2.run - true - true - true - - - make - -j5 - testPoint3.run - true - true - true - - - make - -j5 - testPose2.run - true - true - true - - - make - -j5 - testPose3.run - true - true - true - - - make - -j5 - testRot3M.run - true - true - true - - - make - -j5 - testSphere2.run - true - true - true - - - make - -j5 - testStereoCamera.run - true - true - true - - - make - -j5 - testCal3Unified.run - true - true - true - - - make - -j5 - testRot2.run - true - true - true - - - make - -j5 - testRot3Q.run - true - true - true - - - make - -j5 - testRot3.run - true - true - true - - + make -j4 - testSO3.run + testSimilarity3.run true true true - + + make + -j5 + testInvDepthCamera3.run + true + true + true + + + make + -j5 + testTriangulation.run + true + true + true + + make -j4 - testQuaternion.run + testEvent.run true true true - - make - -j4 - testOrientedPlane3.run - true - true - true - - - make - -j4 - testPinholePose.run - true - true - true - - - make - -j4 - testCyclic.run - true - true - true - - + make -j2 all @@ -1051,7 +572,7 @@ true true - + make -j2 clean @@ -1059,23 +580,143 @@ true true - + + make + -k + check + true + false + true + + + make + + tests/testBayesTree.run + true + false + true + + + make + + testBinaryBayesNet.run + true + false + true + + make -j2 - clean all + testFactorGraph.run true true true - + make -j2 - install + testISAM.run true true true - + + make + -j2 + testJunctionTree.run + true + true + true + + + make + -j2 + testKey.run + true + true + true + + + make + -j2 + testOrdering.run + true + true + true + + + make + + testSymbolicBayesNet.run + true + false + true + + + make + + tests/testSymbolicFactor.run + true + false + true + + + make + + testSymbolicFactorGraph.run + true + false + true + + + make + -j2 + timeSymbolMaps.run + true + true + true + + + make + + tests/testBayesTree + true + false + true + + + make + -j2 + tests/testPose2.run + true + true + true + + + make + -j2 + tests/testPose3.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + check + true + true + true + + make -j2 clean @@ -1227,6 +868,518 @@ true true + + make + -j5 + testGaussianFactorGraphUnordered.run + true + true + true + + + make + -j5 + testGaussianBayesNetUnordered.run + true + true + true + + + make + -j5 + testGaussianConditional.run + true + true + true + + + make + -j5 + testGaussianDensity.run + true + true + true + + + make + -j5 + testGaussianJunctionTree.run + true + true + true + + + make + -j5 + testHessianFactor.run + true + true + true + + + make + -j5 + testJacobianFactor.run + true + true + true + + + make + -j5 + testKalmanFilter.run + true + true + true + + + make + -j5 + testNoiseModel.run + true + true + true + + + make + -j5 + testSampler.run + true + true + true + + + make + -j5 + testSerializationLinear.run + true + true + true + + + make + -j5 + testVectorValues.run + true + true + true + + + make + -j5 + testGaussianBayesTree.run + true + true + true + + + make + -j5 + testCombinedImuFactor.run + true + true + true + + + make + -j5 + testImuFactor.run + true + true + true + + + make + -j5 + testAHRSFactor.run + true + true + true + + + make + -j8 + testAttitudeFactor.run + true + true + true + + + make + -j5 + clean + true + true + true + + + make + -j5 + all + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + testGaussianConditional.run + true + true + true + + + make + -j2 + testGaussianFactor.run + true + true + true + + + make + -j2 + timeGaussianFactor.run + true + true + true + + + make + -j2 + timeVectorConfig.run + true + true + true + + + make + -j2 + testVectorBTree.run + true + true + true + + + make + -j2 + testVectorMap.run + true + true + true + + + make + -j2 + testNoiseModel.run + true + true + true + + + make + -j2 + testBayesNetPreconditioner.run + true + true + true + + + make + + testErrors.run + true + false + true + + + make + -j2 + check + true + true + true + + + make + -j2 + tests/testGaussianJunctionTree.run + true + true + true + + + make + -j2 + tests/testGaussianFactor.run + true + true + true + + + make + -j2 + tests/testGaussianConditional.run + true + true + true + + + make + -j2 + tests/timeSLAMlike.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + testBTree.run + true + true + true + + + make + -j2 + testDSF.run + true + true + true + + + make + -j2 + testDSFVector.run + true + true + true + + + make + -j2 + testMatrix.run + true + true + true + + + make + -j2 + testSPQRUtil.run + true + true + true + + + make + -j2 + testVector.run + true + true + true + + + make + -j2 + timeMatrix.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + testClusterTree.run + true + true + true + + + make + -j2 + testJunctionTree.run + true + true + true + + + make + -j2 + tests/testEliminationTree.run + true + true + true + + + make + -j2 + tests/testSymbolicFactor.run + true + true + true + + + make + -j2 + tests/testVariableSlots.run + true + true + true + + + make + -j2 + tests/testConditional.run + true + true + true + + + make + -j2 + tests/testSymbolicFactorGraph.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + testNonlinearConstraint.run + true + true + true + + + make + -j2 + testLieConfig.run + true + true + true + + + make + -j2 + testConstraintOptimizer.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j5 + testBTree.run + true + true + true + + + make + -j5 + testDSF.run + true + true + true + + + make + -j5 + testDSFMap.run + true + true + true + + + make + -j5 + testDSFVector.run + true + true + true + + + make + -j5 + testFixedVector.run + true + true + true + make -j2 @@ -1368,23 +1521,55 @@ true true - + make - -j2 - all + -j5 + testEliminationTree.run true true true - + make - -j2 - clean + -j5 + testInference.run true true true - + + make + -j5 + testKey.run + true + true + true + + + make + -j1 + testSymbolicBayesTree.run + true + false + true + + + make + -j1 + testSymbolicSequentialSolver.run + true + false + true + + + make + -j4 + testLabeledSymbol.run + true + true + true + + make -j2 check @@ -1392,178 +1577,10 @@ true true - + make -j2 - testGaussianConditional.run - true - true - true - - - make - -j2 - testGaussianFactor.run - true - true - true - - - make - -j2 - timeGaussianFactor.run - true - true - true - - - make - -j2 - timeVectorConfig.run - true - true - true - - - make - -j2 - testVectorBTree.run - true - true - true - - - make - -j2 - testVectorMap.run - true - true - true - - - make - -j2 - testNoiseModel.run - true - true - true - - - make - -j2 - testBayesNetPreconditioner.run - true - true - true - - - make - - testErrors.run - true - false - true - - - make - -j5 - testAntiFactor.run - true - true - true - - - make - -j5 - testPriorFactor.run - true - true - true - - - make - -j5 - testDataset.run - true - true - true - - - make - -j5 - testEssentialMatrixFactor.run - true - true - true - - - make - -j5 - testGeneralSFMFactor_Cal3Bundler.run - true - true - true - - - make - -j5 - testGeneralSFMFactor.run - true - true - true - - - make - -j5 - testProjectionFactor.run - true - true - true - - - make - -j5 - testRotateFactor.run - true - true - true - - - make - -j5 - testPoseRotationPrior.run - true - true - true - - - make - -j5 - testImplicitSchurFactor.run - true - true - true - - - make - -j4 - testRangeFactor.run - true - true - true - - - make - -j4 - testOrientedPlane3Factor.run - true - true - true - - - make - -j4 - testSmartProjectionPoseFactor.run + tests/testLieConfig.run true true true @@ -1975,7 +1992,7 @@ false true - + make -j2 check @@ -1983,54 +2000,38 @@ true true - + make - tests/testGaussianISAM2 + -j2 + clean + true + true + true + + + make + -j2 + install + true + true + true + + + make + -j2 + all + true + true + true + + + cmake + .. true false true - - make - -j2 - check - true - true - true - - - make - -j2 - tests/testGaussianJunctionTree.run - true - true - true - - - make - -j2 - tests/testGaussianFactor.run - true - true - true - - - make - -j2 - tests/testGaussianConditional.run - true - true - true - - - make - -j2 - tests/timeSLAMlike.run - true - true - true - - + make -j2 testGaussianFactor.run @@ -2038,503 +2039,191 @@ true true - + make -j5 - testParticleFactor.run + testCal3Bundler.run true true true - + make -j5 - schedulingExample.run + testCal3DS2.run true true true - + make -j5 - schedulingQuals12.run - true - true - true - - - make - -j5 - schedulingQuals13.run - true - true - true - - - make - -j5 - testCSP.run - true - true - true - - - make - -j5 - testScheduler.run - true - true - true - - - make - -j5 - testSudoku.run - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -k - check - true - false - true - - - make - - tests/testBayesTree.run - true - false - true - - - make - - testBinaryBayesNet.run - true - false - true - - - make - -j2 - testFactorGraph.run - true - true - true - - - make - -j2 - testISAM.run - true - true - true - - - make - -j2 - testJunctionTree.run - true - true - true - - - make - -j2 - testKey.run - true - true - true - - - make - -j2 - testOrdering.run - true - true - true - - - make - - testSymbolicBayesNet.run - true - false - true - - - make - - tests/testSymbolicFactor.run - true - false - true - - - make - - testSymbolicFactorGraph.run - true - false - true - - - make - -j2 - timeSymbolMaps.run - true - true - true - - - make - - tests/testBayesTree - true - false - true - - - make - -j2 - check - true - true - true - - - make - -j2 - timeCalibratedCamera.run - true - true - true - - - make - -j2 - timeRot3.run - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - tests/testPose2.run - true - true - true - - - make - -j2 - tests/testPose3.run - true - true - true - - - make - -j2 - SimpleRotation.run - true - true - true - - - make - -j5 - CameraResectioning.run - true - true - true - - - make - -j5 - PlanarSLAMExample.run - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - easyPoint2KalmanFilter.run - true - true - true - - - make - -j2 - elaboratePoint2KalmanFilter.run - true - true - true - - - make - -j5 - Pose2SLAMExample.run - true - true - true - - - make - -j2 - Pose2SLAMwSPCG_easy.run - true - true - true - - - make - -j5 - UGM_small.run - true - true - true - - - make - -j5 - LocalizationExample.run - true - true - true - - - make - -j5 - OdometryExample.run - true - true - true - - - make - -j5 - RangeISAMExample_plaza2.run - true - true - true - - - make - -j5 - SelfCalibrationExample.run - true - true - true - - - make - -j5 - SFMExample.run - true - true - true - - - make - -j5 - VisualISAMExample.run - true - true - true - - - make - -j5 - VisualISAM2Example.run - true - true - true - - - make - -j5 - Pose2SLAMExample_graphviz.run - true - true - true - - - make - -j5 - Pose2SLAMExample_graph.run - true - true - true - - - make - -j5 - SFMExample_bal.run - true - true - true - - - make - -j5 - Pose2SLAMExample_lago.run - true - true - true - - - make - -j5 - Pose2SLAMExample_g2o.run - true - true - true - - - make - -j5 - SFMExample_SmartFactor.run - true - true - true - - - make - -j4 - Pose2SLAMExampleExpressions.run - true - true - true - - - make - -j4 - SFMExampleExpressions.run - true - true - true - - - make - -j4 - SFMExampleExpressions_bal.run - true - true - true - - - make - -j2 - testRot3.run - true - true - true - - - make - -j2 - testRot2.run - true - true - true - - - make - -j2 - testPose3.run - true - true - true - - - make - -j2 - timeRot3.run - true - true - true - - - make - -j2 - testPose2.run - true - true - true - - - make - -j2 - testCal3_S2.run - true - true - true - - - make - -j2 - testSimpleCamera.run - true - true - true - - - make - -j2 - testHomography2.run - true - true - true - - - make - -j2 testCalibratedCamera.run true true true - + + make + -j5 + testEssentialMatrix.run + true + true + true + + + make + -j1 VERBOSE=1 + testHomography2.run + true + false + true + + + make + -j5 + testPinholeCamera.run + true + true + true + + + make + -j5 + testPoint2.run + true + true + true + + + make + -j5 + testPoint3.run + true + true + true + + + make + -j5 + testPose2.run + true + true + true + + + make + -j5 + testPose3.run + true + true + true + + + make + -j5 + testRot3M.run + true + true + true + + + make + -j5 + testSphere2.run + true + true + true + + + make + -j5 + testStereoCamera.run + true + true + true + + + make + -j5 + testCal3Unified.run + true + true + true + + + make + -j5 + testRot2.run + true + true + true + + + make + -j5 + testRot3Q.run + true + true + true + + + make + -j5 + testRot3.run + true + true + true + + + make + -j4 + testSO3.run + true + true + true + + + make + -j4 + testQuaternion.run + true + true + true + + + make + -j4 + testOrientedPlane3.run + true + true + true + + + make + -j4 + testPinholePose.run + true + true + true + + + make + -j4 + testCyclic.run + true + true + true + + + make + -j2 + all + true + true + true + + make -j2 check @@ -2542,7 +2231,7 @@ true true - + make -j2 clean @@ -2550,10 +2239,58 @@ true true - + + make + -j5 + all + true + false + true + + + make + -j5 + check + true + false + true + + make -j2 - testPoint2.run + all + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + clean all true true true @@ -2598,198 +2335,6 @@ true true - - make - -j5 - testEliminationTree.run - true - true - true - - - make - -j5 - testInference.run - true - true - true - - - make - -j5 - testKey.run - true - true - true - - - make - -j1 - testSymbolicBayesTree.run - true - false - true - - - make - -j1 - testSymbolicSequentialSolver.run - true - false - true - - - make - -j4 - testLabeledSymbol.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - testClusterTree.run - true - true - true - - - make - -j2 - testJunctionTree.run - true - true - true - - - make - -j2 - tests/testEliminationTree.run - true - true - true - - - make - -j2 - tests/testSymbolicFactor.run - true - true - true - - - make - -j2 - tests/testVariableSlots.run - true - true - true - - - make - -j2 - tests/testConditional.run - true - true - true - - - make - -j2 - tests/testSymbolicFactorGraph.run - true - true - true - - - make - -j5 - testLago.run - true - true - true - - - make - -j5 - testLinearContainerFactor.run - true - true - true - - - make - -j5 - testOrdering.run - true - true - true - - - make - -j5 - testValues.run - true - true - true - - - make - -j5 - testWhiteNoiseFactor.run - true - true - true - - - make - -j4 - testExpression.run - true - true - true - - - make - -j4 - testAdaptAutoDiff.run - true - true - true - - - make - -j4 - testCallRecord.run - true - true - true - - - make - -j4 - testExpressionFactor.run - true - true - true - - - make - -j4 - testExecutionTrace.run - true - true - true - make -j5 @@ -2822,135 +2367,31 @@ true true - + make - -j5 + -j2 + check + true + true + true + + + make + -j2 timeCalibratedCamera.run true true true - - make - -j5 - timePinholeCamera.run - true - true - true - - - make - -j5 - timeStereoCamera.run - true - true - true - - - make - -j5 - timeLago.run - true - true - true - - - make - -j5 - timePose3.run - true - true - true - - - make - -j4 - timeAdaptAutoDiff.run - true - true - true - - - make - -j4 - timeCameraExpression.run - true - true - true - - - make - -j4 - timeOneCameraExpression.run - true - true - true - - - make - -j4 - timeSFMExpressions.run - true - true - true - - - make - -j4 - timeIncremental.run - true - true - true - - - make - -j5 - testBTree.run - true - true - true - - - make - -j5 - testDSF.run - true - true - true - - - make - -j5 - testDSFMap.run - true - true - true - - - make - -j5 - testDSFVector.run - true - true - true - - - make - -j5 - testFixedVector.run - true - true - true - - + make -j2 - all + timeRot3.run true true true - + make -j2 clean @@ -2958,110 +2399,6 @@ true true - - make - -j5 - testGaussianFactorGraphUnordered.run - true - true - true - - - make - -j5 - testGaussianBayesNetUnordered.run - true - true - true - - - make - -j5 - testGaussianConditional.run - true - true - true - - - make - -j5 - testGaussianDensity.run - true - true - true - - - make - -j5 - testGaussianJunctionTree.run - true - true - true - - - make - -j5 - testHessianFactor.run - true - true - true - - - make - -j5 - testJacobianFactor.run - true - true - true - - - make - -j5 - testKalmanFilter.run - true - true - true - - - make - -j5 - testNoiseModel.run - true - true - true - - - make - -j5 - testSampler.run - true - true - true - - - make - -j5 - testSerializationLinear.run - true - true - true - - - make - -j5 - testVectorValues.run - true - true - true - - - make - -j5 - testGaussianBayesTree.run - true - true - true - make -j5 @@ -3142,18 +2479,66 @@ true true - + make - -j2 - all + -j5 + schedulingExample.run true true true - + + make + -j5 + schedulingQuals12.run + true + true + true + + + make + -j5 + schedulingQuals13.run + true + true + true + + + make + -j5 + testCSP.run + true + true + true + + + make + -j5 + testScheduler.run + true + true + true + + + make + -j5 + testSudoku.run + true + true + true + + make -j2 - clean + vSFMexample.run + true + true + true + + + make + -j2 + testVSLAMGraph true true true @@ -3438,31 +2823,487 @@ true true - + make - -j2 - vSFMexample.run + -j4 + testNonlinearOPtimizer.run true true true - + make -j5 + testParticleFactor.run + true + true + true + + + make + -j2 + testGaussianFactor.run + true + true + true + + + make + -j2 + install + true + true + true + + + make + -j2 clean true true true - + make - -j5 + -j2 all true true true - + + make + -j2 + clean + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j5 + testAntiFactor.run + true + true + true + + + make + -j5 + testPriorFactor.run + true + true + true + + + make + -j5 + testDataset.run + true + true + true + + + make + -j5 + testEssentialMatrixFactor.run + true + true + true + + + make + -j5 + testGeneralSFMFactor_Cal3Bundler.run + true + true + true + + + make + -j5 + testGeneralSFMFactor.run + true + true + true + + + make + -j5 + testProjectionFactor.run + true + true + true + + + make + -j5 + testRotateFactor.run + true + true + true + + + make + -j5 + testPoseRotationPrior.run + true + true + true + + + make + -j5 + testImplicitSchurFactor.run + true + true + true + + + make + -j4 + testRangeFactor.run + true + true + true + + + make + -j4 + testOrientedPlane3Factor.run + true + true + true + + + make + -j4 + testSmartProjectionPoseFactor.run + true + true + true + + + make + -j4 + testRegularHessianFactor.run + true + true + true + + + make + -j4 + testRegularJacobianFactor.run + true + true + true + + + make + -j2 + SimpleRotation.run + true + true + true + + + make + -j5 + CameraResectioning.run + true + true + true + + + make + -j5 + PlanarSLAMExample.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + easyPoint2KalmanFilter.run + true + true + true + + + make + -j2 + elaboratePoint2KalmanFilter.run + true + true + true + + + make + -j5 + Pose2SLAMExample.run + true + true + true + + + make + -j2 + Pose2SLAMwSPCG_easy.run + true + true + true + + + make + -j5 + UGM_small.run + true + true + true + + + make + -j5 + LocalizationExample.run + true + true + true + + + make + -j5 + OdometryExample.run + true + true + true + + + make + -j5 + RangeISAMExample_plaza2.run + true + true + true + + + make + -j5 + SelfCalibrationExample.run + true + true + true + + + make + -j5 + SFMExample.run + true + true + true + + + make + -j5 + VisualISAMExample.run + true + true + true + + + make + -j5 + VisualISAM2Example.run + true + true + true + + + make + -j5 + Pose2SLAMExample_graphviz.run + true + true + true + + + make + -j5 + Pose2SLAMExample_graph.run + true + true + true + + + make + -j5 + SFMExample_bal.run + true + true + true + + + make + -j5 + Pose2SLAMExample_lago.run + true + true + true + + + make + -j5 + Pose2SLAMExample_g2o.run + true + true + true + + + make + -j5 + SFMExample_SmartFactor.run + true + true + true + + + make + -j4 + Pose2SLAMExampleExpressions.run + true + true + true + + + make + -j4 + SFMExampleExpressions.run + true + true + true + + + make + -j4 + SFMExampleExpressions_bal.run + true + true + true + + + make + -j5 + testLago.run + true + true + true + + + make + -j5 + testLinearContainerFactor.run + true + true + true + + + make + -j5 + testOrdering.run + true + true + true + + + make + -j5 + testValues.run + true + true + true + + + make + -j5 + testWhiteNoiseFactor.run + true + true + true + + + make + -j4 + testExpression.run + true + true + true + + + make + -j4 + testAdaptAutoDiff.run + true + true + true + + + make + -j4 + testCallRecord.run + true + true + true + + + make + -j4 + testExpressionFactor.run + true + true + true + + + make + -j4 + testExecutionTrace.run + true + true + true + + + make + -j4 + testImuFactor.run + true + true + true + + make -j2 check @@ -3470,10 +3311,201 @@ true true - + + make + tests/testGaussianISAM2 + true + false + true + + + make + -j5 + timeCalibratedCamera.run + true + true + true + + + make + -j5 + timePinholeCamera.run + true + true + true + + + make + -j5 + timeStereoCamera.run + true + true + true + + + make + -j5 + timeLago.run + true + true + true + + + make + -j5 + timePose3.run + true + true + true + + + make + -j4 + timeAdaptAutoDiff.run + true + true + true + + + make + -j4 + timeCameraExpression.run + true + true + true + + + make + -j4 + timeOneCameraExpression.run + true + true + true + + + make + -j4 + timeSFMExpressions.run + true + true + true + + + make + -j4 + timeIncremental.run + true + true + true + + + make + -j4 + timeSFMBAL.run + true + true + true + + make -j2 - testVSLAMGraph + testRot3.run + true + true + true + + + make + -j2 + testRot2.run + true + true + true + + + make + -j2 + testPose3.run + true + true + true + + + make + -j2 + timeRot3.run + true + true + true + + + make + -j2 + testPose2.run + true + true + true + + + make + -j2 + testCal3_S2.run + true + true + true + + + make + -j2 + testSimpleCamera.run + true + true + true + + + make + -j2 + testHomography2.run + true + true + true + + + make + -j2 + testCalibratedCamera.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + testPoint2.run + true + true + true + + + make + -j5 + wrap true true true diff --git a/timing/timeSFMBAL.cpp b/timing/timeSFMBAL.cpp index 743d98284..c1e4a9819 100644 --- a/timing/timeSFMBAL.cpp +++ b/timing/timeSFMBAL.cpp @@ -143,7 +143,8 @@ int main(int argc, char* argv[]) { // Optimize // Set parameters to be similar to ceres - LevenbergMarquardtParams params;// = LevenbergMarquardtParams::CeresDefaults(); + LevenbergMarquardtParams params; + SetCeresDefaults(¶ms); params.setOrdering(ordering); params.setVerbosity("ERROR"); params.setVerbosityLM("TRYLAMBDA"); From 9d40113dda74971e78a52c2b20a1908cd0cfcccb Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 18 Jun 2015 09:56:04 -0700 Subject: [PATCH 11/46] Snavely does not work --- timing/timeSFMBAL.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/timing/timeSFMBAL.cpp b/timing/timeSFMBAL.cpp index c1e4a9819..218bfe18e 100644 --- a/timing/timeSFMBAL.cpp +++ b/timing/timeSFMBAL.cpp @@ -34,7 +34,7 @@ using namespace std; using namespace gtsam; -#define USE_GTSAM_FACTOR +//#define USE_GTSAM_FACTOR #ifdef USE_GTSAM_FACTOR #include #include @@ -144,7 +144,7 @@ int main(int argc, char* argv[]) { // Optimize // Set parameters to be similar to ceres LevenbergMarquardtParams params; - SetCeresDefaults(¶ms); + LevenbergMarquardtParams::SetCeresDefaults(¶ms); params.setOrdering(ordering); params.setVerbosity("ERROR"); params.setVerbosityLM("TRYLAMBDA"); From ef73de4b512df127f0da922222f15d154010ac9e Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 20 Jun 2015 12:00:49 -0700 Subject: [PATCH 12/46] Grouped getters/setters, removed define (MATLAB wrapper needs these) --- gtsam/nonlinear/LevenbergMarquardtOptimizer.h | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h index 1e395d550..a965c6cf0 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h @@ -111,17 +111,16 @@ public: virtual ~LevenbergMarquardtParams() {} virtual void print(const std::string& str = "") const; - std::string getVerbosityLM() const { return verbosityLMTranslator(verbosityLM);} - void setVerbosityLM(const std::string& s) { verbosityLM = verbosityLMTranslator(s);} - // @deprecated (just use fields) -#ifdef GTSAM_ALLOW_DEPRECATED + /// @name Getters/Setters, mainly for MATLAB. Use fields above in C++. + /// @{ bool getDiagonalDamping() const { return diagonalDamping; } double getlambdaFactor() const { return lambdaFactor; } double getlambdaInitial() const { return lambdaInitial; } double getlambdaLowerBound() const { return lambdaLowerBound; } double getlambdaUpperBound() const { return lambdaUpperBound; } std::string getLogFile() const { return logFile; } + std::string getVerbosityLM() const { return verbosityLMTranslator(verbosityLM);} void setDiagonalDamping(bool flag) { diagonalDamping = flag; } void setlambdaFactor(double value) { lambdaFactor = value; } void setlambdaInitial(double value) { lambdaInitial = value; } @@ -129,7 +128,8 @@ public: void setlambdaUpperBound(double value) { lambdaUpperBound = value; } void setLogFile(const std::string& s) { logFile = s; } void setUseFixedLambdaFactor(bool flag) { useFixedLambdaFactor = flag;} -#endif + void setVerbosityLM(const std::string& s) { verbosityLM = verbosityLMTranslator(s);} + // @} }; /** From 263805a329e3f41a3e38e92e2620ee4b0c9664de Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 20 Jun 2015 22:01:37 -0700 Subject: [PATCH 13/46] Added constructor for ClusterNode --- gtsam/inference/ClusterTree.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/gtsam/inference/ClusterTree.h b/gtsam/inference/ClusterTree.h index 435631b21..bb7cf17e1 100644 --- a/gtsam/inference/ClusterTree.h +++ b/gtsam/inference/ClusterTree.h @@ -41,6 +41,11 @@ namespace gtsam typedef FastVector Factors; typedef FastVector > Children; + Cluster() {} + Cluster(Key key, const Factors& factors) : factors(factors) { + orderedFrontalKeys.push_back(key); + } + Keys orderedFrontalKeys; ///< Frontal keys of this node Factors factors; ///< Factors associated with this node Children children; ///< sub-trees From cab4eaa567462ab511079f028c5f4b23c3abfa79 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 20 Jun 2015 22:02:08 -0700 Subject: [PATCH 14/46] Re-factored constructor to eliminate overly verbose types --- gtsam/inference/JunctionTree-inst.h | 229 ++++++++++++++-------------- 1 file changed, 117 insertions(+), 112 deletions(-) diff --git a/gtsam/inference/JunctionTree-inst.h b/gtsam/inference/JunctionTree-inst.h index f12e5afd2..70930949e 100644 --- a/gtsam/inference/JunctionTree-inst.h +++ b/gtsam/inference/JunctionTree-inst.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) @@ -26,130 +26,135 @@ #include namespace gtsam { - - namespace { - /* ************************************************************************* */ - template - struct ConstructorTraversalData { - ConstructorTraversalData* const parentData; - typename JunctionTree::sharedNode myJTNode; - FastVector childSymbolicConditionals; - FastVector childSymbolicFactors; - ConstructorTraversalData(ConstructorTraversalData* _parentData) : parentData(_parentData) {} - }; - /* ************************************************************************* */ - // Pre-order visitor function - template - ConstructorTraversalData ConstructorTraversalVisitorPre( +template +struct ConstructorTraversalData { + typedef typename JunctionTree::Node Node; + typedef typename JunctionTree::sharedNode sharedNode; + + ConstructorTraversalData* const parentData; + sharedNode myJTNode; + FastVector childSymbolicConditionals; + FastVector childSymbolicFactors; + + ConstructorTraversalData(ConstructorTraversalData* _parentData) + : parentData(_parentData) {} + + // Pre-order visitor function + static ConstructorTraversalData ConstructorTraversalVisitorPre( const boost::shared_ptr& node, - ConstructorTraversalData& parentData) - { - // On the pre-order pass, before children have been visited, we just set up a traversal data - // structure with its own JT node, and create a child pointer in its parent. - ConstructorTraversalData myData = ConstructorTraversalData(&parentData); - myData.myJTNode = boost::make_shared::Node>(); - myData.myJTNode->orderedFrontalKeys.push_back(node->key); - myData.myJTNode->factors.insert(myData.myJTNode->factors.begin(), node->factors.begin(), node->factors.end()); - parentData.myJTNode->children.push_back(myData.myJTNode); - return myData; - } + ConstructorTraversalData& parentData) { + // On the pre-order pass, before children have been visited, we just set up + // a traversal data structure with its own JT node, and create a child + // pointer in its parent. + ConstructorTraversalData myData = ConstructorTraversalData(&parentData); + myData.myJTNode = boost::make_shared(node->key, node->factors); + parentData.myJTNode->children.push_back(myData.myJTNode); + return myData; + } - /* ************************************************************************* */ - // Post-order visitor function - template - void ConstructorTraversalVisitorPostAlg2( + // Post-order visitor function + static void ConstructorTraversalVisitorPostAlg2( const boost::shared_ptr& ETreeNode, - const ConstructorTraversalData& myData) - { - // In this post-order visitor, we combine the symbolic elimination results from the - // elimination tree children and symbolically eliminate the current elimination tree node. We - // then check whether each of our elimination tree child nodes should be merged with us. The - // check for this is that our number of symbolic elimination parents is exactly 1 less than - // our child's symbolic elimination parents - this condition indicates that eliminating the - // current node did not introduce any parents beyond those already in the child. + const ConstructorTraversalData& myData) { + // In this post-order visitor, we combine the symbolic elimination results + // from the elimination tree children and symbolically eliminate the current + // elimination tree node. We then check whether each of our elimination + // tree child nodes should be merged with us. The check for this is that + // our number of symbolic elimination parents is exactly 1 less than + // our child's symbolic elimination parents - this condition indicates that + // eliminating the current node did not introduce any parents beyond those + // already in the child. - // Do symbolic elimination for this node - class : public FactorGraph {} symbolicFactors; - symbolicFactors.reserve(ETreeNode->factors.size() + myData.childSymbolicFactors.size()); - // Add ETree node factors - symbolicFactors += ETreeNode->factors; - // Add symbolic factors passed up from children - symbolicFactors += myData.childSymbolicFactors; + // Do symbolic elimination for this node + class : public FactorGraph {} + symbolicFactors; + symbolicFactors.reserve(ETreeNode->factors.size() + + myData.childSymbolicFactors.size()); + // Add ETree node factors + symbolicFactors += ETreeNode->factors; + // Add symbolic factors passed up from children + symbolicFactors += myData.childSymbolicFactors; - Ordering keyAsOrdering; keyAsOrdering.push_back(ETreeNode->key); - std::pair symbolicElimResult = - internal::EliminateSymbolic(symbolicFactors, keyAsOrdering); + Ordering keyAsOrdering; + keyAsOrdering.push_back(ETreeNode->key); + std::pair + symbolicElimResult = + internal::EliminateSymbolic(symbolicFactors, keyAsOrdering); - // Store symbolic elimination results in the parent - myData.parentData->childSymbolicConditionals.push_back(symbolicElimResult.first); - myData.parentData->childSymbolicFactors.push_back(symbolicElimResult.second); + // Store symbolic elimination results in the parent + myData.parentData->childSymbolicConditionals.push_back( + symbolicElimResult.first); + myData.parentData->childSymbolicFactors.push_back( + symbolicElimResult.second); + sharedNode node = myData.myJTNode; - // Merge our children if they are in our clique - if our conditional has exactly one fewer - // parent than our child's conditional. - size_t myNrFrontals = 1; - const size_t myNrParents = symbolicElimResult.first->nrParents(); - size_t nrMergedChildren = 0; - assert(myData.myJTNode->children.size() == myData.childSymbolicConditionals.size()); - // Loop over children - int combinedProblemSize = (int) (symbolicElimResult.first->size() * symbolicFactors.size()); - for(size_t child = 0; child < myData.childSymbolicConditionals.size(); ++child) { - // Check if we should merge the child - if(myNrParents + myNrFrontals == myData.childSymbolicConditionals[child]->nrParents()) { - // Get a reference to the child, adjusting the index to account for children previously - // merged and removed from the child list. - const typename JunctionTree::Node& childToMerge = - *myData.myJTNode->children[child - nrMergedChildren]; - // Merge keys, factors, and children. - myData.myJTNode->orderedFrontalKeys.insert( - myData.myJTNode->orderedFrontalKeys.begin(), - childToMerge.orderedFrontalKeys.begin(), - childToMerge.orderedFrontalKeys.end()); - myData.myJTNode->factors.insert(myData.myJTNode->factors.end(), - childToMerge.factors.begin(), - childToMerge.factors.end()); - myData.myJTNode->children.insert(myData.myJTNode->children.end(), - childToMerge.children.begin(), - childToMerge.children.end()); - // Increment problem size - combinedProblemSize = std::max(combinedProblemSize, childToMerge.problemSize_); - // Increment number of frontal variables - myNrFrontals += childToMerge.orderedFrontalKeys.size(); - // Remove child from list. - myData.myJTNode->children.erase(myData.myJTNode->children.begin() + (child - nrMergedChildren)); - // Increment number of merged children - ++ nrMergedChildren; - } + // Merge our children if they are in our clique - if our conditional has + // exactly one fewer parent than our child's conditional. + size_t myNrFrontals = 1; + const size_t myNrParents = symbolicElimResult.first->nrParents(); + size_t nrMergedChildren = 0; + assert(node->children.size() == myData.childSymbolicConditionals.size()); + // Loop over children + int combinedProblemSize = + (int)(symbolicElimResult.first->size() * symbolicFactors.size()); + for (size_t i = 0; i < myData.childSymbolicConditionals.size(); ++i) { + // Check if we should merge the i^th child + if (myNrParents + myNrFrontals == + myData.childSymbolicConditionals[i]->nrParents()) { + // Get a reference to the i, adjusting the index to account for children + // previously merged and removed from the i list. + const Node& child = *node->children[i - nrMergedChildren]; + // Merge keys, factors, and children. + node->orderedFrontalKeys.insert(node->orderedFrontalKeys.begin(), + child.orderedFrontalKeys.begin(), + child.orderedFrontalKeys.end()); + node->factors.insert(node->factors.end(), child.factors.begin(), child.factors.end()); + node->children.insert(node->children.end(), child.children.begin(), child.children.end()); + // Increment problem size + combinedProblemSize = std::max(combinedProblemSize, child.problemSize_); + // Increment number of frontal variables + myNrFrontals += child.orderedFrontalKeys.size(); + // Remove i from list. + node->children.erase(node->children.begin() + (i - nrMergedChildren)); + // Increment number of merged children + ++nrMergedChildren; } - myData.myJTNode->problemSize_ = combinedProblemSize; } + node->problemSize_ = combinedProblemSize; } +}; - /* ************************************************************************* */ - template - template - JunctionTree::JunctionTree(const EliminationTree& eliminationTree) - { - gttic(JunctionTree_FromEliminationTree); - // Here we rely on the BayesNet having been produced by this elimination tree, such that the - // conditionals are arranged in DFS post-order. We traverse the elimination tree, and inspect - // the symbolic conditional corresponding to each node. The elimination tree node is added to - // the same clique with its parent if it has exactly one more Bayes net conditional parent than - // does its elimination tree parent. +/* ************************************************************************* */ +template +template +JunctionTree::JunctionTree( + const EliminationTree& eliminationTree) { + gttic(JunctionTree_FromEliminationTree); + // Here we rely on the BayesNet having been produced by this elimination tree, + // such that the conditionals are arranged in DFS post-order. We traverse the + // elimination tree, and inspect the symbolic conditional corresponding to + // each node. The elimination tree node is added to the same clique with its + // parent if it has exactly one more Bayes net conditional parent than + // does its elimination tree parent. - // Traverse the elimination tree, doing symbolic elimination and merging nodes as we go. Gather - // the created junction tree roots in a dummy Node. - typedef typename EliminationTree::Node ETreeNode; - ConstructorTraversalData rootData(0); - rootData.myJTNode = boost::make_shared(); // Make a dummy node to gather the junction tree roots - treeTraversal::DepthFirstForest(eliminationTree, rootData, - ConstructorTraversalVisitorPre, ConstructorTraversalVisitorPostAlg2); + // Traverse the elimination tree, doing symbolic elimination and merging nodes + // as we go. Gather the created junction tree roots in a dummy Node. + typedef typename EliminationTree::Node ETreeNode; + typedef ConstructorTraversalData Data; + Data rootData(0); + rootData.myJTNode = + boost::make_shared(); // Make a dummy node to gather + // the junction tree roots + treeTraversal::DepthFirstForest(eliminationTree, rootData, + Data::ConstructorTraversalVisitorPre, + Data::ConstructorTraversalVisitorPostAlg2); - // Assign roots from the dummy node - Base::roots_ = rootData.myJTNode->children; + // Assign roots from the dummy node + Base::roots_ = rootData.myJTNode->children; - // Transfer remaining factors from elimination tree - Base::remainingFactors_ = eliminationTree.remainingFactors(); - } + // Transfer remaining factors from elimination tree + Base::remainingFactors_ = eliminationTree.remainingFactors(); +} -} //namespace gtsam +} // namespace gtsam From 724bcdb6a007e5693f5026f9a909a883a3013424 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 20 Jun 2015 22:11:05 -0700 Subject: [PATCH 15/46] Reversed adding of keys --- gtsam/inference/JunctionTree-inst.h | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/gtsam/inference/JunctionTree-inst.h b/gtsam/inference/JunctionTree-inst.h index 70930949e..264985cb1 100644 --- a/gtsam/inference/JunctionTree-inst.h +++ b/gtsam/inference/JunctionTree-inst.h @@ -98,6 +98,7 @@ struct ConstructorTraversalData { // Loop over children int combinedProblemSize = (int)(symbolicElimResult.first->size() * symbolicFactors.size()); + gttic(merge_children); for (size_t i = 0; i < myData.childSymbolicConditionals.size(); ++i) { // Check if we should merge the i^th child if (myNrParents + myNrFrontals == @@ -105,6 +106,10 @@ struct ConstructorTraversalData { // Get a reference to the i, adjusting the index to account for children // previously merged and removed from the i list. const Node& child = *node->children[i - nrMergedChildren]; + // Merge keys. For efficiency, we add keys in reverse order at end, calling reverse after.. + node->orderedFrontalKeys.insert(node->orderedFrontalKeys.end(), + child.orderedFrontalKeys.rbegin(), + child.orderedFrontalKeys.rend()); // Merge keys, factors, and children. node->orderedFrontalKeys.insert(node->orderedFrontalKeys.begin(), child.orderedFrontalKeys.begin(), @@ -121,6 +126,8 @@ struct ConstructorTraversalData { ++nrMergedChildren; } } + std::reverse(node->orderedFrontalKeys.begin(), node->orderedFrontalKeys.end()); + gttoc(merge_children); node->problemSize_ = combinedProblemSize; } }; From 83487370ab452151cdf7da51d2f07645dcc8dc71 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 20 Jun 2015 22:53:04 -0700 Subject: [PATCH 16/46] Headers and timing --- gtsam/symbolic/SymbolicFactor-inst.h | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/gtsam/symbolic/SymbolicFactor-inst.h b/gtsam/symbolic/SymbolicFactor-inst.h index c917b322e..f1e2b48c2 100644 --- a/gtsam/symbolic/SymbolicFactor-inst.h +++ b/gtsam/symbolic/SymbolicFactor-inst.h @@ -17,15 +17,17 @@ #pragma once -#include +#include +#include +#include +#include +#include + #include #include #include -#include -#include -#include -#include +#include namespace gtsam { @@ -37,6 +39,8 @@ namespace gtsam std::pair, boost::shared_ptr > EliminateSymbolic(const FactorGraph& factors, const Ordering& keys) { + gttic(EliminateSymbolic); + // Gather all keys KeySet allKeys; BOOST_FOREACH(const boost::shared_ptr& factor, factors) { From b6541c96de6213caffbc66f3490a888182d99036 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 20 Jun 2015 22:54:23 -0700 Subject: [PATCH 17/46] Reverse keys, finalized --- gtsam/inference/JunctionTree-inst.h | 3 --- 1 file changed, 3 deletions(-) diff --git a/gtsam/inference/JunctionTree-inst.h b/gtsam/inference/JunctionTree-inst.h index 264985cb1..d7ff0281a 100644 --- a/gtsam/inference/JunctionTree-inst.h +++ b/gtsam/inference/JunctionTree-inst.h @@ -111,9 +111,6 @@ struct ConstructorTraversalData { child.orderedFrontalKeys.rbegin(), child.orderedFrontalKeys.rend()); // Merge keys, factors, and children. - node->orderedFrontalKeys.insert(node->orderedFrontalKeys.begin(), - child.orderedFrontalKeys.begin(), - child.orderedFrontalKeys.end()); node->factors.insert(node->factors.end(), child.factors.begin(), child.factors.end()); node->children.insert(node->children.end(), child.children.begin(), child.children.end()); // Increment problem size From 47279b56e0462b62b0879f2e92bd18acbf14d5f0 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 20 Jun 2015 23:45:22 -0700 Subject: [PATCH 18/46] Fixed print (now w symbols) --- gtsam/inference/ClusterTree-inst.h | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/gtsam/inference/ClusterTree-inst.h b/gtsam/inference/ClusterTree-inst.h index ad7ab0063..1987a0a5a 100644 --- a/gtsam/inference/ClusterTree-inst.h +++ b/gtsam/inference/ClusterTree-inst.h @@ -122,10 +122,8 @@ namespace gtsam void ClusterTree::Cluster::print( const std::string& s, const KeyFormatter& keyFormatter) const { - std::cout << s; - BOOST_FOREACH(Key j, orderedFrontalKeys) - std::cout << j << " "; - std::cout << "problemSize = " << problemSize_ << std::endl; + std::cout << s << " (" << problemSize_ << ")" ; + PrintKeyVector(orderedFrontalKeys); } /* ************************************************************************* */ From 6727ae9ae502b38775f34a685ef392cf13b46998 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 20 Jun 2015 23:45:32 -0700 Subject: [PATCH 19/46] salvaged old test --- tests/testGaussianJunctionTreeB.cpp | 387 +++++++++++++++------------- 1 file changed, 204 insertions(+), 183 deletions(-) diff --git a/tests/testGaussianJunctionTreeB.cpp b/tests/testGaussianJunctionTreeB.cpp index cdb1509b6..0b7c16184 100644 --- a/tests/testGaussianJunctionTreeB.cpp +++ b/tests/testGaussianJunctionTreeB.cpp @@ -15,29 +15,42 @@ * @author nikai */ -#include - -#if 0 - #include -#include -#include #include +#include +#include +#include #include #include -#include -#include +#include +#include +#include +#include +#include #include +#include +#include +#include +#include +#include #include -#include -#include -#include -#include +#include +#include +#include +#include +#include -#include -#include // for operator += -#include // for operator += +#include + +#include +#include #include + +#include +#include +#include +#include + using namespace boost::assign; #include @@ -59,180 +72,188 @@ using symbol_shorthand::L; TEST( GaussianJunctionTreeB, constructor2 ) { // create a graph + NonlinearFactorGraph nlfg; + Values values; + boost::tie(nlfg,values) = createNonlinearSmoother(7); + SymbolicFactorGraph::shared_ptr symbolic = nlfg.symbolic(); + + // linearize + GaussianFactorGraph::shared_ptr fg = nlfg.linearize(values); + Ordering ordering; ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4); - GaussianFactorGraph fg = createSmoother(7, ordering).first; // create an ordering - GaussianJunctionTree actual(fg); + GaussianEliminationTree etree(*fg, ordering); + SymbolicEliminationTree stree(*symbolic,ordering); + GaussianJunctionTree actual(etree); - vector frontal1; frontal1 += ordering[X(5)], ordering[X(6)], ordering[X(4)]; - vector frontal2; frontal2 += ordering[X(3)], ordering[X(2)]; - vector frontal3; frontal3 += ordering[X(1)]; - vector frontal4; frontal4 += ordering[X(7)]; - vector sep1; - vector sep2; sep2 += ordering[X(4)]; - vector sep3; sep3 += ordering[X(2)]; - vector sep4; sep4 += ordering[X(6)]; - EXPECT(assert_equal(frontal1, actual.root()->frontal)); - EXPECT(assert_equal(sep1, actual.root()->separator)); - LONGS_EQUAL(5, actual.root()->size()); - list::const_iterator child0it = actual.root()->children().begin(); - list::const_iterator child1it = child0it; ++child1it; - GaussianJunctionTree::sharedClique child0 = *child0it; - GaussianJunctionTree::sharedClique child1 = *child1it; - EXPECT(assert_equal(frontal2, child0->frontal)); - EXPECT(assert_equal(sep2, child0->separator)); - LONGS_EQUAL(4, child0->size()); - EXPECT(assert_equal(frontal3, child0->children().front()->frontal)); - EXPECT(assert_equal(sep3, child0->children().front()->separator)); - LONGS_EQUAL(2, child0->children().front()->size()); - EXPECT(assert_equal(frontal4, child1->frontal)); - EXPECT(assert_equal(sep4, child1->separator)); - LONGS_EQUAL(2, child1->size()); + Ordering frontal1; frontal1 += X(3), X(2), X(4); + Ordering frontal2; frontal2 += X(5), X(6); + Ordering frontal3; frontal3 += X(7); + Ordering frontal4; frontal4 += X(1); + + // Can no longer test these: +// Ordering sep1; +// Ordering sep2; sep2 += X(4); +// Ordering sep3; sep3 += X(6); +// Ordering sep4; sep4 += X(2); + + GaussianJunctionTree::sharedNode root = actual.roots().front(); + FastVector::const_iterator child0it = root->children.begin(); + FastVector::const_iterator child1it = child0it; ++child1it; + GaussianJunctionTree::sharedNode child0 = *child0it; + GaussianJunctionTree::sharedNode child1 = *child1it; + + EXPECT(assert_equal(frontal1, root->orderedFrontalKeys)); + LONGS_EQUAL(5, root->factors.size()); + EXPECT(assert_equal(frontal2, child0->orderedFrontalKeys)); + LONGS_EQUAL(4, child0->factors.size()); + EXPECT(assert_equal(frontal3, child0->children.front()->orderedFrontalKeys)); + LONGS_EQUAL(2, child0->children.front()->factors.size()); + EXPECT(assert_equal(frontal4, child1->orderedFrontalKeys)); + LONGS_EQUAL(2, child1->factors.size()); } -/* ************************************************************************* */ -TEST( GaussianJunctionTreeB, optimizeMultiFrontal ) -{ - // create a graph - GaussianFactorGraph fg; - Ordering ordering; - boost::tie(fg,ordering) = createSmoother(7); - - // optimize the graph - GaussianJunctionTree tree(fg); - VectorValues actual = tree.optimize(&EliminateQR); - - // verify - VectorValues expected(vector(7,2)); // expected solution - Vector v = Vector2(0., 0.); - for (int i=1; i<=7; i++) - expected[ordering[X(i)]] = v; - EXPECT(assert_equal(expected,actual)); -} - -/* ************************************************************************* */ -TEST( GaussianJunctionTreeB, optimizeMultiFrontal2) -{ - // create a graph - example::Graph nlfg = createNonlinearFactorGraph(); - Values noisy = createNoisyValues(); - Ordering ordering; ordering += X(1),X(2),L(1); - GaussianFactorGraph fg = *nlfg.linearize(noisy, ordering); - - // optimize the graph - GaussianJunctionTree tree(fg); - VectorValues actual = tree.optimize(&EliminateQR); - - // verify - VectorValues expected = createCorrectDelta(ordering); // expected solution - EXPECT(assert_equal(expected,actual)); -} - -/* ************************************************************************* */ -TEST(GaussianJunctionTreeB, slamlike) { - Values init; - NonlinearFactorGraph newfactors; - NonlinearFactorGraph fullgraph; - SharedDiagonal odoNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.1, 0.1, M_PI/100.0)); - SharedDiagonal brNoise = noiseModel::Diagonal::Sigmas((Vector(2) << M_PI/100.0, 0.1)); - - size_t i = 0; - - newfactors = NonlinearFactorGraph(); - newfactors.add(PriorFactor(X(0), Pose2(0.0, 0.0, 0.0), odoNoise)); - init.insert(X(0), Pose2(0.01, 0.01, 0.01)); - fullgraph.push_back(newfactors); - - for( ; i<5; ++i) { - newfactors = NonlinearFactorGraph(); - newfactors.add(BetweenFactor(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise)); - init.insert(X(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - fullgraph.push_back(newfactors); - } - - newfactors = NonlinearFactorGraph(); - newfactors.add(BetweenFactor(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise)); - newfactors.add(BearingRangeFactor(X(i), L(0), Rot2::fromAngle(M_PI/4.0), 5.0, brNoise)); - newfactors.add(BearingRangeFactor(X(i), L(1), Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise)); - init.insert(X(i+1), Pose2(1.01, 0.01, 0.01)); - init.insert(L(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); - init.insert(L(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); - fullgraph.push_back(newfactors); - ++ i; - - for( ; i<5; ++i) { - newfactors = NonlinearFactorGraph(); - newfactors.add(BetweenFactor(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise)); - init.insert(X(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); - fullgraph.push_back(newfactors); - } - - newfactors = NonlinearFactorGraph(); - newfactors.add(BetweenFactor(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise)); - newfactors.add(BearingRangeFactor(X(i), L(0), Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise)); - newfactors.add(BearingRangeFactor(X(i), L(1), Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise)); - init.insert(X(i+1), Pose2(6.9, 0.1, 0.01)); - fullgraph.push_back(newfactors); - ++ i; - - // Compare solutions - Ordering ordering = *fullgraph.orderingCOLAMD(init); - GaussianFactorGraph linearized = *fullgraph.linearize(init, ordering); - - GaussianJunctionTree gjt(linearized); - VectorValues deltaactual = gjt.optimize(&EliminateQR); - Values actual = init.retract(deltaactual, ordering); - - GaussianBayesNet gbn = *GaussianSequentialSolver(linearized).eliminate(); - VectorValues delta = optimize(gbn); - Values expected = init.retract(delta, ordering); - - EXPECT(assert_equal(expected, actual)); -} - -/* ************************************************************************* */ -TEST(GaussianJunctionTreeB, simpleMarginal) { - - typedef BayesTree GaussianBayesTree; - - // Create a simple graph - NonlinearFactorGraph fg; - fg.add(PriorFactor(X(0), Pose2(), noiseModel::Isotropic::Sigma(3, 10.0))); - fg.add(BetweenFactor(X(0), X(1), Pose2(1.0, 0.0, 0.0), noiseModel::Diagonal::Sigmas(Vector3(10.0, 1.0, 1.0)))); - - Values init; - init.insert(X(0), Pose2()); - init.insert(X(1), Pose2(1.0, 0.0, 0.0)); - - Ordering ordering; - ordering += X(1), X(0); - - GaussianFactorGraph gfg = *fg.linearize(init, ordering); - - // Compute marginals with both sequential and multifrontal - Matrix expected = GaussianSequentialSolver(gfg).marginalCovariance(1); - - Matrix actual1 = GaussianMultifrontalSolver(gfg).marginalCovariance(1); - - // Compute marginal directly from marginal factor - GaussianFactor::shared_ptr marginalFactor = GaussianMultifrontalSolver(gfg).marginalFactor(1); - JacobianFactor::shared_ptr marginalJacobian = boost::dynamic_pointer_cast(marginalFactor); - Matrix actual2 = inverse(marginalJacobian->getA(marginalJacobian->begin()).transpose() * marginalJacobian->getA(marginalJacobian->begin())); - - // Compute marginal directly from BayesTree - GaussianBayesTree gbt; - gbt.insert(GaussianJunctionTree(gfg).eliminate(EliminateCholesky)); - marginalFactor = gbt.marginalFactor(1, EliminateCholesky); - marginalJacobian = boost::dynamic_pointer_cast(marginalFactor); - Matrix actual3 = inverse(marginalJacobian->getA(marginalJacobian->begin()).transpose() * marginalJacobian->getA(marginalJacobian->begin())); - - EXPECT(assert_equal(expected, actual1)); - EXPECT(assert_equal(expected, actual2)); - EXPECT(assert_equal(expected, actual3)); -} - -#endif +///* ************************************************************************* */ +//TEST( GaussianJunctionTreeB, optimizeMultiFrontal ) +//{ +// // create a graph +// GaussianFactorGraph fg; +// Ordering ordering; +// boost::tie(fg,ordering) = createSmoother(7); +// +// // optimize the graph +// GaussianJunctionTree tree(fg); +// VectorValues actual = tree.optimize(&EliminateQR); +// +// // verify +// VectorValues expected(vector(7,2)); // expected solution +// Vector v = Vector2(0., 0.); +// for (int i=1; i<=7; i++) +// expected[ordering[X(i)]] = v; +// EXPECT(assert_equal(expected,actual)); +//} +// +///* ************************************************************************* */ +//TEST( GaussianJunctionTreeB, optimizeMultiFrontal2) +//{ +// // create a graph +// example::Graph nlfg = createNonlinearFactorGraph(); +// Values noisy = createNoisyValues(); +// Ordering ordering; ordering += X(1),X(2),L(1); +// GaussianFactorGraph fg = *nlfg.linearize(noisy, ordering); +// +// // optimize the graph +// GaussianJunctionTree tree(fg); +// VectorValues actual = tree.optimize(&EliminateQR); +// +// // verify +// VectorValues expected = createCorrectDelta(ordering); // expected solution +// EXPECT(assert_equal(expected,actual)); +//} +// +///* ************************************************************************* */ +//TEST(GaussianJunctionTreeB, slamlike) { +// Values init; +// NonlinearFactorGraph newfactors; +// NonlinearFactorGraph fullgraph; +// SharedDiagonal odoNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.1, 0.1, M_PI/100.0)); +// SharedDiagonal brNoise = noiseModel::Diagonal::Sigmas((Vector(2) << M_PI/100.0, 0.1)); +// +// size_t i = 0; +// +// newfactors = NonlinearFactorGraph(); +// newfactors.add(PriorFactor(X(0), Pose2(0.0, 0.0, 0.0), odoNoise)); +// init.insert(X(0), Pose2(0.01, 0.01, 0.01)); +// fullgraph.push_back(newfactors); +// +// for( ; i<5; ++i) { +// newfactors = NonlinearFactorGraph(); +// newfactors.add(BetweenFactor(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise)); +// init.insert(X(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); +// fullgraph.push_back(newfactors); +// } +// +// newfactors = NonlinearFactorGraph(); +// newfactors.add(BetweenFactor(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise)); +// newfactors.add(BearingRangeFactor(X(i), L(0), Rot2::fromAngle(M_PI/4.0), 5.0, brNoise)); +// newfactors.add(BearingRangeFactor(X(i), L(1), Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise)); +// init.insert(X(i+1), Pose2(1.01, 0.01, 0.01)); +// init.insert(L(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); +// init.insert(L(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); +// fullgraph.push_back(newfactors); +// ++ i; +// +// for( ; i<5; ++i) { +// newfactors = NonlinearFactorGraph(); +// newfactors.add(BetweenFactor(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise)); +// init.insert(X(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); +// fullgraph.push_back(newfactors); +// } +// +// newfactors = NonlinearFactorGraph(); +// newfactors.add(BetweenFactor(X(i), X(i+1), Pose2(1.0, 0.0, 0.0), odoNoise)); +// newfactors.add(BearingRangeFactor(X(i), L(0), Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise)); +// newfactors.add(BearingRangeFactor(X(i), L(1), Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise)); +// init.insert(X(i+1), Pose2(6.9, 0.1, 0.01)); +// fullgraph.push_back(newfactors); +// ++ i; +// +// // Compare solutions +// Ordering ordering = *fullgraph.orderingCOLAMD(init); +// GaussianFactorGraph linearized = *fullgraph.linearize(init, ordering); +// +// GaussianJunctionTree gjt(linearized); +// VectorValues deltaactual = gjt.optimize(&EliminateQR); +// Values actual = init.retract(deltaactual, ordering); +// +// GaussianBayesNet gbn = *GaussianSequentialSolver(linearized).eliminate(); +// VectorValues delta = optimize(gbn); +// Values expected = init.retract(delta, ordering); +// +// EXPECT(assert_equal(expected, actual)); +//} +// +///* ************************************************************************* */ +//TEST(GaussianJunctionTreeB, simpleMarginal) { +// +// typedef BayesTree GaussianBayesTree; +// +// // Create a simple graph +// NonlinearFactorGraph fg; +// fg.add(PriorFactor(X(0), Pose2(), noiseModel::Isotropic::Sigma(3, 10.0))); +// fg.add(BetweenFactor(X(0), X(1), Pose2(1.0, 0.0, 0.0), noiseModel::Diagonal::Sigmas(Vector3(10.0, 1.0, 1.0)))); +// +// Values init; +// init.insert(X(0), Pose2()); +// init.insert(X(1), Pose2(1.0, 0.0, 0.0)); +// +// Ordering ordering; +// ordering += X(1), X(0); +// +// GaussianFactorGraph gfg = *fg.linearize(init, ordering); +// +// // Compute marginals with both sequential and multifrontal +// Matrix expected = GaussianSequentialSolver(gfg).marginalCovariance(1); +// +// Matrix actual1 = GaussianMultifrontalSolver(gfg).marginalCovariance(1); +// +// // Compute marginal directly from marginal factor +// GaussianFactor::shared_ptr marginalFactor = GaussianMultifrontalSolver(gfg).marginalFactor(1); +// JacobianFactor::shared_ptr marginalJacobian = boost::dynamic_pointer_cast(marginalFactor); +// Matrix actual2 = inverse(marginalJacobian->getA(marginalJacobian->begin()).transpose() * marginalJacobian->getA(marginalJacobian->begin())); +// +// // Compute marginal directly from BayesTree +// GaussianBayesTree gbt; +// gbt.insert(GaussianJunctionTree(gfg).eliminate(EliminateCholesky)); +// marginalFactor = gbt.marginalFactor(1, EliminateCholesky); +// marginalJacobian = boost::dynamic_pointer_cast(marginalFactor); +// Matrix actual3 = inverse(marginalJacobian->getA(marginalJacobian->begin()).transpose() * marginalJacobian->getA(marginalJacobian->begin())); +// +// EXPECT(assert_equal(expected, actual1)); +// EXPECT(assert_equal(expected, actual2)); +// EXPECT(assert_equal(expected, actual3)); +//} /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} From f9ddbb1345572c54d9b4094eb0cb4384dde60d2c Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 21 Jun 2015 01:45:56 -0700 Subject: [PATCH 20/46] Eliminated linked list --- gtsam/inference/VariableIndex.h | 2 +- gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp | 2 +- gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp | 2 +- gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/inference/VariableIndex.h b/gtsam/inference/VariableIndex.h index f395fd4ab..3b80f0d01 100644 --- a/gtsam/inference/VariableIndex.h +++ b/gtsam/inference/VariableIndex.h @@ -43,7 +43,7 @@ class GTSAM_EXPORT VariableIndex { public: typedef boost::shared_ptr shared_ptr; - typedef FastList Factors; + typedef FastVector Factors; typedef Factors::iterator Factor_iterator; typedef Factors::const_iterator Factor_const_iterator; diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp index 9d4249af4..aa13b1462 100644 --- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp @@ -407,7 +407,7 @@ void BatchFixedLagSmoother::marginalize(const std::set& marginalizeKeys) { std::set removedFactorSlots; VariableIndex variableIndex(factors_); BOOST_FOREACH(Key key, marginalizeKeys) { - const FastList& slots = variableIndex[key]; + const FastVector& slots = variableIndex[key]; removedFactorSlots.insert(slots.begin(), slots.end()); } diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp index 1d913d61f..1a1622134 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp @@ -536,7 +536,7 @@ void ConcurrentBatchFilter::moveSeparator(const FastList& keysToMove) { std::vector removedFactorSlots; VariableIndex variableIndex(factors_); BOOST_FOREACH(Key key, keysToMove) { - const FastList& slots = variableIndex[key]; + const FastVector& slots = variableIndex[key]; removedFactorSlots.insert(removedFactorSlots.end(), slots.begin(), slots.end()); } diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp index cf3155602..4c4442141 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp @@ -291,7 +291,7 @@ std::vector ConcurrentIncrementalFilter::FindAdjacentFactors(const ISAM2 std::vector removedFactorSlots; const VariableIndex& variableIndex = isam2.getVariableIndex(); BOOST_FOREACH(Key key, keys) { - const FastList& slots = variableIndex[key]; + const FastVector& slots = variableIndex[key]; removedFactorSlots.insert(removedFactorSlots.end(), slots.begin(), slots.end()); } From e9d6feea5cf9e2ca47148d282b544e0c7c6eade4 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 21 Jun 2015 01:46:11 -0700 Subject: [PATCH 21/46] reserve vector --- gtsam/inference/EliminationTree-inst.h | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/gtsam/inference/EliminationTree-inst.h b/gtsam/inference/EliminationTree-inst.h index 70b0dd393..68e623b68 100644 --- a/gtsam/inference/EliminationTree-inst.h +++ b/gtsam/inference/EliminationTree-inst.h @@ -101,10 +101,12 @@ namespace gtsam { { // Retrieve the factors involving this variable and create the current node const VariableIndex::Factors& factors = structure[order[j]]; - nodes[j] = boost::make_shared(); - nodes[j]->key = order[j]; + const sharedNode node = boost::make_shared(); + node->key = order[j]; // for row i \in Struct[A*j] do + node->children.reserve(factors.size()); + node->factors.reserve(factors.size()); BOOST_FOREACH(const size_t i, factors) { // If we already hit a variable in this factor, make the subtree containing the previous // variable in this factor a child of the current node. This means that the variables @@ -123,16 +125,16 @@ namespace gtsam { if (r != j) { // Now that we found the root, hook up parent and child pointers in the nodes. parents[r] = j; - nodes[j]->children.push_back(nodes[r]); + node->children.push_back(nodes[r]); } } else { - // Add the current factor to the current node since we are at the first variable in this - // factor. - nodes[j]->factors.push_back(graph[i]); + // Add the factor to the current node since we are at the first variable in this factor. + node->factors.push_back(graph[i]); factorUsed[i] = true; } prevCol[i] = j; } + nodes[j] = node; } } catch(std::invalid_argument& e) { // If this is thrown from structure[order[j]] above, it means that it was requested to From c3811a54883eb1635d3500dd039dca0612f2468b Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 21 Jun 2015 11:40:47 -0700 Subject: [PATCH 22/46] Fixed machine-dependent outcome --- tests/testGaussianJunctionTreeB.cpp | 44 ++++++++++++++++++----------- 1 file changed, 27 insertions(+), 17 deletions(-) diff --git a/tests/testGaussianJunctionTreeB.cpp b/tests/testGaussianJunctionTreeB.cpp index 0b7c16184..c5401512b 100644 --- a/tests/testGaussianJunctionTreeB.cpp +++ b/tests/testGaussianJunctionTreeB.cpp @@ -85,12 +85,14 @@ TEST( GaussianJunctionTreeB, constructor2 ) // create an ordering GaussianEliminationTree etree(*fg, ordering); SymbolicEliminationTree stree(*symbolic,ordering); + GTSAM_PRINT(stree); GaussianJunctionTree actual(etree); + GTSAM_PRINT(actual); - Ordering frontal1; frontal1 += X(3), X(2), X(4); - Ordering frontal2; frontal2 += X(5), X(6); - Ordering frontal3; frontal3 += X(7); - Ordering frontal4; frontal4 += X(1); + Ordering o324; o324 += X(3), X(2), X(4); + Ordering o56; o56 += X(5), X(6); + Ordering o7; o7 += X(7); + Ordering o1; o1 += X(1); // Can no longer test these: // Ordering sep1; @@ -98,20 +100,28 @@ TEST( GaussianJunctionTreeB, constructor2 ) // Ordering sep3; sep3 += X(6); // Ordering sep4; sep4 += X(2); - GaussianJunctionTree::sharedNode root = actual.roots().front(); - FastVector::const_iterator child0it = root->children.begin(); - FastVector::const_iterator child1it = child0it; ++child1it; - GaussianJunctionTree::sharedNode child0 = *child0it; - GaussianJunctionTree::sharedNode child1 = *child1it; + GaussianJunctionTree::sharedNode x324 = actual.roots().front(); + LONGS_EQUAL(2, x324->children.size()); +#if defined(__APPLE__) // tie-breaking seems different :-( + GaussianJunctionTree::sharedNode x1 = x324->children[0]; + GaussianJunctionTree::sharedNode x56 = x324->children[1]; +#else + GaussianJunctionTree::sharedNode x1 = x324->children[1]; + GaussianJunctionTree::sharedNode x56 = x324->children[0]; +#endif + LONGS_EQUAL(0, x1->children.size()); + LONGS_EQUAL(1, x56->children.size()); + GaussianJunctionTree::sharedNode x7 = x56->children[0]; + LONGS_EQUAL(0, x7->children.size()); - EXPECT(assert_equal(frontal1, root->orderedFrontalKeys)); - LONGS_EQUAL(5, root->factors.size()); - EXPECT(assert_equal(frontal2, child0->orderedFrontalKeys)); - LONGS_EQUAL(4, child0->factors.size()); - EXPECT(assert_equal(frontal3, child0->children.front()->orderedFrontalKeys)); - LONGS_EQUAL(2, child0->children.front()->factors.size()); - EXPECT(assert_equal(frontal4, child1->orderedFrontalKeys)); - LONGS_EQUAL(2, child1->factors.size()); + EXPECT(assert_equal(o324, x324->orderedFrontalKeys)); + EXPECT_LONGS_EQUAL (5, x324->factors.size()); + EXPECT(assert_equal(o56, x56->orderedFrontalKeys)); + EXPECT_LONGS_EQUAL (4, x56->factors.size()); + EXPECT(assert_equal(o7, x7->orderedFrontalKeys)); + EXPECT_LONGS_EQUAL (2, x7->factors.size()); + EXPECT(assert_equal(o1, x1->orderedFrontalKeys)); + EXPECT_LONGS_EQUAL (2, x1->factors.size()); } ///* ************************************************************************* */ From 2dd83fd92c12d7a01bacaa8bc11e05ebf64b210a Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 21 Jun 2015 11:44:17 -0700 Subject: [PATCH 23/46] Count then merge --- gtsam/inference/JunctionTree-inst.h | 125 +++++++++++++++++----------- 1 file changed, 77 insertions(+), 48 deletions(-) diff --git a/gtsam/inference/JunctionTree-inst.h b/gtsam/inference/JunctionTree-inst.h index d7ff0281a..5735a3bd2 100644 --- a/gtsam/inference/JunctionTree-inst.h +++ b/gtsam/inference/JunctionTree-inst.h @@ -27,7 +27,7 @@ namespace gtsam { -template +template struct ConstructorTraversalData { typedef typename JunctionTree::Node Node; typedef typename JunctionTree::sharedNode sharedNode; @@ -37,8 +37,13 @@ struct ConstructorTraversalData { FastVector childSymbolicConditionals; FastVector childSymbolicFactors; - ConstructorTraversalData(ConstructorTraversalData* _parentData) - : parentData(_parentData) {} + // Small inner class to store symbolic factors + class SymbolicFactors: public FactorGraph { + }; + + ConstructorTraversalData(ConstructorTraversalData* _parentData) : + parentData(_parentData) { + } // Pre-order visitor function static ConstructorTraversalData ConstructorTraversalVisitorPre( @@ -64,13 +69,11 @@ struct ConstructorTraversalData { // our number of symbolic elimination parents is exactly 1 less than // our child's symbolic elimination parents - this condition indicates that // eliminating the current node did not introduce any parents beyond those - // already in the child. + // already in the child-> // Do symbolic elimination for this node - class : public FactorGraph {} - symbolicFactors; - symbolicFactors.reserve(ETreeNode->factors.size() + - myData.childSymbolicFactors.size()); + SymbolicFactors symbolicFactors; + symbolicFactors.reserve(ETreeNode->factors.size() + myData.childSymbolicFactors.size()); // Add ETree node factors symbolicFactors += ETreeNode->factors; // Add symbolic factors passed up from children @@ -78,60 +81,87 @@ struct ConstructorTraversalData { Ordering keyAsOrdering; keyAsOrdering.push_back(ETreeNode->key); - std::pair - symbolicElimResult = - internal::EliminateSymbolic(symbolicFactors, keyAsOrdering); + SymbolicConditional::shared_ptr myConditional; + SymbolicFactor::shared_ptr mySeparatorFactor; + boost::tie(myConditional, mySeparatorFactor) = internal::EliminateSymbolic( + symbolicFactors, keyAsOrdering); // Store symbolic elimination results in the parent - myData.parentData->childSymbolicConditionals.push_back( - symbolicElimResult.first); - myData.parentData->childSymbolicFactors.push_back( - symbolicElimResult.second); + myData.parentData->childSymbolicConditionals.push_back(myConditional); + myData.parentData->childSymbolicFactors.push_back(mySeparatorFactor); + sharedNode node = myData.myJTNode; + const FastVector& childConditionals = + myData.childSymbolicConditionals; // Merge our children if they are in our clique - if our conditional has // exactly one fewer parent than our child's conditional. size_t myNrFrontals = 1; - const size_t myNrParents = symbolicElimResult.first->nrParents(); - size_t nrMergedChildren = 0; - assert(node->children.size() == myData.childSymbolicConditionals.size()); - // Loop over children - int combinedProblemSize = - (int)(symbolicElimResult.first->size() * symbolicFactors.size()); + const size_t myNrParents = myConditional->nrParents(); + assert(node->newChildren.size() == childConditionals.size()); + gttic(merge_children); - for (size_t i = 0; i < myData.childSymbolicConditionals.size(); ++i) { + // First count how many keys, factors and children we'll end up with + size_t nrKeys = node->orderedFrontalKeys.size(); + size_t nrFactors = node->factors.size(); + size_t nrChildren = 0; + // Loop over children + for (size_t i = 0; i < childConditionals.size(); ++i) { // Check if we should merge the i^th child - if (myNrParents + myNrFrontals == - myData.childSymbolicConditionals[i]->nrParents()) { + if (myNrParents + myNrFrontals == childConditionals[i]->nrParents()) { // Get a reference to the i, adjusting the index to account for children // previously merged and removed from the i list. - const Node& child = *node->children[i - nrMergedChildren]; - // Merge keys. For efficiency, we add keys in reverse order at end, calling reverse after.. - node->orderedFrontalKeys.insert(node->orderedFrontalKeys.end(), - child.orderedFrontalKeys.rbegin(), - child.orderedFrontalKeys.rend()); - // Merge keys, factors, and children. - node->factors.insert(node->factors.end(), child.factors.begin(), child.factors.end()); - node->children.insert(node->children.end(), child.children.begin(), child.children.end()); - // Increment problem size - combinedProblemSize = std::max(combinedProblemSize, child.problemSize_); + sharedNode child = node->children[i]; + nrKeys += child->orderedFrontalKeys.size(); + nrFactors += child->factors.size(); + nrChildren += child->children.size(); // Increment number of frontal variables - myNrFrontals += child.orderedFrontalKeys.size(); - // Remove i from list. - node->children.erase(node->children.begin() + (i - nrMergedChildren)); - // Increment number of merged children - ++nrMergedChildren; + myNrFrontals += child->orderedFrontalKeys.size(); + } else { + nrChildren += 1; // we keep the child } } + + // now reserve space, and really merge + node->orderedFrontalKeys.reserve(nrKeys); + node->factors.reserve(nrFactors); + typename Node::Children newChildren; + newChildren.reserve(nrChildren); + myNrFrontals = 1; + int combinedProblemSize = (int) (myConditional->size() * symbolicFactors.size()); + // Loop over newChildren + for (size_t i = 0; i < childConditionals.size(); ++i) { + // Check if we should merge the i^th child + sharedNode child = node->children[i]; + if (myNrParents + myNrFrontals == childConditionals[i]->nrParents()) { + // Get a reference to the i, adjusting the index to account for newChildren + // previously merged and removed from the i list. + // Merge keys. For efficiency, we add keys in reverse order at end, calling reverse after.. + node->orderedFrontalKeys.insert(node->orderedFrontalKeys.end(), + child->orderedFrontalKeys.rbegin(), + child->orderedFrontalKeys.rend()); + // Merge keys, factors, and children. + node->factors.insert(node->factors.end(), child->factors.begin(), child->factors.end()); + newChildren.insert(newChildren.end(), child->children.begin(), child->children.end()); + // Increment problem size + combinedProblemSize = std::max(combinedProblemSize, child->problemSize_); + // Increment number of frontal variables + myNrFrontals += child->orderedFrontalKeys.size(); + } else { + newChildren.push_back(child); // we keep the child + } + } + node->children = newChildren; std::reverse(node->orderedFrontalKeys.begin(), node->orderedFrontalKeys.end()); gttoc(merge_children); node->problemSize_ = combinedProblemSize; } -}; +} +; /* ************************************************************************* */ -template -template +template +template JunctionTree::JunctionTree( const EliminationTree& eliminationTree) { gttic(JunctionTree_FromEliminationTree); @@ -147,12 +177,11 @@ JunctionTree::JunctionTree( typedef typename EliminationTree::Node ETreeNode; typedef ConstructorTraversalData Data; Data rootData(0); - rootData.myJTNode = - boost::make_shared(); // Make a dummy node to gather - // the junction tree roots + rootData.myJTNode = boost::make_shared(); // Make a dummy node to gather + // the junction tree roots treeTraversal::DepthFirstForest(eliminationTree, rootData, - Data::ConstructorTraversalVisitorPre, - Data::ConstructorTraversalVisitorPostAlg2); + Data::ConstructorTraversalVisitorPre, + Data::ConstructorTraversalVisitorPostAlg2); // Assign roots from the dummy node Base::roots_ = rootData.myJTNode->children; @@ -161,4 +190,4 @@ JunctionTree::JunctionTree( Base::remainingFactors_ = eliminationTree.remainingFactors(); } -} // namespace gtsam +} // namespace gtsam From 67013cba054e2e5bb81f7fbd8257a3ffb710b401 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 21 Jun 2015 12:49:33 -0700 Subject: [PATCH 24/46] Separated merge decision from actual merging --- gtsam/inference/JunctionTree-inst.h | 61 ++++++++++++++++++----------- 1 file changed, 39 insertions(+), 22 deletions(-) diff --git a/gtsam/inference/JunctionTree-inst.h b/gtsam/inference/JunctionTree-inst.h index 5735a3bd2..232246d5e 100644 --- a/gtsam/inference/JunctionTree-inst.h +++ b/gtsam/inference/JunctionTree-inst.h @@ -73,7 +73,8 @@ struct ConstructorTraversalData { // Do symbolic elimination for this node SymbolicFactors symbolicFactors; - symbolicFactors.reserve(ETreeNode->factors.size() + myData.childSymbolicFactors.size()); + symbolicFactors.reserve( + ETreeNode->factors.size() + myData.childSymbolicFactors.size()); // Add ETree node factors symbolicFactors += ETreeNode->factors; // Add symbolic factors passed up from children @@ -96,29 +97,42 @@ struct ConstructorTraversalData { // Merge our children if they are in our clique - if our conditional has // exactly one fewer parent than our child's conditional. - size_t myNrFrontals = 1; const size_t myNrParents = myConditional->nrParents(); - assert(node->newChildren.size() == childConditionals.size()); + const size_t nrChildren = node->children.size(); + assert(childConditionals.size() == nrChildren); gttic(merge_children); - // First count how many keys, factors and children we'll end up with + + // decide which children to merge, as index into children + std::vector merge(nrChildren, false); + { + size_t myNrFrontals = 1; + for (size_t i = 0; i < nrChildren; ++i) { + // Check if we should merge the i^th child + if (myNrParents + myNrFrontals == childConditionals[i]->nrParents()) { + sharedNode child = node->children[i]; + // Increment number of frontal variables + myNrFrontals += child->orderedFrontalKeys.size(); + merge[i] = true; + } + } + } + + // Count how many keys, factors and children we'll end up with size_t nrKeys = node->orderedFrontalKeys.size(); size_t nrFactors = node->factors.size(); - size_t nrChildren = 0; + size_t nrNewChildren = 0; // Loop over children - for (size_t i = 0; i < childConditionals.size(); ++i) { - // Check if we should merge the i^th child - if (myNrParents + myNrFrontals == childConditionals[i]->nrParents()) { + for (size_t i = 0; i < nrChildren; ++i) { + if (merge[i]) { // Get a reference to the i, adjusting the index to account for children // previously merged and removed from the i list. sharedNode child = node->children[i]; nrKeys += child->orderedFrontalKeys.size(); nrFactors += child->factors.size(); - nrChildren += child->children.size(); - // Increment number of frontal variables - myNrFrontals += child->orderedFrontalKeys.size(); + nrNewChildren += child->children.size(); } else { - nrChildren += 1; // we keep the child + nrNewChildren += 1; // we keep the child } } @@ -126,14 +140,14 @@ struct ConstructorTraversalData { node->orderedFrontalKeys.reserve(nrKeys); node->factors.reserve(nrFactors); typename Node::Children newChildren; - newChildren.reserve(nrChildren); - myNrFrontals = 1; - int combinedProblemSize = (int) (myConditional->size() * symbolicFactors.size()); + newChildren.reserve(nrNewChildren); + int combinedProblemSize = (int) (myConditional->size() + * symbolicFactors.size()); // Loop over newChildren - for (size_t i = 0; i < childConditionals.size(); ++i) { + for (size_t i = 0; i < nrChildren; ++i) { // Check if we should merge the i^th child sharedNode child = node->children[i]; - if (myNrParents + myNrFrontals == childConditionals[i]->nrParents()) { + if (merge[i]) { // Get a reference to the i, adjusting the index to account for newChildren // previously merged and removed from the i list. // Merge keys. For efficiency, we add keys in reverse order at end, calling reverse after.. @@ -141,18 +155,21 @@ struct ConstructorTraversalData { child->orderedFrontalKeys.rbegin(), child->orderedFrontalKeys.rend()); // Merge keys, factors, and children. - node->factors.insert(node->factors.end(), child->factors.begin(), child->factors.end()); - newChildren.insert(newChildren.end(), child->children.begin(), child->children.end()); + node->factors.insert(node->factors.end(), child->factors.begin(), + child->factors.end()); + newChildren.insert(newChildren.end(), child->children.begin(), + child->children.end()); // Increment problem size - combinedProblemSize = std::max(combinedProblemSize, child->problemSize_); + combinedProblemSize = std::max(combinedProblemSize, + child->problemSize_); // Increment number of frontal variables - myNrFrontals += child->orderedFrontalKeys.size(); } else { newChildren.push_back(child); // we keep the child } } node->children = newChildren; - std::reverse(node->orderedFrontalKeys.begin(), node->orderedFrontalKeys.end()); + std::reverse(node->orderedFrontalKeys.begin(), + node->orderedFrontalKeys.end()); gttoc(merge_children); node->problemSize_ = combinedProblemSize; } From 0d0a9e5b16308c1190ede3f4d4e9dfd901cf3b35 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 21 Jun 2015 12:57:26 -0700 Subject: [PATCH 25/46] Formatting only --- gtsam/inference/ClusterTree.h | 209 +++++++++++++++++----------------- 1 file changed, 105 insertions(+), 104 deletions(-) diff --git a/gtsam/inference/ClusterTree.h b/gtsam/inference/ClusterTree.h index bb7cf17e1..260397ab1 100644 --- a/gtsam/inference/ClusterTree.h +++ b/gtsam/inference/ClusterTree.h @@ -13,118 +13,119 @@ #include #include -namespace gtsam -{ +namespace gtsam { - /** - * A cluster-tree is associated with a factor graph and is defined as in Koller-Friedman: - * each node k represents a subset \f$ C_k \sub X \f$, and the tree is family preserving, in that - * each factor \f$ f_i \f$ is associated with a single cluster and \f$ scope(f_i) \sub C_k \f$. - * \nosubgrouping - */ - template - class ClusterTree - { - public: - typedef GRAPH FactorGraphType; ///< The factor graph type - typedef typename GRAPH::FactorType FactorType; ///< The type of factors - typedef ClusterTree This; ///< This class - typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class - typedef boost::shared_ptr sharedFactor; ///< Shared pointer to a factor - typedef BAYESTREE BayesTreeType; ///< The BayesTree type produced by elimination - typedef typename BayesTreeType::ConditionalType ConditionalType; ///< The type of conditionals - typedef boost::shared_ptr sharedConditional; ///< Shared pointer to a conditional - typedef typename FactorGraphType::Eliminate Eliminate; ///< Typedef for an eliminate subroutine +/** + * A cluster-tree is associated with a factor graph and is defined as in Koller-Friedman: + * each node k represents a subset \f$ C_k \sub X \f$, and the tree is family preserving, in that + * each factor \f$ f_i \f$ is associated with a single cluster and \f$ scope(f_i) \sub C_k \f$. + * \nosubgrouping + */ +template +class ClusterTree { +public: + typedef GRAPH FactorGraphType; ///< The factor graph type + typedef typename GRAPH::FactorType FactorType; ///< The type of factors + typedef ClusterTree This; ///< This class + typedef boost::shared_ptr shared_ptr; ///< Shared pointer to this class + typedef boost::shared_ptr sharedFactor; ///< Shared pointer to a factor + typedef BAYESTREE BayesTreeType; ///< The BayesTree type produced by elimination + typedef typename BayesTreeType::ConditionalType ConditionalType; ///< The type of conditionals + typedef boost::shared_ptr sharedConditional; ///< Shared pointer to a conditional + typedef typename FactorGraphType::Eliminate Eliminate; ///< Typedef for an eliminate subroutine - struct Cluster { - typedef Ordering Keys; - typedef FastVector Factors; - typedef FastVector > Children; + struct Cluster { + typedef Ordering Keys; + typedef FastVector Factors; + typedef FastVector > Children; - Cluster() {} - Cluster(Key key, const Factors& factors) : factors(factors) { - orderedFrontalKeys.push_back(key); - } + Cluster() { + } + Cluster(Key key, const Factors& factors) : + factors(factors) { + orderedFrontalKeys.push_back(key); + } - Keys orderedFrontalKeys; ///< Frontal keys of this node - Factors factors; ///< Factors associated with this node - Children children; ///< sub-trees - int problemSize_; + Keys orderedFrontalKeys; ///< Frontal keys of this node + Factors factors; ///< Factors associated with this node + Children children; ///< sub-trees + int problemSize_; - int problemSize() const { return problemSize_; } - - /** print this node */ - void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; - }; - - typedef boost::shared_ptr sharedCluster; ///< Shared pointer to Cluster - typedef Cluster Node; ///< Define Node=Cluster for compatibility with tree traversal functions - typedef sharedCluster sharedNode; ///< Define Node=Cluster for compatibility with tree traversal functions - - /** concept check */ - GTSAM_CONCEPT_TESTABLE_TYPE(FactorType); - - protected: - FastVector roots_; - FastVector remainingFactors_; - - /// @name Standard Constructors - /// @{ - - /** Copy constructor - makes a deep copy of the tree structure, but only pointers to factors are - * copied, factors are not cloned. */ - ClusterTree(const This& other) { *this = other; } - - /// @} - - public: - /// @name Testable - /// @{ - - /** Print the cluster tree */ - void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; - - /// @} - - /// @name Standard Interface - /// @{ - - /** Eliminate the factors to a Bayes tree and remaining factor graph - * @param function The function to use to eliminate, see the namespace functions - * in GaussianFactorGraph.h - * @return The Bayes tree and factor graph resulting from elimination - */ - std::pair, boost::shared_ptr > - eliminate(const Eliminate& function) const; - - /// @} - - /// @name Advanced Interface - /// @{ - - /** Return the set of roots (one for a tree, multiple for a forest) */ - const FastVector& roots() const { return roots_; } - - /** Return the remaining factors that are not pulled into elimination */ - const FastVector& remainingFactors() const { return remainingFactors_; } - - /// @} - - protected: - /// @name Details - - /// Assignment operator - makes a deep copy of the tree structure, but only pointers to factors - /// are copied, factors are not cloned. - This& operator=(const This& other); - - /// Default constructor to be used in derived classes - ClusterTree() {} - - /// @} + int problemSize() const { + return problemSize_; + } + /** print this node */ + void print(const std::string& s = "", const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const; }; + typedef boost::shared_ptr sharedCluster; ///< Shared pointer to Cluster + typedef Cluster Node; ///< Define Node=Cluster for compatibility with tree traversal functions + typedef sharedCluster sharedNode; ///< Define Node=Cluster for compatibility with tree traversal functions + + /** concept check */ + GTSAM_CONCEPT_TESTABLE_TYPE(FactorType); + +protected: + FastVector roots_; + FastVector remainingFactors_; + + /// @name Standard Constructors + /// @{ + + /** Copy constructor - makes a deep copy of the tree structure, but only pointers to factors are + * copied, factors are not cloned. */ + ClusterTree(const This& other) {*this = other;} + + /// @} + +public: + /// @name Testable + /// @{ + + /** Print the cluster tree */ + void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + + /// @} + + /// @name Standard Interface + /// @{ + + /** Eliminate the factors to a Bayes tree and remaining factor graph + * @param function The function to use to eliminate, see the namespace functions + * in GaussianFactorGraph.h + * @return The Bayes tree and factor graph resulting from elimination + */ + std::pair, boost::shared_ptr > + eliminate(const Eliminate& function) const; + + /// @} + + /// @name Advanced Interface + /// @{ + + /** Return the set of roots (one for a tree, multiple for a forest) */ + const FastVector& roots() const {return roots_;} + + /** Return the remaining factors that are not pulled into elimination */ + const FastVector& remainingFactors() const {return remainingFactors_;} + + /// @} + +protected: + /// @name Details + + /// Assignment operator - makes a deep copy of the tree structure, but only pointers to factors + /// are copied, factors are not cloned. + This& operator=(const This& other); + + /// Default constructor to be used in derived classes + ClusterTree() {} + + /// @} + +}; } - From c8f8791bab62fad97e9ed671533c9c733a09d500 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 21 Jun 2015 13:06:42 -0700 Subject: [PATCH 26/46] Moved merging to ClusterTree --- gtsam/inference/ClusterTree.h | 55 +++++++++++++++++++++ gtsam/inference/JunctionTree-inst.h | 74 ++++------------------------- 2 files changed, 65 insertions(+), 64 deletions(-) diff --git a/gtsam/inference/ClusterTree.h b/gtsam/inference/ClusterTree.h index 260397ab1..0b98d3e36 100644 --- a/gtsam/inference/ClusterTree.h +++ b/gtsam/inference/ClusterTree.h @@ -58,6 +58,61 @@ public: /** print this node */ void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + + void mergeChildren(const std::vector& merge) { + gttic(merge_children); + size_t nrChildren = children.size(); + + // Count how many keys, factors and children we'll end up with + size_t nrKeys = this->orderedFrontalKeys.size(); + size_t nrFactors = this->factors.size(); + size_t nrNewChildren = 0; + // Loop over children + for (size_t i = 0; i < nrChildren; ++i) { + if (merge[i]) { + // Get a reference to the i, adjusting the index to account for children + // previously merged and removed from the i list. + sharedNode child = this->children[i]; + nrKeys += child->orderedFrontalKeys.size(); + nrFactors += child->factors.size(); + nrNewChildren += child->children.size(); + } else { + nrNewChildren += 1; // we keep the child + } + } + + // now reserve space, and really merge + this->orderedFrontalKeys.reserve(nrKeys); + this->factors.reserve(nrFactors); + typename Node::Children newChildren; + newChildren.reserve(nrNewChildren); + // Loop over newChildren + for (size_t i = 0; i < nrChildren; ++i) { + // Check if we should merge the i^th child + sharedNode child = this->children[i]; + if (merge[i]) { + // Get a reference to the i, adjusting the index to account for newChildren + // previously merged and removed from the i list. + // Merge keys. For efficiency, we add keys in reverse order at end, calling reverse after.. + this->orderedFrontalKeys.insert(this->orderedFrontalKeys.end(), + child->orderedFrontalKeys.rbegin(), + child->orderedFrontalKeys.rend()); + // Merge keys, factors, and children. + this->factors.insert(this->factors.end(), child->factors.begin(), + child->factors.end()); + newChildren.insert(newChildren.end(), child->children.begin(), + child->children.end()); + // Increment problem size + problemSize_ = std::max(problemSize_, child->problemSize_); + // Increment number of frontal variables + } else { + newChildren.push_back(child); // we keep the child + } + } + this->children = newChildren; + std::reverse(this->orderedFrontalKeys.begin(), + this->orderedFrontalKeys.end()); + } }; typedef boost::shared_ptr sharedCluster; ///< Shared pointer to Cluster diff --git a/gtsam/inference/JunctionTree-inst.h b/gtsam/inference/JunctionTree-inst.h index 232246d5e..de349ddc8 100644 --- a/gtsam/inference/JunctionTree-inst.h +++ b/gtsam/inference/JunctionTree-inst.h @@ -94,6 +94,7 @@ struct ConstructorTraversalData { sharedNode node = myData.myJTNode; const FastVector& childConditionals = myData.childSymbolicConditionals; + node->problemSize_ = (int) (myConditional->size() * symbolicFactors.size()); // Merge our children if they are in our clique - if our conditional has // exactly one fewer parent than our child's conditional. @@ -105,76 +106,21 @@ struct ConstructorTraversalData { // decide which children to merge, as index into children std::vector merge(nrChildren, false); - { - size_t myNrFrontals = 1; - for (size_t i = 0; i < nrChildren; ++i) { - // Check if we should merge the i^th child - if (myNrParents + myNrFrontals == childConditionals[i]->nrParents()) { - sharedNode child = node->children[i]; - // Increment number of frontal variables - myNrFrontals += child->orderedFrontalKeys.size(); - merge[i] = true; - } - } - } - - // Count how many keys, factors and children we'll end up with - size_t nrKeys = node->orderedFrontalKeys.size(); - size_t nrFactors = node->factors.size(); - size_t nrNewChildren = 0; - // Loop over children - for (size_t i = 0; i < nrChildren; ++i) { - if (merge[i]) { - // Get a reference to the i, adjusting the index to account for children - // previously merged and removed from the i list. - sharedNode child = node->children[i]; - nrKeys += child->orderedFrontalKeys.size(); - nrFactors += child->factors.size(); - nrNewChildren += child->children.size(); - } else { - nrNewChildren += 1; // we keep the child - } - } - - // now reserve space, and really merge - node->orderedFrontalKeys.reserve(nrKeys); - node->factors.reserve(nrFactors); - typename Node::Children newChildren; - newChildren.reserve(nrNewChildren); - int combinedProblemSize = (int) (myConditional->size() - * symbolicFactors.size()); - // Loop over newChildren + size_t myNrFrontals = 1; for (size_t i = 0; i < nrChildren; ++i) { // Check if we should merge the i^th child - sharedNode child = node->children[i]; - if (merge[i]) { - // Get a reference to the i, adjusting the index to account for newChildren - // previously merged and removed from the i list. - // Merge keys. For efficiency, we add keys in reverse order at end, calling reverse after.. - node->orderedFrontalKeys.insert(node->orderedFrontalKeys.end(), - child->orderedFrontalKeys.rbegin(), - child->orderedFrontalKeys.rend()); - // Merge keys, factors, and children. - node->factors.insert(node->factors.end(), child->factors.begin(), - child->factors.end()); - newChildren.insert(newChildren.end(), child->children.begin(), - child->children.end()); - // Increment problem size - combinedProblemSize = std::max(combinedProblemSize, - child->problemSize_); + if (myNrParents + myNrFrontals == childConditionals[i]->nrParents()) { + sharedNode child = node->children[i]; // Increment number of frontal variables - } else { - newChildren.push_back(child); // we keep the child + myNrFrontals += child->orderedFrontalKeys.size(); + merge[i] = true; } } - node->children = newChildren; - std::reverse(node->orderedFrontalKeys.begin(), - node->orderedFrontalKeys.end()); - gttoc(merge_children); - node->problemSize_ = combinedProblemSize; + + // now really merge + node->mergeChildren(merge); } -} -; +}; /* ************************************************************************* */ template From 000f807eda758ee2423181e06b933b240e164fa9 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 21 Jun 2015 13:10:44 -0700 Subject: [PATCH 27/46] Formatting only --- gtsam/inference/ClusterTree-inst.h | 310 ++++++++++++++--------------- 1 file changed, 155 insertions(+), 155 deletions(-) diff --git a/gtsam/inference/ClusterTree-inst.h b/gtsam/inference/ClusterTree-inst.h index 1987a0a5a..b76430c20 100644 --- a/gtsam/inference/ClusterTree-inst.h +++ b/gtsam/inference/ClusterTree-inst.h @@ -7,178 +7,178 @@ * @brief Collects factorgraph fragments defined on variable clusters, arranged in a tree */ -#include -#include #include #include #include +#include +#include #include #include -namespace gtsam -{ - namespace - { - /* ************************************************************************* */ - // Elimination traversal data - stores a pointer to the parent data and collects the factors - // resulting from elimination of the children. Also sets up BayesTree cliques with parent and - // child pointers. - template - struct EliminationData { - EliminationData* const parentData; - size_t myIndexInParent; - FastVector childFactors; - boost::shared_ptr bayesTreeNode; - EliminationData(EliminationData* _parentData, size_t nChildren) : - parentData(_parentData), - bayesTreeNode(boost::make_shared()) - { - if(parentData) { - myIndexInParent = parentData->childFactors.size(); - parentData->childFactors.push_back(typename CLUSTERTREE::sharedFactor()); - } else { - myIndexInParent = 0; - } - // Set up BayesTree parent and child pointers - if(parentData) { - if(parentData->parentData) // If our parent is not the dummy node - bayesTreeNode->parent_ = parentData->bayesTreeNode; - parentData->bayesTreeNode->children.push_back(bayesTreeNode); - } - } - }; +namespace gtsam { +namespace { +/* ************************************************************************* */ +// Elimination traversal data - stores a pointer to the parent data and collects the factors +// resulting from elimination of the children. Also sets up BayesTree cliques with parent and +// child pointers. +template +struct EliminationData { + EliminationData* const parentData; + size_t myIndexInParent; + FastVector childFactors; + boost::shared_ptr bayesTreeNode; + EliminationData(EliminationData* _parentData, size_t nChildren) : + parentData(_parentData), bayesTreeNode( + boost::make_shared()) { + if (parentData) { + myIndexInParent = parentData->childFactors.size(); + parentData->childFactors.push_back(typename CLUSTERTREE::sharedFactor()); + } else { + myIndexInParent = 0; + } + // Set up BayesTree parent and child pointers + if (parentData) { + if (parentData->parentData) // If our parent is not the dummy node + bayesTreeNode->parent_ = parentData->bayesTreeNode; + parentData->bayesTreeNode->children.push_back(bayesTreeNode); + } + } +}; - /* ************************************************************************* */ - // Elimination pre-order visitor - just creates the EliminationData structure for the visited - // node. - template - EliminationData eliminationPreOrderVisitor( - const typename CLUSTERTREE::sharedNode& node, EliminationData& parentData) - { - EliminationData myData(&parentData, node->children.size()); - myData.bayesTreeNode->problemSize_ = node->problemSize(); - return myData; +/* ************************************************************************* */ +// Elimination pre-order visitor - just creates the EliminationData structure for the visited +// node. +template +EliminationData eliminationPreOrderVisitor( + const typename CLUSTERTREE::sharedNode& node, + EliminationData& parentData) { + EliminationData myData(&parentData, node->children.size()); + myData.bayesTreeNode->problemSize_ = node->problemSize(); + return myData; +} + +/* ************************************************************************* */ +// Elimination post-order visitor - combine the child factors with our own factors, add the +// resulting conditional to the BayesTree, and add the remaining factor to the parent. +template +struct EliminationPostOrderVisitor { + const typename CLUSTERTREE::Eliminate& eliminationFunction; + typename CLUSTERTREE::BayesTreeType::Nodes& nodesIndex; + EliminationPostOrderVisitor( + const typename CLUSTERTREE::Eliminate& eliminationFunction, + typename CLUSTERTREE::BayesTreeType::Nodes& nodesIndex) : + eliminationFunction(eliminationFunction), nodesIndex(nodesIndex) { + } + void operator()(const typename CLUSTERTREE::sharedNode& node, + EliminationData& myData) { + // Typedefs + typedef typename CLUSTERTREE::sharedFactor sharedFactor; + typedef typename CLUSTERTREE::FactorType FactorType; + typedef typename CLUSTERTREE::FactorGraphType FactorGraphType; + typedef typename CLUSTERTREE::ConditionalType ConditionalType; + typedef typename CLUSTERTREE::BayesTreeType::Node BTNode; + + // Gather factors + FactorGraphType gatheredFactors; + gatheredFactors.reserve(node->factors.size() + node->children.size()); + gatheredFactors += node->factors; + gatheredFactors += myData.childFactors; + + // Check for Bayes tree orphan subtrees, and add them to our children + BOOST_FOREACH(const sharedFactor& f, node->factors) { + if (const BayesTreeOrphanWrapper* asSubtree = + dynamic_cast*>(f.get())) { + myData.bayesTreeNode->children.push_back(asSubtree->clique); + asSubtree->clique->parent_ = myData.bayesTreeNode; + } } - /* ************************************************************************* */ - // Elimination post-order visitor - combine the child factors with our own factors, add the - // resulting conditional to the BayesTree, and add the remaining factor to the parent. - template - struct EliminationPostOrderVisitor - { - const typename CLUSTERTREE::Eliminate& eliminationFunction; - typename CLUSTERTREE::BayesTreeType::Nodes& nodesIndex; - EliminationPostOrderVisitor(const typename CLUSTERTREE::Eliminate& eliminationFunction, - typename CLUSTERTREE::BayesTreeType::Nodes& nodesIndex) : - eliminationFunction(eliminationFunction), nodesIndex(nodesIndex) {} - void operator()(const typename CLUSTERTREE::sharedNode& node, EliminationData& myData) - { - // Typedefs - typedef typename CLUSTERTREE::sharedFactor sharedFactor; - typedef typename CLUSTERTREE::FactorType FactorType; - typedef typename CLUSTERTREE::FactorGraphType FactorGraphType; - typedef typename CLUSTERTREE::ConditionalType ConditionalType; - typedef typename CLUSTERTREE::BayesTreeType::Node BTNode; + // Do dense elimination step + std::pair, boost::shared_ptr > eliminationResult = + eliminationFunction(gatheredFactors, node->orderedFrontalKeys); - // Gather factors - FactorGraphType gatheredFactors; - gatheredFactors.reserve(node->factors.size() + node->children.size()); - gatheredFactors += node->factors; - gatheredFactors += myData.childFactors; + // Store conditional in BayesTree clique, and in the case of ISAM2Clique also store the remaining factor + myData.bayesTreeNode->setEliminationResult(eliminationResult); - // Check for Bayes tree orphan subtrees, and add them to our children - BOOST_FOREACH(const sharedFactor& f, node->factors) - { - if(const BayesTreeOrphanWrapper* asSubtree = dynamic_cast*>(f.get())) - { - myData.bayesTreeNode->children.push_back(asSubtree->clique); - asSubtree->clique->parent_ = myData.bayesTreeNode; - } - } + // Fill nodes index - we do this here instead of calling insertRoot at the end to avoid + // putting orphan subtrees in the index - they'll already be in the index of the ISAM2 + // object they're added to. + BOOST_FOREACH(const Key& j, myData.bayesTreeNode->conditional()->frontals()) + nodesIndex.insert(std::make_pair(j, myData.bayesTreeNode)); - // Do dense elimination step - std::pair, boost::shared_ptr > eliminationResult = - eliminationFunction(gatheredFactors, node->orderedFrontalKeys); - - // Store conditional in BayesTree clique, and in the case of ISAM2Clique also store the remaining factor - myData.bayesTreeNode->setEliminationResult(eliminationResult); - - // Fill nodes index - we do this here instead of calling insertRoot at the end to avoid - // putting orphan subtrees in the index - they'll already be in the index of the ISAM2 - // object they're added to. - BOOST_FOREACH(const Key& j, myData.bayesTreeNode->conditional()->frontals()) - nodesIndex.insert(std::make_pair(j, myData.bayesTreeNode)); - - // Store remaining factor in parent's gathered factors - if(!eliminationResult.second->empty()) - myData.parentData->childFactors[myData.myIndexInParent] = eliminationResult.second; - } - }; + // Store remaining factor in parent's gathered factors + if (!eliminationResult.second->empty()) + myData.parentData->childFactors[myData.myIndexInParent] = + eliminationResult.second; } +}; +} - /* ************************************************************************* */ - template - void ClusterTree::Cluster::print( - const std::string& s, const KeyFormatter& keyFormatter) const +/* ************************************************************************* */ +template +void ClusterTree::Cluster::print(const std::string& s, + const KeyFormatter& keyFormatter) const { + std::cout << s << " (" << problemSize_ << ")"; + PrintKeyVector(orderedFrontalKeys); +} + +/* ************************************************************************* */ +template +void ClusterTree::print(const std::string& s, + const KeyFormatter& keyFormatter) const { + treeTraversal::PrintForest(*this, s, keyFormatter); +} + +/* ************************************************************************* */ +template +ClusterTree& ClusterTree::operator=( + const This& other) { + // Start by duplicating the tree. + roots_ = treeTraversal::CloneForest(other); + + // Assign the remaining factors - these are pointers to factors in the original factor graph and + // we do not clone them. + remainingFactors_ = other.remainingFactors_; + + return *this; +} + +/* ************************************************************************* */ +template +std::pair, boost::shared_ptr > ClusterTree< + BAYESTREE, GRAPH>::eliminate(const Eliminate& function) const { + gttic(ClusterTree_eliminate); + // Do elimination (depth-first traversal). The rootsContainer stores a 'dummy' BayesTree node + // that contains all of the roots as its children. rootsContainer also stores the remaining + // uneliminated factors passed up from the roots. + boost::shared_ptr result = boost::make_shared(); + EliminationData rootsContainer(0, roots_.size()); + EliminationPostOrderVisitor visitorPost(function, result->nodes_); { - std::cout << s << " (" << problemSize_ << ")" ; - PrintKeyVector(orderedFrontalKeys); - } - - /* ************************************************************************* */ - template - void ClusterTree::print( - const std::string& s, const KeyFormatter& keyFormatter) const - { - treeTraversal::PrintForest(*this, s, keyFormatter); - } - - /* ************************************************************************* */ - template - ClusterTree& ClusterTree::operator=(const This& other) - { - // Start by duplicating the tree. - roots_ = treeTraversal::CloneForest(other); - - // Assign the remaining factors - these are pointers to factors in the original factor graph and - // we do not clone them. - remainingFactors_ = other.remainingFactors_; - - return *this; - } - - /* ************************************************************************* */ - template - std::pair, boost::shared_ptr > - ClusterTree::eliminate(const Eliminate& function) const - { - gttic(ClusterTree_eliminate); - // Do elimination (depth-first traversal). The rootsContainer stores a 'dummy' BayesTree node - // that contains all of the roots as its children. rootsContainer also stores the remaining - // uneliminated factors passed up from the roots. - boost::shared_ptr result = boost::make_shared(); - EliminationData rootsContainer(0, roots_.size()); - EliminationPostOrderVisitor visitorPost(function, result->nodes_); - { - TbbOpenMPMixedScope threadLimiter; // Limits OpenMP threads since we're mixing TBB and OpenMP - treeTraversal::DepthFirstForestParallel(*this, rootsContainer, + TbbOpenMPMixedScope threadLimiter; // Limits OpenMP threads since we're mixing TBB and OpenMP + treeTraversal::DepthFirstForestParallel(*this, rootsContainer, eliminationPreOrderVisitor, visitorPost, 10); - } - - // Create BayesTree from roots stored in the dummy BayesTree node. - result->roots_.insert(result->roots_.end(), rootsContainer.bayesTreeNode->children.begin(), rootsContainer.bayesTreeNode->children.end()); - - // Add remaining factors that were not involved with eliminated variables - boost::shared_ptr allRemainingFactors = boost::make_shared(); - allRemainingFactors->reserve(remainingFactors_.size() + rootsContainer.childFactors.size()); - allRemainingFactors->push_back(remainingFactors_.begin(), remainingFactors_.end()); - BOOST_FOREACH(const sharedFactor& factor, rootsContainer.childFactors) - if(factor) - allRemainingFactors->push_back(factor); - - // Return result - return std::make_pair(result, allRemainingFactors); } + // Create BayesTree from roots stored in the dummy BayesTree node. + result->roots_.insert(result->roots_.end(), + rootsContainer.bayesTreeNode->children.begin(), + rootsContainer.bayesTreeNode->children.end()); + + // Add remaining factors that were not involved with eliminated variables + boost::shared_ptr allRemainingFactors = boost::make_shared< + FactorGraphType>(); + allRemainingFactors->reserve( + remainingFactors_.size() + rootsContainer.childFactors.size()); + allRemainingFactors->push_back(remainingFactors_.begin(), + remainingFactors_.end()); + BOOST_FOREACH(const sharedFactor& factor, rootsContainer.childFactors) + if (factor) + allRemainingFactors->push_back(factor); + + // Return result + return std::make_pair(result, allRemainingFactors); +} + } From b95bc712f41f6c1dff21df02f431b8fedc2f9d02 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 21 Jun 2015 13:18:28 -0700 Subject: [PATCH 28/46] Kill stray timing --- gtsam/inference/JunctionTree-inst.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/gtsam/inference/JunctionTree-inst.h b/gtsam/inference/JunctionTree-inst.h index de349ddc8..2df7aa930 100644 --- a/gtsam/inference/JunctionTree-inst.h +++ b/gtsam/inference/JunctionTree-inst.h @@ -102,8 +102,6 @@ struct ConstructorTraversalData { const size_t nrChildren = node->children.size(); assert(childConditionals.size() == nrChildren); - gttic(merge_children); - // decide which children to merge, as index into children std::vector merge(nrChildren, false); size_t myNrFrontals = 1; From d34c1808a8d1f4b0dceea6a4b490ab367f89a96d Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 21 Jun 2015 13:18:43 -0700 Subject: [PATCH 29/46] Moved to inst file --- gtsam/inference/ClusterTree-inst.h | 56 +++++++++++++++++++++++++++++ gtsam/inference/ClusterTree.h | 58 ++---------------------------- 2 files changed, 59 insertions(+), 55 deletions(-) diff --git a/gtsam/inference/ClusterTree-inst.h b/gtsam/inference/ClusterTree-inst.h index b76430c20..bbe878907 100644 --- a/gtsam/inference/ClusterTree-inst.h +++ b/gtsam/inference/ClusterTree-inst.h @@ -123,6 +123,62 @@ void ClusterTree::Cluster::print(const std::string& s, PrintKeyVector(orderedFrontalKeys); } +/* ************************************************************************* */ +template +void ClusterTree::Cluster::mergeChildren( + const std::vector& merge) { + gttic(Cluster::mergeChildren); + size_t nrChildren = children.size(); + + // Count how many keys, factors and children we'll end up with + size_t nrKeys = orderedFrontalKeys.size(); + size_t nrFactors = factors.size(); + size_t nrNewChildren = 0; + // Loop over children + for (size_t i = 0; i < nrChildren; ++i) { + if (merge[i]) { + // Get a reference to the i, adjusting the index to account for children + // previously merged and removed from the i list. + sharedNode child = children[i]; + nrKeys += child->orderedFrontalKeys.size(); + nrFactors += child->factors.size(); + nrNewChildren += child->children.size(); + } else { + nrNewChildren += 1; // we keep the child + } + } + + // now reserve space, and really merge + orderedFrontalKeys.reserve(nrKeys); + factors.reserve(nrFactors); + typename Node::Children newChildren; + newChildren.reserve(nrNewChildren); + // Loop over newChildren + for (size_t i = 0; i < nrChildren; ++i) { + // Check if we should merge the i^th child + sharedNode child = children[i]; + if (merge[i]) { + // Get a reference to the i, adjusting the index to account for newChildren + // previously merged and removed from the i list. + // Merge keys. For efficiency, we add keys in reverse order at end, calling reverse after.. + orderedFrontalKeys.insert(orderedFrontalKeys.end(), + child->orderedFrontalKeys.rbegin(), child->orderedFrontalKeys.rend()); + // Merge keys, factors, and children. + factors.insert(factors.end(), child->factors.begin(), + child->factors.end()); + newChildren.insert(newChildren.end(), child->children.begin(), + child->children.end()); + // Increment problem size + problemSize_ = std::max(problemSize_, child->problemSize_); + // Increment number of frontal variables + } else { + newChildren.push_back(child); // we keep the child + } + } + children = newChildren; + std::reverse(orderedFrontalKeys.begin(), orderedFrontalKeys.end()); +} + /* ************************************************************************* */ template void ClusterTree::print(const std::string& s, diff --git a/gtsam/inference/ClusterTree.h b/gtsam/inference/ClusterTree.h index 0b98d3e36..b00532d22 100644 --- a/gtsam/inference/ClusterTree.h +++ b/gtsam/inference/ClusterTree.h @@ -55,64 +55,12 @@ public: return problemSize_; } - /** print this node */ + /// print this node void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; - void mergeChildren(const std::vector& merge) { - gttic(merge_children); - size_t nrChildren = children.size(); - - // Count how many keys, factors and children we'll end up with - size_t nrKeys = this->orderedFrontalKeys.size(); - size_t nrFactors = this->factors.size(); - size_t nrNewChildren = 0; - // Loop over children - for (size_t i = 0; i < nrChildren; ++i) { - if (merge[i]) { - // Get a reference to the i, adjusting the index to account for children - // previously merged and removed from the i list. - sharedNode child = this->children[i]; - nrKeys += child->orderedFrontalKeys.size(); - nrFactors += child->factors.size(); - nrNewChildren += child->children.size(); - } else { - nrNewChildren += 1; // we keep the child - } - } - - // now reserve space, and really merge - this->orderedFrontalKeys.reserve(nrKeys); - this->factors.reserve(nrFactors); - typename Node::Children newChildren; - newChildren.reserve(nrNewChildren); - // Loop over newChildren - for (size_t i = 0; i < nrChildren; ++i) { - // Check if we should merge the i^th child - sharedNode child = this->children[i]; - if (merge[i]) { - // Get a reference to the i, adjusting the index to account for newChildren - // previously merged and removed from the i list. - // Merge keys. For efficiency, we add keys in reverse order at end, calling reverse after.. - this->orderedFrontalKeys.insert(this->orderedFrontalKeys.end(), - child->orderedFrontalKeys.rbegin(), - child->orderedFrontalKeys.rend()); - // Merge keys, factors, and children. - this->factors.insert(this->factors.end(), child->factors.begin(), - child->factors.end()); - newChildren.insert(newChildren.end(), child->children.begin(), - child->children.end()); - // Increment problem size - problemSize_ = std::max(problemSize_, child->problemSize_); - // Increment number of frontal variables - } else { - newChildren.push_back(child); // we keep the child - } - } - this->children = newChildren; - std::reverse(this->orderedFrontalKeys.begin(), - this->orderedFrontalKeys.end()); - } + /// Merge all children for which bit is set into this node + void mergeChildren(const std::vector& merge); }; typedef boost::shared_ptr sharedCluster; ///< Shared pointer to Cluster From e2d49922d22ced379efe31fbaf4a7c0601bd56f6 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 21 Jun 2015 13:39:55 -0700 Subject: [PATCH 30/46] Switch to list - made code be container-agnostic --- gtsam/inference/ClusterTree-inst.h | 19 +++++++------------ gtsam/inference/ClusterTree.h | 2 +- gtsam/inference/JunctionTree-inst.h | 11 +++++++---- tests/testGaussianJunctionTreeB.cpp | 10 +++++----- 4 files changed, 20 insertions(+), 22 deletions(-) diff --git a/gtsam/inference/ClusterTree-inst.h b/gtsam/inference/ClusterTree-inst.h index bbe878907..d777fcfe7 100644 --- a/gtsam/inference/ClusterTree-inst.h +++ b/gtsam/inference/ClusterTree-inst.h @@ -128,38 +128,32 @@ template void ClusterTree::Cluster::mergeChildren( const std::vector& merge) { gttic(Cluster::mergeChildren); - size_t nrChildren = children.size(); // Count how many keys, factors and children we'll end up with size_t nrKeys = orderedFrontalKeys.size(); size_t nrFactors = factors.size(); size_t nrNewChildren = 0; // Loop over children - for (size_t i = 0; i < nrChildren; ++i) { + size_t i = 0; + BOOST_FOREACH(const sharedNode& child, children) { if (merge[i]) { - // Get a reference to the i, adjusting the index to account for children - // previously merged and removed from the i list. - sharedNode child = children[i]; nrKeys += child->orderedFrontalKeys.size(); nrFactors += child->factors.size(); nrNewChildren += child->children.size(); } else { nrNewChildren += 1; // we keep the child } + ++i; } // now reserve space, and really merge orderedFrontalKeys.reserve(nrKeys); factors.reserve(nrFactors); typename Node::Children newChildren; - newChildren.reserve(nrNewChildren); - // Loop over newChildren - for (size_t i = 0; i < nrChildren; ++i) { - // Check if we should merge the i^th child - sharedNode child = children[i]; + // newChildren.reserve(nrNewChildren); + i = 0; + BOOST_FOREACH(const sharedNode& child, children) { if (merge[i]) { - // Get a reference to the i, adjusting the index to account for newChildren - // previously merged and removed from the i list. // Merge keys. For efficiency, we add keys in reverse order at end, calling reverse after.. orderedFrontalKeys.insert(orderedFrontalKeys.end(), child->orderedFrontalKeys.rbegin(), child->orderedFrontalKeys.rend()); @@ -174,6 +168,7 @@ void ClusterTree::Cluster::mergeChildren( } else { newChildren.push_back(child); // we keep the child } + ++i; } children = newChildren; std::reverse(orderedFrontalKeys.begin(), orderedFrontalKeys.end()); diff --git a/gtsam/inference/ClusterTree.h b/gtsam/inference/ClusterTree.h index b00532d22..41232e1a1 100644 --- a/gtsam/inference/ClusterTree.h +++ b/gtsam/inference/ClusterTree.h @@ -37,7 +37,7 @@ public: struct Cluster { typedef Ordering Keys; typedef FastVector Factors; - typedef FastVector > Children; + typedef FastList > Children; Cluster() { } diff --git a/gtsam/inference/JunctionTree-inst.h b/gtsam/inference/JunctionTree-inst.h index 2df7aa930..352a8dded 100644 --- a/gtsam/inference/JunctionTree-inst.h +++ b/gtsam/inference/JunctionTree-inst.h @@ -104,15 +104,15 @@ struct ConstructorTraversalData { // decide which children to merge, as index into children std::vector merge(nrChildren, false); - size_t myNrFrontals = 1; - for (size_t i = 0; i < nrChildren; ++i) { + size_t myNrFrontals = 1, i = 0; + BOOST_FOREACH(const sharedNode& child, node->children) { // Check if we should merge the i^th child if (myNrParents + myNrFrontals == childConditionals[i]->nrParents()) { - sharedNode child = node->children[i]; // Increment number of frontal variables myNrFrontals += child->orderedFrontalKeys.size(); merge[i] = true; } + ++i; } // now really merge @@ -145,7 +145,10 @@ JunctionTree::JunctionTree( Data::ConstructorTraversalVisitorPostAlg2); // Assign roots from the dummy node - Base::roots_ = rootData.myJTNode->children; + typedef typename JunctionTree::Node Node; + const typename Node::Children& children = rootData.myJTNode->children; + Base::roots_.reserve(children.size()); + Base::roots_.insert(Base::roots_.begin(), children.begin(), children.end()); // Transfer remaining factors from elimination tree Base::remainingFactors_ = eliminationTree.remainingFactors(); diff --git a/tests/testGaussianJunctionTreeB.cpp b/tests/testGaussianJunctionTreeB.cpp index c5401512b..18d249894 100644 --- a/tests/testGaussianJunctionTreeB.cpp +++ b/tests/testGaussianJunctionTreeB.cpp @@ -103,15 +103,15 @@ TEST( GaussianJunctionTreeB, constructor2 ) GaussianJunctionTree::sharedNode x324 = actual.roots().front(); LONGS_EQUAL(2, x324->children.size()); #if defined(__APPLE__) // tie-breaking seems different :-( - GaussianJunctionTree::sharedNode x1 = x324->children[0]; - GaussianJunctionTree::sharedNode x56 = x324->children[1]; + GaussianJunctionTree::sharedNode x1 = x324->children.front(); + GaussianJunctionTree::sharedNode x56 = x324->children.back(); #else - GaussianJunctionTree::sharedNode x1 = x324->children[1]; - GaussianJunctionTree::sharedNode x56 = x324->children[0]; + GaussianJunctionTree::sharedNode x1 = x324->children.back(); + GaussianJunctionTree::sharedNode x56 = x324->children.front(); #endif LONGS_EQUAL(0, x1->children.size()); LONGS_EQUAL(1, x56->children.size()); - GaussianJunctionTree::sharedNode x7 = x56->children[0]; + GaussianJunctionTree::sharedNode x7 = x56->children.front(); LONGS_EQUAL(0, x7->children.size()); EXPECT(assert_equal(o324, x324->orderedFrontalKeys)); From 22b7d8276aa80a56a19b6ac3e719175402d6f700 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 21 Jun 2015 13:56:48 -0700 Subject: [PATCH 31/46] Formatting --- gtsam/base/treeTraversal-inst.h | 395 ++++++++++++++++---------------- 1 file changed, 200 insertions(+), 195 deletions(-) diff --git a/gtsam/base/treeTraversal-inst.h b/gtsam/base/treeTraversal-inst.h index 3edd7d076..669186e3f 100644 --- a/gtsam/base/treeTraversal-inst.h +++ b/gtsam/base/treeTraversal-inst.h @@ -1,19 +1,19 @@ /* ---------------------------------------------------------------------------- -* 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) + * 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 + * See LICENSE for the license information -* -------------------------------------------------------------------------- */ + * -------------------------------------------------------------------------- */ /** -* @file treeTraversal-inst.h -* @author Richard Roberts -* @date April 9, 2013 -*/ + * @file treeTraversal-inst.h + * @author Richard Roberts + * @date April 9, 2013 + */ #pragma once #include @@ -33,192 +33,197 @@ namespace gtsam { - /** Internal functions used for traversing trees */ - namespace treeTraversal { +/** Internal functions used for traversing trees */ +namespace treeTraversal { - /* ************************************************************************* */ - namespace { - // Internal node used in DFS preorder stack - template - struct TraversalNode { - bool expanded; - const boost::shared_ptr& treeNode; - DATA& parentData; - typename FastList::iterator dataPointer; - TraversalNode(const boost::shared_ptr& _treeNode, DATA& _parentData) : - expanded(false), treeNode(_treeNode), parentData(_parentData) {} - }; - - // Do nothing - default argument for post-visitor for tree traversal - struct no_op { - template - void operator()(const boost::shared_ptr& node, const DATA& data) {} - }; - - } - - /** Traverse a forest depth-first with pre-order and post-order visits. - * @param forest The forest of trees to traverse. The method \c forest.roots() should exist - * and return a collection of (shared) pointers to \c FOREST::Node. - * @param visitorPre \c visitorPre(node, parentData) will be called at every node, before - * visiting its children, and will be passed, by reference, the \c DATA object returned - * by the visit to its parent. Likewise, \c visitorPre should return the \c DATA object - * to pass to the children. The returned \c DATA object will be copy-constructed only - * upon returning to store internally, thus may be modified by visiting the children. - * Regarding efficiency, this copy-on-return is usually optimized out by the compiler. - * @param visitorPost \c visitorPost(node, data) will be called at every node, after visiting - * its children, and will be passed, by reference, the \c DATA object returned by the - * call to \c visitorPre (the \c DATA object may be modified by visiting the children). - * @param rootData The data to pass by reference to \c visitorPre when it is called on each - * root node. */ - template - void DepthFirstForest(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost) - { - // Typedefs - typedef typename FOREST::Node Node; - typedef boost::shared_ptr sharedNode; - - // Depth first traversal stack - typedef TraversalNode TraversalNode; - typedef FastList Stack; - Stack stack; - FastList dataList; // List to store node data as it is returned from the pre-order visitor - - // Add roots to stack (insert such that they are visited and processed in order - { - typename Stack::iterator insertLocation = stack.begin(); - BOOST_FOREACH(const sharedNode& root, forest.roots()) - stack.insert(insertLocation, TraversalNode(root, rootData)); - } - - // Traverse - while(!stack.empty()) - { - // Get next node - TraversalNode& node = stack.front(); - - if(node.expanded) { - // If already expanded, then the data stored in the node is no longer needed, so visit - // then delete it. - (void) visitorPost(node.treeNode, *node.dataPointer); - dataList.erase(node.dataPointer); - stack.pop_front(); - } else { - // If not already visited, visit the node and add its children (use reverse iterators so - // children are processed in the order they appear) - node.dataPointer = dataList.insert(dataList.end(), visitorPre(node.treeNode, node.parentData)); - typename Stack::iterator insertLocation = stack.begin(); - BOOST_FOREACH(const sharedNode& child, node.treeNode->children) - stack.insert(insertLocation, TraversalNode(child, *node.dataPointer)); - node.expanded = true; - } - } - assert(dataList.empty()); - } - - /** Traverse a forest depth-first, with a pre-order visit but no post-order visit. - * @param forest The forest of trees to traverse. The method \c forest.roots() should exist - * and return a collection of (shared) pointers to \c FOREST::Node. - * @param visitorPre \c visitorPre(node, parentData) will be called at every node, before - * visiting its children, and will be passed, by reference, the \c DATA object returned - * by the visit to its parent. Likewise, \c visitorPre should return the \c DATA object - * to pass to the children. The returned \c DATA object will be copy-constructed only - * upon returning to store internally, thus may be modified by visiting the children. - * Regarding efficiency, this copy-on-return is usually optimized out by the compiler. - * @param rootData The data to pass by reference to \c visitorPre when it is called on each - * root node. */ - template - void DepthFirstForest(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre) - { - no_op visitorPost; - DepthFirstForest(forest, rootData, visitorPre, visitorPost); - } - - /** Traverse a forest depth-first with pre-order and post-order visits. - * @param forest The forest of trees to traverse. The method \c forest.roots() should exist - * and return a collection of (shared) pointers to \c FOREST::Node. - * @param visitorPre \c visitorPre(node, parentData) will be called at every node, before - * visiting its children, and will be passed, by reference, the \c DATA object returned - * by the visit to its parent. Likewise, \c visitorPre should return the \c DATA object - * to pass to the children. The returned \c DATA object will be copy-constructed only - * upon returning to store internally, thus may be modified by visiting the children. - * Regarding efficiency, this copy-on-return is usually optimized out by the compiler. - * @param visitorPost \c visitorPost(node, data) will be called at every node, after visiting - * its children, and will be passed, by reference, the \c DATA object returned by the - * call to \c visitorPre (the \c DATA object may be modified by visiting the children). - * @param rootData The data to pass by reference to \c visitorPre when it is called on each - * root node. */ - template - void DepthFirstForestParallel(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, - int problemSizeThreshold = 10) - { -#ifdef GTSAM_USE_TBB - // Typedefs - typedef typename FOREST::Node Node; - typedef boost::shared_ptr sharedNode; - - tbb::task::spawn_root_and_wait(internal::CreateRootTask( - forest.roots(), rootData, visitorPre, visitorPost, problemSizeThreshold)); -#else - DepthFirstForest(forest, rootData, visitorPre, visitorPost); -#endif - } - - - /* ************************************************************************* */ - /** Traversal function for CloneForest */ - namespace { - template - boost::shared_ptr - CloneForestVisitorPre(const boost::shared_ptr& node, const boost::shared_ptr& parentPointer) - { - // Clone the current node and add it to its cloned parent - boost::shared_ptr clone = boost::make_shared(*node); - clone->children.clear(); - parentPointer->children.push_back(clone); - return clone; - } - } - - /** Clone a tree, copy-constructing new nodes (calling boost::make_shared) and setting up child - * pointers for a clone of the original tree. - * @param forest The forest of trees to clone. The method \c forest.roots() should exist and - * return a collection of shared pointers to \c FOREST::Node. - * @return The new collection of roots. */ - template - FastVector > CloneForest(const FOREST& forest) - { - typedef typename FOREST::Node Node; - boost::shared_ptr rootContainer = boost::make_shared(); - DepthFirstForest(forest, rootContainer, CloneForestVisitorPre); - return FastVector >(rootContainer->children.begin(), rootContainer->children.end()); - } - - - /* ************************************************************************* */ - /** Traversal function for PrintForest */ - namespace { - struct PrintForestVisitorPre - { - const KeyFormatter& formatter; - PrintForestVisitorPre(const KeyFormatter& formatter) : formatter(formatter) {} - template std::string operator()(const boost::shared_ptr& node, const std::string& parentString) - { - // Print the current node - node->print(parentString + "-", formatter); - // Increment the indentation - return parentString + "| "; - } - }; - } - - /** Print a tree, prefixing each line with \c str, and formatting keys using \c keyFormatter. - * To print each node, this function calls the \c print function of the tree nodes. */ - template - void PrintForest(const FOREST& forest, std::string str, const KeyFormatter& keyFormatter) { - PrintForestVisitorPre visitor(keyFormatter); - DepthFirstForest(forest, str, visitor); - } +/* ************************************************************************* */ +namespace { +// Internal node used in DFS preorder stack +template +struct TraversalNode { + bool expanded; + const boost::shared_ptr& treeNode; + DATA& parentData; + typename FastList::iterator dataPointer; + TraversalNode(const boost::shared_ptr& _treeNode, DATA& _parentData) : + expanded(false), treeNode(_treeNode), parentData(_parentData) { } +}; + +// Do nothing - default argument for post-visitor for tree traversal +struct no_op { + template + void operator()(const boost::shared_ptr& node, const DATA& data) { + } +}; + +} + +/** Traverse a forest depth-first with pre-order and post-order visits. + * @param forest The forest of trees to traverse. The method \c forest.roots() should exist + * and return a collection of (shared) pointers to \c FOREST::Node. + * @param visitorPre \c visitorPre(node, parentData) will be called at every node, before + * visiting its children, and will be passed, by reference, the \c DATA object returned + * by the visit to its parent. Likewise, \c visitorPre should return the \c DATA object + * to pass to the children. The returned \c DATA object will be copy-constructed only + * upon returning to store internally, thus may be modified by visiting the children. + * Regarding efficiency, this copy-on-return is usually optimized out by the compiler. + * @param visitorPost \c visitorPost(node, data) will be called at every node, after visiting + * its children, and will be passed, by reference, the \c DATA object returned by the + * call to \c visitorPre (the \c DATA object may be modified by visiting the children). + * @param rootData The data to pass by reference to \c visitorPre when it is called on each + * root node. */ +template +void DepthFirstForest(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre, + VISITOR_POST& visitorPost) { + // Typedefs + typedef typename FOREST::Node Node; + typedef boost::shared_ptr sharedNode; + + // Depth first traversal stack + typedef TraversalNode TraversalNode; + typedef FastList Stack; + Stack stack; + FastList dataList; // List to store node data as it is returned from the pre-order visitor + + // Add roots to stack (insert such that they are visited and processed in order + { + typename Stack::iterator insertLocation = stack.begin(); + BOOST_FOREACH(const sharedNode& root, forest.roots()) + stack.insert(insertLocation, TraversalNode(root, rootData)); + } + + // Traverse + while (!stack.empty()) { + // Get next node + TraversalNode& node = stack.front(); + + if (node.expanded) { + // If already expanded, then the data stored in the node is no longer needed, so visit + // then delete it. + (void) visitorPost(node.treeNode, *node.dataPointer); + dataList.erase(node.dataPointer); + stack.pop_front(); + } else { + // If not already visited, visit the node and add its children (use reverse iterators so + // children are processed in the order they appear) + node.dataPointer = dataList.insert(dataList.end(), + visitorPre(node.treeNode, node.parentData)); + typename Stack::iterator insertLocation = stack.begin(); + BOOST_FOREACH(const sharedNode& child, node.treeNode->children) + stack.insert(insertLocation, TraversalNode(child, *node.dataPointer)); + node.expanded = true; + } + } + assert(dataList.empty()); +} + +/** Traverse a forest depth-first, with a pre-order visit but no post-order visit. + * @param forest The forest of trees to traverse. The method \c forest.roots() should exist + * and return a collection of (shared) pointers to \c FOREST::Node. + * @param visitorPre \c visitorPre(node, parentData) will be called at every node, before + * visiting its children, and will be passed, by reference, the \c DATA object returned + * by the visit to its parent. Likewise, \c visitorPre should return the \c DATA object + * to pass to the children. The returned \c DATA object will be copy-constructed only + * upon returning to store internally, thus may be modified by visiting the children. + * Regarding efficiency, this copy-on-return is usually optimized out by the compiler. + * @param rootData The data to pass by reference to \c visitorPre when it is called on each + * root node. */ +template +void DepthFirstForest(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre) { + no_op visitorPost; + DepthFirstForest(forest, rootData, visitorPre, visitorPost); +} + +/** Traverse a forest depth-first with pre-order and post-order visits. + * @param forest The forest of trees to traverse. The method \c forest.roots() should exist + * and return a collection of (shared) pointers to \c FOREST::Node. + * @param visitorPre \c visitorPre(node, parentData) will be called at every node, before + * visiting its children, and will be passed, by reference, the \c DATA object returned + * by the visit to its parent. Likewise, \c visitorPre should return the \c DATA object + * to pass to the children. The returned \c DATA object will be copy-constructed only + * upon returning to store internally, thus may be modified by visiting the children. + * Regarding efficiency, this copy-on-return is usually optimized out by the compiler. + * @param visitorPost \c visitorPost(node, data) will be called at every node, after visiting + * its children, and will be passed, by reference, the \c DATA object returned by the + * call to \c visitorPre (the \c DATA object may be modified by visiting the children). + * @param rootData The data to pass by reference to \c visitorPre when it is called on each + * root node. */ +template +void DepthFirstForestParallel(FOREST& forest, DATA& rootData, + VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, + int problemSizeThreshold = 10) { +#ifdef GTSAM_USE_TBB + // Typedefs + typedef typename FOREST::Node Node; + typedef boost::shared_ptr sharedNode; + + tbb::task::spawn_root_and_wait( + internal::CreateRootTask(forest.roots(), rootData, visitorPre, + visitorPost, problemSizeThreshold)); +#else + DepthFirstForest(forest, rootData, visitorPre, visitorPost); +#endif +} + +/* ************************************************************************* */ +/** Traversal function for CloneForest */ +namespace { +template +boost::shared_ptr CloneForestVisitorPre( + const boost::shared_ptr& node, + const boost::shared_ptr& parentPointer) { + // Clone the current node and add it to its cloned parent + boost::shared_ptr clone = boost::make_shared(*node); + clone->children.clear(); + parentPointer->children.push_back(clone); + return clone; +} +} + +/** Clone a tree, copy-constructing new nodes (calling boost::make_shared) and setting up child + * pointers for a clone of the original tree. + * @param forest The forest of trees to clone. The method \c forest.roots() should exist and + * return a collection of shared pointers to \c FOREST::Node. + * @return The new collection of roots. */ +template +FastVector > CloneForest( + const FOREST& forest) { + typedef typename FOREST::Node Node; + boost::shared_ptr rootContainer = boost::make_shared(); + DepthFirstForest(forest, rootContainer, CloneForestVisitorPre); + return FastVector >(rootContainer->children.begin(), + rootContainer->children.end()); +} + +/* ************************************************************************* */ +/** Traversal function for PrintForest */ +namespace { +struct PrintForestVisitorPre { + const KeyFormatter& formatter; + PrintForestVisitorPre(const KeyFormatter& formatter) : + formatter(formatter) { + } + template std::string operator()( + const boost::shared_ptr& node, const std::string& parentString) { + // Print the current node + node->print(parentString + "-", formatter); + // Increment the indentation + return parentString + "| "; + } +}; +} + +/** Print a tree, prefixing each line with \c str, and formatting keys using \c keyFormatter. + * To print each node, this function calls the \c print function of the tree nodes. */ +template +void PrintForest(const FOREST& forest, std::string str, + const KeyFormatter& keyFormatter) { + PrintForestVisitorPre visitor(keyFormatter); + DepthFirstForest(forest, str, visitor); +} +} } From a35adc127cc52c09048e0cd887bfab35a562001e Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 21 Jun 2015 14:18:24 -0700 Subject: [PATCH 32/46] Reverted back to vector --- gtsam/inference/ClusterTree.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/inference/ClusterTree.h b/gtsam/inference/ClusterTree.h index 41232e1a1..b00532d22 100644 --- a/gtsam/inference/ClusterTree.h +++ b/gtsam/inference/ClusterTree.h @@ -37,7 +37,7 @@ public: struct Cluster { typedef Ordering Keys; typedef FastVector Factors; - typedef FastList > Children; + typedef FastVector > Children; Cluster() { } From ef829c333e114646132bcf664ee785fd2dfbcbea Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 21 Jun 2015 14:19:01 -0700 Subject: [PATCH 33/46] Refactored elimination traversal a tiny bit --- gtsam/inference/ClusterTree-inst.h | 156 ++++++++++++++--------------- 1 file changed, 77 insertions(+), 79 deletions(-) diff --git a/gtsam/inference/ClusterTree-inst.h b/gtsam/inference/ClusterTree-inst.h index d777fcfe7..2cf1f5c58 100644 --- a/gtsam/inference/ClusterTree-inst.h +++ b/gtsam/inference/ClusterTree-inst.h @@ -17,23 +17,29 @@ #include namespace gtsam { -namespace { + /* ************************************************************************* */ // Elimination traversal data - stores a pointer to the parent data and collects the factors // resulting from elimination of the children. Also sets up BayesTree cliques with parent and // child pointers. template struct EliminationData { + // Typedefs + typedef typename CLUSTERTREE::sharedFactor sharedFactor; + typedef typename CLUSTERTREE::FactorType FactorType; + typedef typename CLUSTERTREE::FactorGraphType FactorGraphType; + typedef typename CLUSTERTREE::ConditionalType ConditionalType; + typedef typename CLUSTERTREE::BayesTreeType::Node BTNode; + EliminationData* const parentData; size_t myIndexInParent; - FastVector childFactors; - boost::shared_ptr bayesTreeNode; + FastVector childFactors; + boost::shared_ptr bayesTreeNode; EliminationData(EliminationData* _parentData, size_t nChildren) : - parentData(_parentData), bayesTreeNode( - boost::make_shared()) { + parentData(_parentData), bayesTreeNode(boost::make_shared()) { if (parentData) { myIndexInParent = parentData->childFactors.size(); - parentData->childFactors.push_back(typename CLUSTERTREE::sharedFactor()); + parentData->childFactors.push_back(sharedFactor()); } else { myIndexInParent = 0; } @@ -44,76 +50,67 @@ struct EliminationData { parentData->bayesTreeNode->children.push_back(bayesTreeNode); } } -}; -/* ************************************************************************* */ -// Elimination pre-order visitor - just creates the EliminationData structure for the visited -// node. -template -EliminationData eliminationPreOrderVisitor( - const typename CLUSTERTREE::sharedNode& node, - EliminationData& parentData) { - EliminationData myData(&parentData, node->children.size()); - myData.bayesTreeNode->problemSize_ = node->problemSize(); - return myData; -} - -/* ************************************************************************* */ -// Elimination post-order visitor - combine the child factors with our own factors, add the -// resulting conditional to the BayesTree, and add the remaining factor to the parent. -template -struct EliminationPostOrderVisitor { - const typename CLUSTERTREE::Eliminate& eliminationFunction; - typename CLUSTERTREE::BayesTreeType::Nodes& nodesIndex; - EliminationPostOrderVisitor( - const typename CLUSTERTREE::Eliminate& eliminationFunction, - typename CLUSTERTREE::BayesTreeType::Nodes& nodesIndex) : - eliminationFunction(eliminationFunction), nodesIndex(nodesIndex) { + // Elimination pre-order visitor - creates the EliminationData structure for the visited node. + static EliminationData EliminationPreOrderVisitor( + const typename CLUSTERTREE::sharedNode& node, + EliminationData& parentData) { + assert(node); + EliminationData myData(&parentData, node->children.size()); + myData.bayesTreeNode->problemSize_ = node->problemSize(); + return myData; } - void operator()(const typename CLUSTERTREE::sharedNode& node, - EliminationData& myData) { - // Typedefs - typedef typename CLUSTERTREE::sharedFactor sharedFactor; - typedef typename CLUSTERTREE::FactorType FactorType; - typedef typename CLUSTERTREE::FactorGraphType FactorGraphType; - typedef typename CLUSTERTREE::ConditionalType ConditionalType; - typedef typename CLUSTERTREE::BayesTreeType::Node BTNode; - // Gather factors - FactorGraphType gatheredFactors; - gatheredFactors.reserve(node->factors.size() + node->children.size()); - gatheredFactors += node->factors; - gatheredFactors += myData.childFactors; - - // Check for Bayes tree orphan subtrees, and add them to our children - BOOST_FOREACH(const sharedFactor& f, node->factors) { - if (const BayesTreeOrphanWrapper* asSubtree = - dynamic_cast*>(f.get())) { - myData.bayesTreeNode->children.push_back(asSubtree->clique); - asSubtree->clique->parent_ = myData.bayesTreeNode; - } + // Elimination post-order visitor - combine the child factors with our own factors, add the + // resulting conditional to the BayesTree, and add the remaining factor to the parent. + struct EliminationPostOrderVisitor { + const typename CLUSTERTREE::Eliminate& eliminationFunction; + typename CLUSTERTREE::BayesTreeType::Nodes& nodesIndex; + EliminationPostOrderVisitor( + const typename CLUSTERTREE::Eliminate& eliminationFunction, + typename CLUSTERTREE::BayesTreeType::Nodes& nodesIndex) : + eliminationFunction(eliminationFunction), nodesIndex(nodesIndex) { } + void operator()(const typename CLUSTERTREE::sharedNode& node, + EliminationData& myData) { + assert(node); - // Do dense elimination step - std::pair, boost::shared_ptr > eliminationResult = - eliminationFunction(gatheredFactors, node->orderedFrontalKeys); + // Gather factors + FactorGraphType gatheredFactors; + gatheredFactors.reserve(node->factors.size() + node->children.size()); + gatheredFactors += node->factors; + gatheredFactors += myData.childFactors; - // Store conditional in BayesTree clique, and in the case of ISAM2Clique also store the remaining factor - myData.bayesTreeNode->setEliminationResult(eliminationResult); + // Check for Bayes tree orphan subtrees, and add them to our children + BOOST_FOREACH(const sharedFactor& f, node->factors) { + if (const BayesTreeOrphanWrapper* asSubtree = + dynamic_cast*>(f.get())) { + myData.bayesTreeNode->children.push_back(asSubtree->clique); + asSubtree->clique->parent_ = myData.bayesTreeNode; + } + } - // Fill nodes index - we do this here instead of calling insertRoot at the end to avoid - // putting orphan subtrees in the index - they'll already be in the index of the ISAM2 - // object they're added to. - BOOST_FOREACH(const Key& j, myData.bayesTreeNode->conditional()->frontals()) - nodesIndex.insert(std::make_pair(j, myData.bayesTreeNode)); + // Do dense elimination step + std::pair, + boost::shared_ptr > eliminationResult = + eliminationFunction(gatheredFactors, node->orderedFrontalKeys); - // Store remaining factor in parent's gathered factors - if (!eliminationResult.second->empty()) - myData.parentData->childFactors[myData.myIndexInParent] = - eliminationResult.second; - } + // Store conditional in BayesTree clique, and in the case of ISAM2Clique also store the remaining factor + myData.bayesTreeNode->setEliminationResult(eliminationResult); + + // Fill nodes index - we do this here instead of calling insertRoot at the end to avoid + // putting orphan subtrees in the index - they'll already be in the index of the ISAM2 + // object they're added to. + BOOST_FOREACH(const Key& j, myData.bayesTreeNode->conditional()->frontals()) + nodesIndex.insert(std::make_pair(j, myData.bayesTreeNode)); + + // Store remaining factor in parent's gathered factors + if (!eliminationResult.second->empty()) + myData.parentData->childFactors[myData.myIndexInParent] = + eliminationResult.second; + } + }; }; -} /* ************************************************************************* */ template @@ -150,7 +147,7 @@ void ClusterTree::Cluster::mergeChildren( orderedFrontalKeys.reserve(nrKeys); factors.reserve(nrFactors); typename Node::Children newChildren; - // newChildren.reserve(nrNewChildren); + newChildren.reserve(nrNewChildren); i = 0; BOOST_FOREACH(const sharedNode& child, children) { if (merge[i]) { @@ -204,12 +201,14 @@ std::pair, boost::shared_ptr > ClusterTree< // that contains all of the roots as its children. rootsContainer also stores the remaining // uneliminated factors passed up from the roots. boost::shared_ptr result = boost::make_shared(); - EliminationData rootsContainer(0, roots_.size()); - EliminationPostOrderVisitor visitorPost(function, result->nodes_); + typedef EliminationData Data; + Data rootsContainer(0, roots_.size()); + typename Data::EliminationPostOrderVisitor visitorPost(function, + result->nodes_); { TbbOpenMPMixedScope threadLimiter; // Limits OpenMP threads since we're mixing TBB and OpenMP treeTraversal::DepthFirstForestParallel(*this, rootsContainer, - eliminationPreOrderVisitor, visitorPost, 10); + Data::EliminationPreOrderVisitor, visitorPost, 10); } // Create BayesTree from roots stored in the dummy BayesTree node. @@ -218,18 +217,17 @@ std::pair, boost::shared_ptr > ClusterTree< rootsContainer.bayesTreeNode->children.end()); // Add remaining factors that were not involved with eliminated variables - boost::shared_ptr allRemainingFactors = boost::make_shared< + boost::shared_ptr remaining = boost::make_shared< FactorGraphType>(); - allRemainingFactors->reserve( + remaining->reserve( remainingFactors_.size() + rootsContainer.childFactors.size()); - allRemainingFactors->push_back(remainingFactors_.begin(), - remainingFactors_.end()); - BOOST_FOREACH(const sharedFactor& factor, rootsContainer.childFactors) + remaining->push_back(remainingFactors_.begin(), remainingFactors_.end()); + BOOST_FOREACH(const sharedFactor& factor, rootsContainer.childFactors) { if (factor) - allRemainingFactors->push_back(factor); - + remaining->push_back(factor); + } // Return result - return std::make_pair(result, allRemainingFactors); + return std::make_pair(result, remaining); } } From 5237c7492834d2949613759d19f03bed3d3b9a90 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 21 Jun 2015 14:36:23 -0700 Subject: [PATCH 34/46] Strengthened test and now checks problemSize_ --- tests/testGaussianJunctionTreeB.cpp | 66 +++++++++++++++++------------ 1 file changed, 39 insertions(+), 27 deletions(-) diff --git a/tests/testGaussianJunctionTreeB.cpp b/tests/testGaussianJunctionTreeB.cpp index c5401512b..4fa7fc761 100644 --- a/tests/testGaussianJunctionTreeB.cpp +++ b/tests/testGaussianJunctionTreeB.cpp @@ -64,35 +64,39 @@ using symbol_shorthand::L; /* ************************************************************************* * Bayes tree for smoother with "nested dissection" ordering: - C1 x5 x6 x4 - C2 x3 x2 : x4 - C3 x1 : x2 - C4 x7 : x6 -*/ -TEST( GaussianJunctionTreeB, constructor2 ) -{ + C1 x5 x6 x4 + C2 x3 x2 : x4 + C3 x1 : x2 + C4 x7 : x6 + */ +TEST( GaussianJunctionTreeB, constructor2 ) { // create a graph NonlinearFactorGraph nlfg; Values values; - boost::tie(nlfg,values) = createNonlinearSmoother(7); + boost::tie(nlfg, values) = createNonlinearSmoother(7); SymbolicFactorGraph::shared_ptr symbolic = nlfg.symbolic(); // linearize GaussianFactorGraph::shared_ptr fg = nlfg.linearize(values); - Ordering ordering; ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4); + Ordering ordering; + ordering += X(1), X(3), X(5), X(7), X(2), X(6), X(4); // create an ordering GaussianEliminationTree etree(*fg, ordering); - SymbolicEliminationTree stree(*symbolic,ordering); + SymbolicEliminationTree stree(*symbolic, ordering); GTSAM_PRINT(stree); GaussianJunctionTree actual(etree); GTSAM_PRINT(actual); - Ordering o324; o324 += X(3), X(2), X(4); - Ordering o56; o56 += X(5), X(6); - Ordering o7; o7 += X(7); - Ordering o1; o1 += X(1); + Ordering o324; + o324 += X(3), X(2), X(4); + Ordering o56; + o56 += X(5), X(6); + Ordering o7; + o7 += X(7); + Ordering o1; + o1 += X(1); // Can no longer test these: // Ordering sep1; @@ -102,26 +106,31 @@ TEST( GaussianJunctionTreeB, constructor2 ) GaussianJunctionTree::sharedNode x324 = actual.roots().front(); LONGS_EQUAL(2, x324->children.size()); -#if defined(__APPLE__) // tie-breaking seems different :-( GaussianJunctionTree::sharedNode x1 = x324->children[0]; GaussianJunctionTree::sharedNode x56 = x324->children[1]; -#else - GaussianJunctionTree::sharedNode x1 = x324->children[1]; - GaussianJunctionTree::sharedNode x56 = x324->children[0]; -#endif + if (x1->children.size() > 0) + x1.swap(x56); // makes it work with different tie-breakers + LONGS_EQUAL(0, x1->children.size()); LONGS_EQUAL(1, x56->children.size()); GaussianJunctionTree::sharedNode x7 = x56->children[0]; LONGS_EQUAL(0, x7->children.size()); EXPECT(assert_equal(o324, x324->orderedFrontalKeys)); - EXPECT_LONGS_EQUAL (5, x324->factors.size()); - EXPECT(assert_equal(o56, x56->orderedFrontalKeys)); - EXPECT_LONGS_EQUAL (4, x56->factors.size()); - EXPECT(assert_equal(o7, x7->orderedFrontalKeys)); - EXPECT_LONGS_EQUAL (2, x7->factors.size()); - EXPECT(assert_equal(o1, x1->orderedFrontalKeys)); - EXPECT_LONGS_EQUAL (2, x1->factors.size()); + EXPECT_LONGS_EQUAL(5, x324->factors.size()); + EXPECT_LONGS_EQUAL(9, x324->problemSize_); + + EXPECT(assert_equal(o56, x56->orderedFrontalKeys)); + EXPECT_LONGS_EQUAL(4, x56->factors.size()); + EXPECT_LONGS_EQUAL(9, x56->problemSize_); + + EXPECT(assert_equal(o7, x7->orderedFrontalKeys)); + EXPECT_LONGS_EQUAL(2, x7->factors.size()); + EXPECT_LONGS_EQUAL(4, x7->problemSize_); + + EXPECT(assert_equal(o1, x1->orderedFrontalKeys)); + EXPECT_LONGS_EQUAL(2, x1->factors.size()); + EXPECT_LONGS_EQUAL(4, x1->problemSize_); } ///* ************************************************************************* */ @@ -266,6 +275,9 @@ TEST( GaussianJunctionTreeB, constructor2 ) //} /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ From b546a1f0a79374051b4873dd9d4631b77ec528ad Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 21 Jun 2015 14:39:56 -0700 Subject: [PATCH 35/46] Use front/back --- tests/testGaussianJunctionTreeB.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tests/testGaussianJunctionTreeB.cpp b/tests/testGaussianJunctionTreeB.cpp index 4fa7fc761..57890d876 100644 --- a/tests/testGaussianJunctionTreeB.cpp +++ b/tests/testGaussianJunctionTreeB.cpp @@ -106,8 +106,8 @@ TEST( GaussianJunctionTreeB, constructor2 ) { GaussianJunctionTree::sharedNode x324 = actual.roots().front(); LONGS_EQUAL(2, x324->children.size()); - GaussianJunctionTree::sharedNode x1 = x324->children[0]; - GaussianJunctionTree::sharedNode x56 = x324->children[1]; + GaussianJunctionTree::sharedNode x1 = x324->children.front(); + GaussianJunctionTree::sharedNode x56 = x324->children.back(); if (x1->children.size() > 0) x1.swap(x56); // makes it work with different tie-breakers From f9bf63b71c88f9aa131f0d49c2117e7330a94029 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 21 Jun 2015 14:55:30 -0700 Subject: [PATCH 36/46] Documentation and formatting --- gtsam/inference/ClusterTree-inst.h | 20 +++++++----- gtsam/nonlinear/NonlinearFactorGraph.cpp | 39 +++++++++++++----------- 2 files changed, 34 insertions(+), 25 deletions(-) diff --git a/gtsam/inference/ClusterTree-inst.h b/gtsam/inference/ClusterTree-inst.h index 2cf1f5c58..ed19ba4e0 100644 --- a/gtsam/inference/ClusterTree-inst.h +++ b/gtsam/inference/ClusterTree-inst.h @@ -63,14 +63,19 @@ struct EliminationData { // Elimination post-order visitor - combine the child factors with our own factors, add the // resulting conditional to the BayesTree, and add the remaining factor to the parent. - struct EliminationPostOrderVisitor { - const typename CLUSTERTREE::Eliminate& eliminationFunction; - typename CLUSTERTREE::BayesTreeType::Nodes& nodesIndex; + class EliminationPostOrderVisitor { + const typename CLUSTERTREE::Eliminate& eliminationFunction_; + typename CLUSTERTREE::BayesTreeType::Nodes& nodesIndex_; + + public: + // Construct functor EliminationPostOrderVisitor( const typename CLUSTERTREE::Eliminate& eliminationFunction, typename CLUSTERTREE::BayesTreeType::Nodes& nodesIndex) : - eliminationFunction(eliminationFunction), nodesIndex(nodesIndex) { + eliminationFunction_(eliminationFunction), nodesIndex_(nodesIndex) { } + + // Function that does the HEAVY lifting void operator()(const typename CLUSTERTREE::sharedNode& node, EliminationData& myData) { assert(node); @@ -90,10 +95,11 @@ struct EliminationData { } } - // Do dense elimination step + // >>>>>>>>>>>>>> Do dense elimination step >>>>>>>>>>>>>>>>>>>>>>>>>>>>> std::pair, boost::shared_ptr > eliminationResult = - eliminationFunction(gatheredFactors, node->orderedFrontalKeys); + eliminationFunction_(gatheredFactors, node->orderedFrontalKeys); + // <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< // Store conditional in BayesTree clique, and in the case of ISAM2Clique also store the remaining factor myData.bayesTreeNode->setEliminationResult(eliminationResult); @@ -102,7 +108,7 @@ struct EliminationData { // putting orphan subtrees in the index - they'll already be in the index of the ISAM2 // object they're added to. BOOST_FOREACH(const Key& j, myData.bayesTreeNode->conditional()->frontals()) - nodesIndex.insert(std::make_pair(j, myData.bayesTreeNode)); + nodesIndex_.insert(std::make_pair(j, myData.bayesTreeNode)); // Store remaining factor in parent's gathered factors if (!eliminationResult.second->empty()) diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp index 298ccf4b7..7dacb5bcb 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.cpp +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -276,23 +276,26 @@ SymbolicFactorGraph::shared_ptr NonlinearFactorGraph::symbolic() const namespace { #ifdef GTSAM_USE_TBB - struct _LinearizeOneFactor { - const NonlinearFactorGraph& graph; - const Values& linearizationPoint; - GaussianFactorGraph& result; - _LinearizeOneFactor(const NonlinearFactorGraph& graph, const Values& linearizationPoint, GaussianFactorGraph& result) : - graph(graph), linearizationPoint(linearizationPoint), result(result) {} - void operator()(const tbb::blocked_range& r) const - { - for(size_t i = r.begin(); i != r.end(); ++i) - { - if(graph[i]) - result[i] = graph[i]->linearize(linearizationPoint); - else - result[i] = GaussianFactor::shared_ptr(); - } +class _LinearizeOneFactor { + const NonlinearFactorGraph& nonlinearGraph_; + const Values& linearizationPoint_; + GaussianFactorGraph& result_; +public: + // Create functor with constant parameters + _LinearizeOneFactor(const NonlinearFactorGraph& graph, + const Values& linearizationPoint, GaussianFactorGraph& result) : + nonlinearGraph_(graph), linearizationPoint_(linearizationPoint), result_(result) { + } + // Operator that linearizes a given range of the factors + void operator()(const tbb::blocked_range& blocked_range) const { + for (size_t i = blocked_range.begin(); i != blocked_range.end(); ++i) { + if (nonlinearGraph_[i]) + result_[i] = nonlinearGraph_[i]->linearize(linearizationPoint_); + else + result_[i] = GaussianFactor::shared_ptr(); } - }; + } +}; #endif } @@ -319,9 +322,9 @@ GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize(const Values& li // linearize all factors BOOST_FOREACH(const sharedFactor& factor, this->factors_) { if(factor) { - (*linearFG) += factor->linearize(linearizationPoint); + (*linearFG) += factor->linearize(linearizationPoint_); } else - (*linearFG) += GaussianFactor::shared_ptr(); + (*linearFG) += GaussianFactor::shared_ptr(); } #endif From 2b9e9ae9787921ac4c8bd93597d2a515ab6e3584 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 21 Jun 2015 18:20:12 -0700 Subject: [PATCH 37/46] Turned off AutoDiff by default --- timing/timeSFMBAL.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/timing/timeSFMBAL.cpp b/timing/timeSFMBAL.cpp index 218bfe18e..68b3c4932 100644 --- a/timing/timeSFMBAL.cpp +++ b/timing/timeSFMBAL.cpp @@ -34,7 +34,7 @@ using namespace std; using namespace gtsam; -//#define USE_GTSAM_FACTOR +#define USE_GTSAM_FACTOR #ifdef USE_GTSAM_FACTOR #include #include From c30bd3e654d427a3ff3b85813617d8db9135ab98 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 21 Jun 2015 20:09:38 -0700 Subject: [PATCH 38/46] Comments --- gtsam/inference/ClusterTree-inst.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/inference/ClusterTree-inst.h b/gtsam/inference/ClusterTree-inst.h index ed19ba4e0..a71ccebf9 100644 --- a/gtsam/inference/ClusterTree-inst.h +++ b/gtsam/inference/ClusterTree-inst.h @@ -19,9 +19,9 @@ namespace gtsam { /* ************************************************************************* */ -// Elimination traversal data - stores a pointer to the parent data and collects the factors -// resulting from elimination of the children. Also sets up BayesTree cliques with parent and -// child pointers. +// Elimination traversal data - stores a pointer to the parent data and collects +// the factors resulting from elimination of the children. Also sets up BayesTree +// cliques with parent and child pointers. template struct EliminationData { // Typedefs From a3ed636fce680aec31af21900703c9917e6e2a29 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 21 Jun 2015 21:09:31 -0700 Subject: [PATCH 39/46] Use symbols --- gtsam/linear/tests/testScatter.cpp | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/gtsam/linear/tests/testScatter.cpp b/gtsam/linear/tests/testScatter.cpp index 19d099616..465763282 100644 --- a/gtsam/linear/tests/testScatter.cpp +++ b/gtsam/linear/tests/testScatter.cpp @@ -17,10 +17,12 @@ #include #include +#include #include using namespace std; using namespace gtsam; +using symbol_shorthand::X; /* ************************************************************************* */ TEST(HessianFactor, CombineAndEliminate) { @@ -43,14 +45,14 @@ TEST(HessianFactor, CombineAndEliminate) { Vector3 s2(3.6, 3.6, 3.6); GaussianFactorGraph gfg; - gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true)); - gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true)); - gfg.add(1, A21, b2, noiseModel::Diagonal::Sigmas(s2, true)); + gfg.add(X(1), A01, b0, noiseModel::Diagonal::Sigmas(s0, true)); + gfg.add(X(0), A10, X(1), A11, b1, noiseModel::Diagonal::Sigmas(s1, true)); + gfg.add(X(1), A21, b2, noiseModel::Diagonal::Sigmas(s2, true)); Scatter scatter(gfg); EXPECT_LONGS_EQUAL(2, scatter.size()); - EXPECT_LONGS_EQUAL(0, scatter.at(0).slot); - EXPECT_LONGS_EQUAL(1, scatter.at(1).slot); + EXPECT(assert_equal(X(1), scatter.at(0).key)); + EXPECT(assert_equal(X(0), scatter.at(1).key)); EXPECT_LONGS_EQUAL(n, scatter.at(0).dimension); EXPECT_LONGS_EQUAL(n, scatter.at(1).dimension); } From a28ebc5461a6781e0b37d796ee68468a386b0452 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 21 Jun 2015 22:41:12 -0700 Subject: [PATCH 40/46] Switched to vector --- gtsam/linear/HessianFactor.cpp | 13 +++--- gtsam/linear/Scatter.cpp | 74 +++++++++++++++++++--------------- gtsam/linear/Scatter.h | 20 ++++----- 3 files changed, 61 insertions(+), 46 deletions(-) diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index f3e54cffb..2b275b60f 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -237,11 +237,14 @@ HessianFactor::HessianFactor(const GaussianFactorGraph& factors, // Allocate and copy keys gttic(allocate); // Allocate with dimensions for each variable plus 1 at the end for the information vector - keys_.resize(scatter->size()); - FastVector dims(scatter->size() + 1); - BOOST_FOREACH(const Scatter::value_type& key_slotentry, *scatter) { - keys_[key_slotentry.second.slot] = key_slotentry.first; - dims[key_slotentry.second.slot] = key_slotentry.second.dimension; + const size_t n = scatter->size(); + keys_.resize(n); + FastVector dims(n + 1); + DenseIndex slot = 0; + BOOST_FOREACH(const SlotEntry& slotentry, *scatter) { + keys_[slot] = slotentry.key; + dims[slot] = slotentry.dimension; + ++slot; } dims.back() = 1; info_ = SymmetricBlockMatrix(dims); diff --git a/gtsam/linear/Scatter.cpp b/gtsam/linear/Scatter.cpp index 942b42160..dde83712a 100644 --- a/gtsam/linear/Scatter.cpp +++ b/gtsam/linear/Scatter.cpp @@ -21,6 +21,7 @@ #include #include +#include using namespace std; @@ -29,53 +30,62 @@ namespace gtsam { /* ************************************************************************* */ string SlotEntry::toString() const { ostringstream oss; - oss << "SlotEntry: slot=" << slot << ", dim=" << dimension; + oss << "SlotEntry: key=" << key << ", dim=" << dimension; return oss.str(); } /* ************************************************************************* */ Scatter::Scatter(const GaussianFactorGraph& gfg, - boost::optional ordering) { + boost::optional ordering) { gttic(Scatter_Constructor); - static const DenseIndex none = std::numeric_limits::max(); - - // First do the set union. - BOOST_FOREACH (const GaussianFactor::shared_ptr& factor, gfg) { - if (factor) { - for (GaussianFactor::const_iterator variable = factor->begin(); - variable != factor->end(); ++variable) { - // TODO: Fix this hack to cope with zero-row Jacobians that come from - // BayesTreeOrphanWrappers - const JacobianFactor* asJacobian = - dynamic_cast(factor.get()); - if (!asJacobian || asJacobian->cols() > 1) - insert( - make_pair(*variable, SlotEntry(none, factor->getDim(variable)))); - } - } - } // If we have an ordering, pre-fill the ordered variables first - size_t slot = 0; if (ordering) { BOOST_FOREACH (Key key, *ordering) { - const_iterator entry = find(key); - if (entry == end()) - throw std::invalid_argument( - "The ordering provided to the HessianFactor Scatter constructor\n" - "contained extra variables that did not appear in the factors to " - "combine."); - at(key).slot = (slot++); + push_back(SlotEntry(key, 0)); } } - // Next fill in the slot indices (we can only get these after doing the set - // union. - BOOST_FOREACH (value_type& var_slot, *this) { - if (var_slot.second.slot == none) var_slot.second.slot = (slot++); + iterator first = end(); // remember position + + // Now, find dimensions of variables and/or extend + BOOST_FOREACH (const GaussianFactor::shared_ptr& factor, gfg) { + if (!factor) continue; + + // TODO: Fix this hack to cope with zero-row Jacobians that come from BayesTreeOrphanWrappers + const JacobianFactor* asJacobian = dynamic_cast(factor.get()); + if (asJacobian && asJacobian->cols() <= 1) continue; + + // loop over variables + for (GaussianFactor::const_iterator variable = factor->begin(); + variable != factor->end(); ++variable) { + const Key key = *variable; + iterator it = find(key); // theoretically expensive, yet cache friendly + if (it!=end()) + it->dimension = factor->getDim(variable); + else + push_back(SlotEntry(key, factor->getDim(variable))); + } } + + // Filter out keys with zero dimensions (if ordering had more keys) + erase(std::remove_if(begin(), end(), SlotEntry::Zero), end()); + + // To keep the same behavior as before, sort the keys after the ordering + if (first != end()) std::sort(begin(), end()); +} + +/* ************************************************************************* */ +FastVector::iterator Scatter::find(Key key) { + iterator it = begin(); + while(it != end()) { + if (it->key == key) + return it; + ++it; + } + return it; // end() } /* ************************************************************************* */ -} // gtsam +} // gtsam diff --git a/gtsam/linear/Scatter.h b/gtsam/linear/Scatter.h index 7a37ba1e0..39bfa6b8d 100644 --- a/gtsam/linear/Scatter.h +++ b/gtsam/linear/Scatter.h @@ -32,30 +32,32 @@ class Ordering; /// One SlotEntry stores the slot index for a variable, as well its dim. struct GTSAM_EXPORT SlotEntry { - DenseIndex slot; + Key key; size_t dimension; - SlotEntry(DenseIndex _slot, size_t _dimension) - : slot(_slot), dimension(_dimension) {} + SlotEntry(Key _key, size_t _dimension) : key(_key), dimension(_dimension) {} std::string toString() const; + friend bool operator<(const SlotEntry& p, const SlotEntry& q) { + return p.key < q.key; + } + static bool Zero(const SlotEntry& p) { return p.dimension==0;} }; /** * Scatter is an intermediate data structure used when building a HessianFactor - * incrementally, to get the keys in the right order. The "scatter" is a map + * incrementally, to get the keys in the right order. In spirit, it is a map * from global variable indices to slot indices in the union of involved * variables. We also include the dimensionality of the variable. */ -class Scatter : public FastMap { +class Scatter : public FastVector { public: /// Constructor Scatter(const GaussianFactorGraph& gfg, boost::optional ordering = boost::none); - /// Get the slot corresponding to the given key - DenseIndex slot(Key key) const { return at(key).slot; } + private: - /// Get the dimension corresponding to the given key - DenseIndex dim(Key key) const { return at(key).dimension; } + /// Find the SlotEntry with the right key (linear time worst case) + iterator find(Key key); }; } // \ namespace gtsam From 41d4c4893886eadce687a18ac3355791ef24b2ef Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 21 Jun 2015 22:48:36 -0700 Subject: [PATCH 41/46] Checked scatter --- gtsam/linear/tests/testGaussianBayesTree.cpp | 24 ++++++++++++++++++-- 1 file changed, 22 insertions(+), 2 deletions(-) diff --git a/gtsam/linear/tests/testGaussianBayesTree.cpp b/gtsam/linear/tests/testGaussianBayesTree.cpp index 05f8c3d2a..1beac1994 100644 --- a/gtsam/linear/tests/testGaussianBayesTree.cpp +++ b/gtsam/linear/tests/testGaussianBayesTree.cpp @@ -83,13 +83,33 @@ TEST( GaussianBayesTree, eliminate ) { GaussianBayesTree bt = *chain.eliminateMultifrontal(chainOrdering); + Scatter scatter(chain); + BOOST_FOREACH(const Scatter::value_type& entry, scatter) + cout << entry.first << " " << entry.second.toString() << endl; + + EXPECT_LONGS_EQUAL(4, scatter.size()); + EXPECT_LONGS_EQUAL(0, scatter.at(x1).slot); + EXPECT_LONGS_EQUAL(1, scatter.at(x2).slot); + EXPECT_LONGS_EQUAL(2, scatter.at(x3).slot); + EXPECT_LONGS_EQUAL(3, scatter.at(x4).slot); + Matrix two = (Matrix(1, 1) << 2.).finished(); Matrix one = (Matrix(1, 1) << 1.).finished(); GaussianBayesTree bayesTree_expected; bayesTree_expected.insertRoot( - MakeClique(GaussianConditional(pair_list_of(x3, (Matrix(2, 1) << 2., 0.).finished()) (x4, (Matrix(2, 1) << 2., 2.).finished()), 2, Vector2(2., 2.)), list_of - (MakeClique(GaussianConditional(pair_list_of(x2, (Matrix(2, 1) << -2.*sqrt(2.), 0.).finished()) (x1, (Matrix(2, 1) << -sqrt(2.), -sqrt(2.)).finished()) (x3, (Matrix(2, 1) << -sqrt(2.), sqrt(2.)).finished()), 2, (Vector(2) << -2.*sqrt(2.), 0.).finished()))))); + MakeClique( + GaussianConditional( + pair_list_of(x3, (Matrix21() << 2., 0.).finished())( + x4, (Matrix21() << 2., 2.).finished()), 2, Vector2(2., 2.)), + list_of( + MakeClique( + GaussianConditional( + pair_list_of(x2, + (Matrix21() << -2. * sqrt(2.), 0.).finished())(x1, + (Matrix21() << -sqrt(2.), -sqrt(2.)).finished())(x3, + (Matrix21() << -sqrt(2.), sqrt(2.)).finished()), 2, + (Vector(2) << -2. * sqrt(2.), 0.).finished()))))); EXPECT(assert_equal(bayesTree_expected, bt)); } From 0d2558cbb5c59bf7643082de0fb54dcf82d2982e Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 21 Jun 2015 23:33:59 -0700 Subject: [PATCH 42/46] Removed print --- gtsam/linear/tests/testGaussianBayesTree.cpp | 11 ++++------- 1 file changed, 4 insertions(+), 7 deletions(-) diff --git a/gtsam/linear/tests/testGaussianBayesTree.cpp b/gtsam/linear/tests/testGaussianBayesTree.cpp index 1beac1994..4f7c5c335 100644 --- a/gtsam/linear/tests/testGaussianBayesTree.cpp +++ b/gtsam/linear/tests/testGaussianBayesTree.cpp @@ -84,14 +84,11 @@ TEST( GaussianBayesTree, eliminate ) GaussianBayesTree bt = *chain.eliminateMultifrontal(chainOrdering); Scatter scatter(chain); - BOOST_FOREACH(const Scatter::value_type& entry, scatter) - cout << entry.first << " " << entry.second.toString() << endl; - EXPECT_LONGS_EQUAL(4, scatter.size()); - EXPECT_LONGS_EQUAL(0, scatter.at(x1).slot); - EXPECT_LONGS_EQUAL(1, scatter.at(x2).slot); - EXPECT_LONGS_EQUAL(2, scatter.at(x3).slot); - EXPECT_LONGS_EQUAL(3, scatter.at(x4).slot); + EXPECT_LONGS_EQUAL(1, scatter.at(0).key); + EXPECT_LONGS_EQUAL(2, scatter.at(1).key); + EXPECT_LONGS_EQUAL(3, scatter.at(2).key); + EXPECT_LONGS_EQUAL(4, scatter.at(3).key); Matrix two = (Matrix(1, 1) << 2.).finished(); Matrix one = (Matrix(1, 1) << 1.).finished(); From c33bb7a89a43979ca06546aaee7e1ca55006c0ea Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 21 Jun 2015 23:34:11 -0700 Subject: [PATCH 43/46] Solved Scatter bug --- gtsam/linear/Scatter.cpp | 10 +++++----- gtsam/linear/tests/testScatter.cpp | 4 ++-- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/gtsam/linear/Scatter.cpp b/gtsam/linear/Scatter.cpp index dde83712a..ed2af529f 100644 --- a/gtsam/linear/Scatter.cpp +++ b/gtsam/linear/Scatter.cpp @@ -46,8 +46,6 @@ Scatter::Scatter(const GaussianFactorGraph& gfg, } } - iterator first = end(); // remember position - // Now, find dimensions of variables and/or extend BOOST_FOREACH (const GaussianFactor::shared_ptr& factor, gfg) { if (!factor) continue; @@ -68,11 +66,13 @@ Scatter::Scatter(const GaussianFactorGraph& gfg, } } + // To keep the same behavior as before, sort the keys after the ordering + 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()); - - // To keep the same behavior as before, sort the keys after the ordering - if (first != end()) std::sort(begin(), end()); } /* ************************************************************************* */ diff --git a/gtsam/linear/tests/testScatter.cpp b/gtsam/linear/tests/testScatter.cpp index 465763282..575f29c26 100644 --- a/gtsam/linear/tests/testScatter.cpp +++ b/gtsam/linear/tests/testScatter.cpp @@ -51,8 +51,8 @@ TEST(HessianFactor, CombineAndEliminate) { Scatter scatter(gfg); EXPECT_LONGS_EQUAL(2, scatter.size()); - EXPECT(assert_equal(X(1), scatter.at(0).key)); - EXPECT(assert_equal(X(0), scatter.at(1).key)); + EXPECT(assert_equal(X(0), scatter.at(0).key)); + EXPECT(assert_equal(X(1), scatter.at(1).key)); EXPECT_LONGS_EQUAL(n, scatter.at(0).dimension); EXPECT_LONGS_EQUAL(n, scatter.at(1).dimension); } From 304f12b106c733f88df3226e98ed75c5e0ba5f54 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 22 Jun 2015 00:33:35 -0700 Subject: [PATCH 44/46] Small fixes for TBB/Timing path --- gtsam/inference/ClusterTree-inst.h | 2 +- gtsam/nonlinear/NonlinearFactorGraph.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/inference/ClusterTree-inst.h b/gtsam/inference/ClusterTree-inst.h index a71ccebf9..6b28f2dbf 100644 --- a/gtsam/inference/ClusterTree-inst.h +++ b/gtsam/inference/ClusterTree-inst.h @@ -130,7 +130,7 @@ void ClusterTree::Cluster::print(const std::string& s, template void ClusterTree::Cluster::mergeChildren( const std::vector& merge) { - gttic(Cluster::mergeChildren); + gttic(Cluster_mergeChildren); // Count how many keys, factors and children we'll end up with size_t nrKeys = orderedFrontalKeys.size(); diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp index 291907a73..fbc7190de 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.cpp +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -324,7 +324,7 @@ GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize(const Values& li // linearize all factors BOOST_FOREACH(const sharedFactor& factor, this->factors_) { if(factor) { - (*linearFG) += factor->linearize(linearizationPoint_); + (*linearFG) += factor->linearize(linearizationPoint); } else (*linearFG) += GaussianFactor::shared_ptr(); } From 48c3f5813b6dd4968e2a8f2d498d2b0ef29fab47 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 22 Jun 2015 00:35:21 -0700 Subject: [PATCH 45/46] removed printing --- tests/testGaussianJunctionTreeB.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/tests/testGaussianJunctionTreeB.cpp b/tests/testGaussianJunctionTreeB.cpp index 57890d876..98adfc1dc 100644 --- a/tests/testGaussianJunctionTreeB.cpp +++ b/tests/testGaussianJunctionTreeB.cpp @@ -85,9 +85,7 @@ TEST( GaussianJunctionTreeB, constructor2 ) { // create an ordering GaussianEliminationTree etree(*fg, ordering); SymbolicEliminationTree stree(*symbolic, ordering); - GTSAM_PRINT(stree); GaussianJunctionTree actual(etree); - GTSAM_PRINT(actual); Ordering o324; o324 += X(3), X(2), X(4); From ff7393b9dac53affb0c9c42beede42ad3135ef43 Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 22 Jun 2015 13:34:49 -0700 Subject: [PATCH 46/46] Reverted to develop version --- .cproject | 3146 ++++++++++++++++++++++++++--------------------------- 1 file changed, 1557 insertions(+), 1589 deletions(-) diff --git a/.cproject b/.cproject index 866092c95..ac9b166ec 100644 --- a/.cproject +++ b/.cproject @@ -5,13 +5,13 @@ + + - - @@ -60,13 +60,13 @@ + + - - @@ -116,13 +116,13 @@ + + - - @@ -484,6 +484,341 @@ + + make + -j5 + testCombinedImuFactor.run + true + true + true + + + make + -j5 + testImuFactor.run + true + true + true + + + make + -j5 + testAHRSFactor.run + true + true + true + + + make + -j8 + testAttitudeFactor.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + testNonlinearConstraint.run + true + true + true + + + make + -j2 + testLieConfig.run + true + true + true + + + make + -j2 + testConstraintOptimizer.run + true + true + true + + + make + -j2 + testGaussianFactor.run + true + true + true + + + make + -j4 + testImuFactor.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + testBTree.run + true + true + true + + + make + -j2 + testDSF.run + true + true + true + + + make + -j2 + testDSFVector.run + true + true + true + + + make + -j2 + testMatrix.run + true + true + true + + + make + -j2 + testSPQRUtil.run + true + true + true + + + make + -j2 + testVector.run + true + true + true + + + make + -j2 + timeMatrix.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j5 + wrap + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + install + true + true + true + + + make + -j2 + all + true + true + true + + + cmake + .. + true + false + true + + + make + -j2 + check + true + true + true + + + make + -j2 + tests/testLieConfig.run + true + true + true + + + make + -j4 + testSimilarity3.run + true + true + true + + + make + -j5 + testInvDepthCamera3.run + true + true + true + + + make + -j5 + testTriangulation.run + true + true + true + + + make + -j4 + testEvent.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j5 + all + true + false + true + + + make + -j5 + check + true + false + true + make -j5 @@ -532,39 +867,183 @@ true true - - make - -j4 - testSimilarity3.run - true - true - true - - + make -j5 - testInvDepthCamera3.run + testCal3Bundler.run true true true - + make -j5 - testTriangulation.run + testCal3DS2.run true true true - + + make + -j5 + testCalibratedCamera.run + true + true + true + + + make + -j5 + testEssentialMatrix.run + true + true + true + + + make + -j1 VERBOSE=1 + testHomography2.run + true + false + true + + + make + -j5 + testPinholeCamera.run + true + true + true + + + make + -j5 + testPoint2.run + true + true + true + + + make + -j5 + testPoint3.run + true + true + true + + + make + -j5 + testPose2.run + true + true + true + + + make + -j5 + testPose3.run + true + true + true + + + make + -j5 + testRot3M.run + true + true + true + + + make + -j5 + testSphere2.run + true + true + true + + + make + -j5 + testStereoCamera.run + true + true + true + + + make + -j5 + testCal3Unified.run + true + true + true + + + make + -j5 + testRot2.run + true + true + true + + + make + -j5 + testRot3Q.run + true + true + true + + + make + -j5 + testRot3.run + true + true + true + + make -j4 - testEvent.run + testSO3.run true true true - + + make + -j4 + testQuaternion.run + true + true + true + + + make + -j4 + testOrientedPlane3.run + true + true + true + + + make + -j4 + testPinholePose.run + true + true + true + + + make + -j4 + testCyclic.run + true + true + true + + make -j2 all @@ -572,7 +1051,7 @@ true true - + make -j2 clean @@ -580,143 +1059,23 @@ true true - - make - -k - check - true - false - true - - - make - - tests/testBayesTree.run - true - false - true - - - make - - testBinaryBayesNet.run - true - false - true - - + make -j2 - testFactorGraph.run + clean all true true true - + make -j2 - testISAM.run + install true true true - - make - -j2 - testJunctionTree.run - true - true - true - - - make - -j2 - testKey.run - true - true - true - - - make - -j2 - testOrdering.run - true - true - true - - - make - - testSymbolicBayesNet.run - true - false - true - - - make - - tests/testSymbolicFactor.run - true - false - true - - - make - - testSymbolicFactorGraph.run - true - false - true - - - make - -j2 - timeSymbolMaps.run - true - true - true - - - make - - tests/testBayesTree - true - false - true - - - make - -j2 - tests/testPose2.run - true - true - true - - - make - -j2 - tests/testPose3.run - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - check - true - true - true - - + make -j2 clean @@ -868,518 +1227,6 @@ true true - - make - -j5 - testGaussianFactorGraphUnordered.run - true - true - true - - - make - -j5 - testGaussianBayesNetUnordered.run - true - true - true - - - make - -j5 - testGaussianConditional.run - true - true - true - - - make - -j5 - testGaussianDensity.run - true - true - true - - - make - -j5 - testGaussianJunctionTree.run - true - true - true - - - make - -j5 - testHessianFactor.run - true - true - true - - - make - -j5 - testJacobianFactor.run - true - true - true - - - make - -j5 - testKalmanFilter.run - true - true - true - - - make - -j5 - testNoiseModel.run - true - true - true - - - make - -j5 - testSampler.run - true - true - true - - - make - -j5 - testSerializationLinear.run - true - true - true - - - make - -j5 - testVectorValues.run - true - true - true - - - make - -j5 - testGaussianBayesTree.run - true - true - true - - - make - -j5 - testCombinedImuFactor.run - true - true - true - - - make - -j5 - testImuFactor.run - true - true - true - - - make - -j5 - testAHRSFactor.run - true - true - true - - - make - -j8 - testAttitudeFactor.run - true - true - true - - - make - -j5 - clean - true - true - true - - - make - -j5 - all - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - testGaussianConditional.run - true - true - true - - - make - -j2 - testGaussianFactor.run - true - true - true - - - make - -j2 - timeGaussianFactor.run - true - true - true - - - make - -j2 - timeVectorConfig.run - true - true - true - - - make - -j2 - testVectorBTree.run - true - true - true - - - make - -j2 - testVectorMap.run - true - true - true - - - make - -j2 - testNoiseModel.run - true - true - true - - - make - -j2 - testBayesNetPreconditioner.run - true - true - true - - - make - - testErrors.run - true - false - true - - - make - -j2 - check - true - true - true - - - make - -j2 - tests/testGaussianJunctionTree.run - true - true - true - - - make - -j2 - tests/testGaussianFactor.run - true - true - true - - - make - -j2 - tests/testGaussianConditional.run - true - true - true - - - make - -j2 - tests/timeSLAMlike.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - testBTree.run - true - true - true - - - make - -j2 - testDSF.run - true - true - true - - - make - -j2 - testDSFVector.run - true - true - true - - - make - -j2 - testMatrix.run - true - true - true - - - make - -j2 - testSPQRUtil.run - true - true - true - - - make - -j2 - testVector.run - true - true - true - - - make - -j2 - timeMatrix.run - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - testClusterTree.run - true - true - true - - - make - -j2 - testJunctionTree.run - true - true - true - - - make - -j2 - tests/testEliminationTree.run - true - true - true - - - make - -j2 - tests/testSymbolicFactor.run - true - true - true - - - make - -j2 - tests/testVariableSlots.run - true - true - true - - - make - -j2 - tests/testConditional.run - true - true - true - - - make - -j2 - tests/testSymbolicFactorGraph.run - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - testNonlinearConstraint.run - true - true - true - - - make - -j2 - testLieConfig.run - true - true - true - - - make - -j2 - testConstraintOptimizer.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j5 - testBTree.run - true - true - true - - - make - -j5 - testDSF.run - true - true - true - - - make - -j5 - testDSFMap.run - true - true - true - - - make - -j5 - testDSFVector.run - true - true - true - - - make - -j5 - testFixedVector.run - true - true - true - make -j2 @@ -1521,55 +1368,23 @@ true true - + make - -j5 - testEliminationTree.run + -j2 + all true true true - + make - -j5 - testInference.run + -j2 + clean true true true - - make - -j5 - testKey.run - true - true - true - - - make - -j1 - testSymbolicBayesTree.run - true - false - true - - - make - -j1 - testSymbolicSequentialSolver.run - true - false - true - - - make - -j4 - testLabeledSymbol.run - true - true - true - - + make -j2 check @@ -1577,10 +1392,178 @@ true true - + make -j2 - tests/testLieConfig.run + testGaussianConditional.run + true + true + true + + + make + -j2 + testGaussianFactor.run + true + true + true + + + make + -j2 + timeGaussianFactor.run + true + true + true + + + make + -j2 + timeVectorConfig.run + true + true + true + + + make + -j2 + testVectorBTree.run + true + true + true + + + make + -j2 + testVectorMap.run + true + true + true + + + make + -j2 + testNoiseModel.run + true + true + true + + + make + -j2 + testBayesNetPreconditioner.run + true + true + true + + + make + + testErrors.run + true + false + true + + + make + -j5 + testAntiFactor.run + true + true + true + + + make + -j5 + testPriorFactor.run + true + true + true + + + make + -j5 + testDataset.run + true + true + true + + + make + -j5 + testEssentialMatrixFactor.run + true + true + true + + + make + -j5 + testGeneralSFMFactor_Cal3Bundler.run + true + true + true + + + make + -j5 + testGeneralSFMFactor.run + true + true + true + + + make + -j5 + testProjectionFactor.run + true + true + true + + + make + -j5 + testRotateFactor.run + true + true + true + + + make + -j5 + testPoseRotationPrior.run + true + true + true + + + make + -j5 + testImplicitSchurFactor.run + true + true + true + + + make + -j4 + testRangeFactor.run + true + true + true + + + make + -j4 + testOrientedPlane3Factor.run + true + true + true + + + make + -j4 + testSmartProjectionPoseFactor.run true true true @@ -1992,7 +1975,7 @@ false true - + make -j2 check @@ -2000,38 +1983,54 @@ true true - + make - -j2 - clean - true - true - true - - - make - -j2 - install - true - true - true - - - make - -j2 - all - true - true - true - - - cmake - .. + tests/testGaussianISAM2 true false true - + + make + -j2 + check + true + true + true + + + make + -j2 + tests/testGaussianJunctionTree.run + true + true + true + + + make + -j2 + tests/testGaussianFactor.run + true + true + true + + + make + -j2 + tests/testGaussianConditional.run + true + true + true + + + make + -j2 + tests/timeSLAMlike.run + true + true + true + + make -j2 testGaussianFactor.run @@ -2039,191 +2038,503 @@ true true - + make -j5 - testCal3Bundler.run + testParticleFactor.run true true true - + make -j5 - testCal3DS2.run + schedulingExample.run true true true - + make -j5 - testCalibratedCamera.run + schedulingQuals12.run true true true - + make -j5 - testEssentialMatrix.run + schedulingQuals13.run true true true - + make - -j1 VERBOSE=1 - testHomography2.run + -j5 + testCSP.run + true + true + true + + + make + -j5 + testScheduler.run + true + true + true + + + make + -j5 + testSudoku.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -k + check true false true - + make - -j5 - testPinholeCamera.run + + tests/testBayesTree.run + true + false + true + + + make + + testBinaryBayesNet.run + true + false + true + + + make + -j2 + testFactorGraph.run true true true - + make - -j5 - testPoint2.run + -j2 + testISAM.run true true true - + make - -j5 - testPoint3.run + -j2 + testJunctionTree.run true true true - + make - -j5 - testPose2.run + -j2 + testKey.run true true true - + make - -j5 - testPose3.run + -j2 + testOrdering.run true true true - + make - -j5 - testRot3M.run + + testSymbolicBayesNet.run + true + false + true + + + make + + tests/testSymbolicFactor.run + true + false + true + + + make + + testSymbolicFactorGraph.run + true + false + true + + + make + -j2 + timeSymbolMaps.run true true true - + make - -j5 - testSphere2.run + + tests/testBayesTree + true + false + true + + + make + -j2 + check true true true - + make - -j5 - testStereoCamera.run + -j2 + timeCalibratedCamera.run true true true - + make - -j5 - testCal3Unified.run + -j2 + timeRot3.run true true true - + make - -j5 - testRot2.run + -j2 + clean true true true - + make - -j5 - testRot3Q.run + -j2 + tests/testPose2.run true true true - + + make + -j2 + tests/testPose3.run + true + true + true + + + make + -j2 + SimpleRotation.run + true + true + true + + make -j5 + CameraResectioning.run + true + true + true + + + make + -j5 + PlanarSLAMExample.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + easyPoint2KalmanFilter.run + true + true + true + + + make + -j2 + elaboratePoint2KalmanFilter.run + true + true + true + + + make + -j5 + Pose2SLAMExample.run + true + true + true + + + make + -j2 + Pose2SLAMwSPCG_easy.run + true + true + true + + + make + -j5 + UGM_small.run + true + true + true + + + make + -j5 + LocalizationExample.run + true + true + true + + + make + -j5 + OdometryExample.run + true + true + true + + + make + -j5 + RangeISAMExample_plaza2.run + true + true + true + + + make + -j5 + SelfCalibrationExample.run + true + true + true + + + make + -j5 + SFMExample.run + true + true + true + + + make + -j5 + VisualISAMExample.run + true + true + true + + + make + -j5 + VisualISAM2Example.run + true + true + true + + + make + -j5 + Pose2SLAMExample_graphviz.run + true + true + true + + + make + -j5 + Pose2SLAMExample_graph.run + true + true + true + + + make + -j5 + SFMExample_bal.run + true + true + true + + + make + -j5 + Pose2SLAMExample_lago.run + true + true + true + + + make + -j5 + Pose2SLAMExample_g2o.run + true + true + true + + + make + -j5 + SFMExample_SmartFactor.run + true + true + true + + + make + -j4 + Pose2SLAMExampleExpressions.run + true + true + true + + + make + -j4 + SFMExampleExpressions.run + true + true + true + + + make + -j4 + SFMExampleExpressions_bal.run + true + true + true + + + make + -j2 testRot3.run true true true - - make - -j4 - testSO3.run - true - true - true - - - make - -j4 - testQuaternion.run - true - true - true - - - make - -j4 - testOrientedPlane3.run - true - true - true - - - make - -j4 - testPinholePose.run - true - true - true - - - make - -j4 - testCyclic.run - true - true - true - - + make -j2 - all + testRot2.run true true true - + + make + -j2 + testPose3.run + true + true + true + + + make + -j2 + timeRot3.run + true + true + true + + + make + -j2 + testPose2.run + true + true + true + + + make + -j2 + testCal3_S2.run + true + true + true + + + make + -j2 + testSimpleCamera.run + true + true + true + + + make + -j2 + testHomography2.run + true + true + true + + + make + -j2 + testCalibratedCamera.run + true + true + true + + make -j2 check @@ -2231,7 +2542,7 @@ true true - + make -j2 clean @@ -2239,58 +2550,10 @@ true true - - make - -j5 - all - true - false - true - - - make - -j5 - check - true - false - true - - + make -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - clean all + testPoint2.run true true true @@ -2335,6 +2598,198 @@ true true + + make + -j5 + testEliminationTree.run + true + true + true + + + make + -j5 + testInference.run + true + true + true + + + make + -j5 + testKey.run + true + true + true + + + make + -j1 + testSymbolicBayesTree.run + true + false + true + + + make + -j1 + testSymbolicSequentialSolver.run + true + false + true + + + make + -j4 + testLabeledSymbol.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + testClusterTree.run + true + true + true + + + make + -j2 + testJunctionTree.run + true + true + true + + + make + -j2 + tests/testEliminationTree.run + true + true + true + + + make + -j2 + tests/testSymbolicFactor.run + true + true + true + + + make + -j2 + tests/testVariableSlots.run + true + true + true + + + make + -j2 + tests/testConditional.run + true + true + true + + + make + -j2 + tests/testSymbolicFactorGraph.run + true + true + true + + + make + -j5 + testLago.run + true + true + true + + + make + -j5 + testLinearContainerFactor.run + true + true + true + + + make + -j5 + testOrdering.run + true + true + true + + + make + -j5 + testValues.run + true + true + true + + + make + -j5 + testWhiteNoiseFactor.run + true + true + true + + + make + -j4 + testExpression.run + true + true + true + + + make + -j4 + testAdaptAutoDiff.run + true + true + true + + + make + -j4 + testCallRecord.run + true + true + true + + + make + -j4 + testExpressionFactor.run + true + true + true + + + make + -j4 + testExecutionTrace.run + true + true + true + make -j5 @@ -2367,31 +2822,135 @@ true true - + make - -j2 - check - true - true - true - - - make - -j2 + -j5 timeCalibratedCamera.run true true true - + make - -j2 - timeRot3.run + -j5 + timePinholeCamera.run true true true - + + make + -j5 + timeStereoCamera.run + true + true + true + + + make + -j5 + timeLago.run + true + true + true + + + make + -j5 + timePose3.run + true + true + true + + + make + -j4 + timeAdaptAutoDiff.run + true + true + true + + + make + -j4 + timeCameraExpression.run + true + true + true + + + make + -j4 + timeOneCameraExpression.run + true + true + true + + + make + -j4 + timeSFMExpressions.run + true + true + true + + + make + -j4 + timeIncremental.run + true + true + true + + + make + -j5 + testBTree.run + true + true + true + + + make + -j5 + testDSF.run + true + true + true + + + make + -j5 + testDSFMap.run + true + true + true + + + make + -j5 + testDSFVector.run + true + true + true + + + make + -j5 + testFixedVector.run + true + true + true + + + make + -j2 + all + true + true + true + + make -j2 clean @@ -2399,6 +2958,110 @@ true true + + make + -j5 + testGaussianFactorGraphUnordered.run + true + true + true + + + make + -j5 + testGaussianBayesNetUnordered.run + true + true + true + + + make + -j5 + testGaussianConditional.run + true + true + true + + + make + -j5 + testGaussianDensity.run + true + true + true + + + make + -j5 + testGaussianJunctionTree.run + true + true + true + + + make + -j5 + testHessianFactor.run + true + true + true + + + make + -j5 + testJacobianFactor.run + true + true + true + + + make + -j5 + testKalmanFilter.run + true + true + true + + + make + -j5 + testNoiseModel.run + true + true + true + + + make + -j5 + testSampler.run + true + true + true + + + make + -j5 + testSerializationLinear.run + true + true + true + + + make + -j5 + testVectorValues.run + true + true + true + + + make + -j5 + testGaussianBayesTree.run + true + true + true + make -j5 @@ -2479,66 +3142,18 @@ true true - - make - -j5 - schedulingExample.run - true - true - true - - - make - -j5 - schedulingQuals12.run - true - true - true - - - make - -j5 - schedulingQuals13.run - true - true - true - - - make - -j5 - testCSP.run - true - true - true - - - make - -j5 - testScheduler.run - true - true - true - - - make - -j5 - testSudoku.run - true - true - true - - + make -j2 - vSFMexample.run + all true true true - + make -j2 - testVSLAMGraph + clean true true true @@ -2823,487 +3438,31 @@ true true - + make - -j4 - testNonlinearOPtimizer.run + -j2 + vSFMexample.run true true true - + make -j5 - testParticleFactor.run - true - true - true - - - make - -j2 - testGaussianFactor.run - true - true - true - - - make - -j2 - install - true - true - true - - - make - -j2 clean true true true - + make - -j2 + -j5 all true true true - - make - -j2 - clean - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j5 - testAntiFactor.run - true - true - true - - - make - -j5 - testPriorFactor.run - true - true - true - - - make - -j5 - testDataset.run - true - true - true - - - make - -j5 - testEssentialMatrixFactor.run - true - true - true - - - make - -j5 - testGeneralSFMFactor_Cal3Bundler.run - true - true - true - - - make - -j5 - testGeneralSFMFactor.run - true - true - true - - - make - -j5 - testProjectionFactor.run - true - true - true - - - make - -j5 - testRotateFactor.run - true - true - true - - - make - -j5 - testPoseRotationPrior.run - true - true - true - - - make - -j5 - testImplicitSchurFactor.run - true - true - true - - - make - -j4 - testRangeFactor.run - true - true - true - - - make - -j4 - testOrientedPlane3Factor.run - true - true - true - - - make - -j4 - testSmartProjectionPoseFactor.run - true - true - true - - - make - -j4 - testRegularHessianFactor.run - true - true - true - - - make - -j4 - testRegularJacobianFactor.run - true - true - true - - - make - -j2 - SimpleRotation.run - true - true - true - - - make - -j5 - CameraResectioning.run - true - true - true - - - make - -j5 - PlanarSLAMExample.run - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - easyPoint2KalmanFilter.run - true - true - true - - - make - -j2 - elaboratePoint2KalmanFilter.run - true - true - true - - - make - -j5 - Pose2SLAMExample.run - true - true - true - - - make - -j2 - Pose2SLAMwSPCG_easy.run - true - true - true - - - make - -j5 - UGM_small.run - true - true - true - - - make - -j5 - LocalizationExample.run - true - true - true - - - make - -j5 - OdometryExample.run - true - true - true - - - make - -j5 - RangeISAMExample_plaza2.run - true - true - true - - - make - -j5 - SelfCalibrationExample.run - true - true - true - - - make - -j5 - SFMExample.run - true - true - true - - - make - -j5 - VisualISAMExample.run - true - true - true - - - make - -j5 - VisualISAM2Example.run - true - true - true - - - make - -j5 - Pose2SLAMExample_graphviz.run - true - true - true - - - make - -j5 - Pose2SLAMExample_graph.run - true - true - true - - - make - -j5 - SFMExample_bal.run - true - true - true - - - make - -j5 - Pose2SLAMExample_lago.run - true - true - true - - - make - -j5 - Pose2SLAMExample_g2o.run - true - true - true - - - make - -j5 - SFMExample_SmartFactor.run - true - true - true - - - make - -j4 - Pose2SLAMExampleExpressions.run - true - true - true - - - make - -j4 - SFMExampleExpressions.run - true - true - true - - - make - -j4 - SFMExampleExpressions_bal.run - true - true - true - - - make - -j5 - testLago.run - true - true - true - - - make - -j5 - testLinearContainerFactor.run - true - true - true - - - make - -j5 - testOrdering.run - true - true - true - - - make - -j5 - testValues.run - true - true - true - - - make - -j5 - testWhiteNoiseFactor.run - true - true - true - - - make - -j4 - testExpression.run - true - true - true - - - make - -j4 - testAdaptAutoDiff.run - true - true - true - - - make - -j4 - testCallRecord.run - true - true - true - - - make - -j4 - testExpressionFactor.run - true - true - true - - - make - -j4 - testExecutionTrace.run - true - true - true - - - make - -j4 - testImuFactor.run - true - true - true - - + make -j2 check @@ -3311,201 +3470,10 @@ true true - - make - tests/testGaussianISAM2 - true - false - true - - - make - -j5 - timeCalibratedCamera.run - true - true - true - - - make - -j5 - timePinholeCamera.run - true - true - true - - - make - -j5 - timeStereoCamera.run - true - true - true - - - make - -j5 - timeLago.run - true - true - true - - - make - -j5 - timePose3.run - true - true - true - - - make - -j4 - timeAdaptAutoDiff.run - true - true - true - - - make - -j4 - timeCameraExpression.run - true - true - true - - - make - -j4 - timeOneCameraExpression.run - true - true - true - - - make - -j4 - timeSFMExpressions.run - true - true - true - - - make - -j4 - timeIncremental.run - true - true - true - - - make - -j4 - timeSFMBAL.run - true - true - true - - + make -j2 - testRot3.run - true - true - true - - - make - -j2 - testRot2.run - true - true - true - - - make - -j2 - testPose3.run - true - true - true - - - make - -j2 - timeRot3.run - true - true - true - - - make - -j2 - testPose2.run - true - true - true - - - make - -j2 - testCal3_S2.run - true - true - true - - - make - -j2 - testSimpleCamera.run - true - true - true - - - make - -j2 - testHomography2.run - true - true - true - - - make - -j2 - testCalibratedCamera.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - testPoint2.run - true - true - true - - - make - -j5 - wrap + testVSLAMGraph true true true