From e0f6570f8fe63a5969ad65f8c2774b0cdbf780ab Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 7 Jun 2015 20:53:54 -0700 Subject: [PATCH 01/47] Timing script that takes BAL file as input. Compile with BUILD_TYPE=Timing --- timing/timeSFMBAL.cpp | 67 +++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 67 insertions(+) create mode 100644 timing/timeSFMBAL.cpp diff --git a/timing/timeSFMBAL.cpp b/timing/timeSFMBAL.cpp new file mode 100644 index 000000000..49bf23024 --- /dev/null +++ b/timing/timeSFMBAL.cpp @@ -0,0 +1,67 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file timeSFMBAL.cpp + * @brief time structure from motion with BAL file + * @author Frank Dellaert + * @date June 6, 2015 + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +//#define TERNARY + +int main(int argc, char *argv[]) { + typedef GeneralSFMFactor, Point3> sfmFactor; + using symbol_shorthand::P; + + 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!"); + + 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))); + } + + Values initial = initialCamerasAndPointsEstimate(db); + + LevenbergMarquardtOptimizer lm(graph, initial); + + Values actual = lm.optimize(); + tictoc_print_(); + + return 0; +} From 73a09c508d81678e9f5ff926f08a6651b7c6923b Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 7 Jun 2015 20:54:03 -0700 Subject: [PATCH 02/47] isUnit --- gtsam/linear/NoiseModel.h | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 923e7c7e9..05ed21977 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -62,10 +62,11 @@ namespace gtsam { Base(size_t dim = 1):dim_(dim) {} virtual ~Base() {} - /// true if a constrained noise mode, saves slow/clumsy dynamic casting - virtual bool isConstrained() const { - return false; // default false - } + /// true if a constrained noise model, saves slow/clumsy dynamic casting + virtual bool isConstrained() const { return false; } // default false + + /// true if a unit noise model, saves slow/clumsy dynamic casting + virtual bool isUnit() const { return false; } // default false /// Dimensionality inline size_t dim() const { return dim_;} @@ -390,9 +391,7 @@ namespace gtsam { virtual ~Constrained() {} /// true if a constrained noise mode, saves slow/clumsy dynamic casting - virtual bool isConstrained() const { - return true; - } + virtual bool isConstrained() const { return true; } /// Return true if a particular dimension is free or constrained bool constrained(size_t i) const; @@ -590,6 +589,9 @@ namespace gtsam { return shared_ptr(new Unit(dim)); } + /// true if a unit noise model, saves slow/clumsy dynamic casting + virtual bool isUnit() const { return true; } + virtual void print(const std::string& name) const; virtual double Mahalanobis(const Vector& v) const {return v.dot(v); } virtual Vector whiten(const Vector& v) const { return v; } From 5fd5f5786f672d39b983de867eaa8c4939058ce4 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 7 Jun 2015 20:54:24 -0700 Subject: [PATCH 03/47] Header discipline, and split up updateATA timing --- gtsam/linear/HessianFactor.cpp | 27 ++++++++++++++------------- 1 file changed, 14 insertions(+), 13 deletions(-) diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index f2bebcab9..6df4e7337 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -15,17 +15,17 @@ * @date Dec 8, 2010 */ -#include -#include -#include -#include -#include #include #include #include #include #include #include +#include +#include +#include +#include +#include #include #include @@ -405,7 +405,7 @@ double HessianFactor::error(const VectorValues& c) const { /* ************************************************************************* */ void HessianFactor::updateATA(const HessianFactor& update, const Scatter& scatter) { - gttic(updateATA); + gttic(updateATA_HessianFactor); // This function updates 'combined' with the information in 'update'. 'scatter' maps variables in // the update factor to slots in the combined factor. @@ -440,15 +440,16 @@ void HessianFactor::updateATA(const JacobianFactor& update, // 'scatter' maps variables in the update factor to slots in the combined // factor. - gttic(updateATA); + gttic(updateATA_JacobianFactor); if (update.rows() > 0) { gttic(whiten); // Whiten the factor if it has a noise model boost::optional _whitenedFactor; const JacobianFactor* whitenedFactor = &update; - if (update.get_model()) { - if (update.get_model()->isConstrained()) + const SharedDiagonal& model = update.get_model(); + if (model && !model->isUnit()) { + if (model->isConstrained()) throw invalid_argument( "Cannot update HessianFactor from JacobianFactor with constrained noise model"); _whitenedFactor = update.whiten(); @@ -457,10 +458,10 @@ void HessianFactor::updateATA(const JacobianFactor& update, gttoc(whiten); // A is the whitened Jacobian matrix A, and we are going to perform I += A'*A below - const VerticalBlockMatrix& A = whitenedFactor->matrixObject(); + const VerticalBlockMatrix& Ab = whitenedFactor->matrixObject(); // N is number of variables in HessianFactor, n in JacobianFactor - DenseIndex N = this->info_.nBlocks() - 1, n = A.nBlocks() - 1; + DenseIndex N = this->info_.nBlocks() - 1, n = Ab.nBlocks() - 1; // First build an array of slots gttic(slots); @@ -479,10 +480,10 @@ void HessianFactor::updateATA(const JacobianFactor& update, // Fill off-diagonal blocks with Ai'*Aj for (DenseIndex i = 0; i < j; ++i) { DenseIndex I = slots[i]; // get block in Hessian - info_(I, J).knownOffDiagonal() += A(i).transpose() * A(j); + info_(I, J).knownOffDiagonal() += Ab(i).transpose() * Ab(j); } // Fill diagonal block with Aj'*Aj - info_(J, J).selfadjointView().rankUpdate(A(j).transpose()); + info_(J, J).selfadjointView().rankUpdate(Ab(j).transpose()); } gttoc(update); } From c6269dfe7684c7955bf14b2c6954412a7c1c29f3 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 8 Jun 2015 18:28:27 -0400 Subject: [PATCH 04/47] Better iteration timing --- gtsam/nonlinear/NonlinearOptimizer.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index 00746d9b7..77d26d361 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -68,6 +68,7 @@ void NonlinearOptimizer::defaultOptimize() { // Do next iteration currentError = this->error(); this->iterate(); + tictoc_finishedIteration(); // Maybe show output if(params.verbosity >= NonlinearOptimizerParams::VALUES) this->values().print("newValues"); From 116c9d43980c87f6638f95a953aae4a79356c9a5 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 8 Jun 2015 18:28:46 -0400 Subject: [PATCH 05/47] Use same defaults as ceres --- gtsam/nonlinear/LevenbergMarquardtOptimizer.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h index 009b480f2..b3cc3746d 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h @@ -60,7 +60,7 @@ public: double max_diagonal_; ///< when using diagonal damping saturates the maximum diagonal entries (default: 1e32) LevenbergMarquardtParams() : - lambdaInitial(1e-5), lambdaFactor(10.0), lambdaUpperBound(1e5), lambdaLowerBound( + lambdaInitial(1e-4), lambdaFactor(2.0), lambdaUpperBound(1e5), lambdaLowerBound( 0.0), verbosityLM(SILENT), minModelFidelity(1e-3), diagonalDamping(false), reuse_diagonal_(false), useFixedLambdaFactor_(true), min_diagonal_(1e-6), max_diagonal_(1e32) { From 7f49a7a1fb170c90736c7bac04e04f69ef63bf2c Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 8 Jun 2015 20:00:49 -0400 Subject: [PATCH 06/47] Fixed comments --- gtsam/inference/Ordering.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/inference/Ordering.h b/gtsam/inference/Ordering.h index 274d5681c..98c369fcb 100644 --- a/gtsam/inference/Ordering.h +++ b/gtsam/inference/Ordering.h @@ -104,9 +104,9 @@ namespace gtsam { /// VariableIndex, it is faster to use COLAMD(const VariableIndex&). This function constrains /// the variables in \c constrainLast to the end of the ordering, and orders all other variables /// before in a fill-reducing ordering. If \c forceOrder is true, the variables in \c - /// constrainLast will be ordered in the same order specified in the vector \c - /// constrainLast. If \c forceOrder is false, the variables in \c constrainFirst will be - /// ordered after all the others, but will be rearranged by CCOLAMD to reduce fill-in as well. + /// constrainFirst will be ordered in the same order specified in the vector \c + /// constrainFirst. If \c forceOrder is false, the variables in \c constrainFirst will be + /// ordered before all the others, but will be rearranged by CCOLAMD to reduce fill-in as well. template static Ordering colamdConstrainedFirst(const FactorGraph& graph, const std::vector& constrainFirst, bool forceOrder = false) { @@ -117,7 +117,7 @@ namespace gtsam { /// orders all other variables after in a fill-reducing ordering. If \c forceOrder is true, the /// variables in \c constrainFirst will be ordered in the same order specified in the /// vector \c constrainFirst. If \c forceOrder is false, the variables in \c - /// constrainFirst will be ordered after all the others, but will be rearranged by CCOLAMD to + /// constrainFirst will be ordered before all the others, but will be rearranged by CCOLAMD to /// reduce fill-in as well. static GTSAM_EXPORT Ordering colamdConstrainedFirst(const VariableIndex& variableIndex, const std::vector& constrainFirst, bool forceOrder = false); From 627febfbaa1ea92b5dba8c291630d61b819c9b34 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 8 Jun 2015 20:01:04 -0400 Subject: [PATCH 07/47] Fixed headers --- gtsam/base/FastDefaultAllocator.h | 5 ++--- gtsam/base/FastMap.h | 2 +- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/gtsam/base/FastDefaultAllocator.h b/gtsam/base/FastDefaultAllocator.h index 156a87f55..bf5cfc498 100644 --- a/gtsam/base/FastDefaultAllocator.h +++ b/gtsam/base/FastDefaultAllocator.h @@ -17,8 +17,7 @@ */ #pragma once - -#include +#include // Configuration from CMake #if !defined GTSAM_ALLOCATOR_BOOSTPOOL && !defined GTSAM_ALLOCATOR_TBB && !defined GTSAM_ALLOCATOR_STL # ifdef GTSAM_USE_TBB @@ -85,4 +84,4 @@ namespace gtsam }; } -} \ No newline at end of file +} diff --git a/gtsam/base/FastMap.h b/gtsam/base/FastMap.h index 7cd6e28b8..65d532191 100644 --- a/gtsam/base/FastMap.h +++ b/gtsam/base/FastMap.h @@ -19,9 +19,9 @@ #pragma once #include -#include #include #include +#include namespace gtsam { From f3e54ff916f3c9e665139ab462567dcb8809c438 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 8 Jun 2015 20:01:33 -0400 Subject: [PATCH 08/47] Some refactoring/formatting --- gtsam/base/timing.cpp | 61 ++++++++++++++++++------------------- gtsam/base/timing.h | 71 +++++++++++++++++++++++++++---------------- 2 files changed, 74 insertions(+), 58 deletions(-) diff --git a/gtsam/base/timing.cpp b/gtsam/base/timing.cpp index 36f1c2f5f..b76746ba0 100644 --- a/gtsam/base/timing.cpp +++ b/gtsam/base/timing.cpp @@ -16,24 +16,28 @@ * @date Oct 5, 2010 */ -#include -#include -#include -#include -#include -#include -#include -#include - #include #include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + namespace gtsam { namespace internal { -GTSAM_EXPORT boost::shared_ptr timingRoot( +GTSAM_EXPORT boost::shared_ptr gTimingRoot( new TimingOutline("Total", getTicTocID("Total"))); -GTSAM_EXPORT boost::weak_ptr timingCurrent(timingRoot); +GTSAM_EXPORT boost::weak_ptr gCurrentTimer(gTimingRoot); /* ************************************************************************* */ // Implementation of TimingOutline @@ -50,8 +54,8 @@ void TimingOutline::add(size_t usecs, size_t usecsWall) { } /* ************************************************************************* */ -TimingOutline::TimingOutline(const std::string& label, size_t myId) : - myId_(myId), t_(0), tWall_(0), t2_(0.0), tIt_(0), tMax_(0), tMin_(0), n_(0), myOrder_( +TimingOutline::TimingOutline(const std::string& label, size_t id) : + id_(id), t_(0), tWall_(0), t2_(0.0), tIt_(0), tMax_(0), tMin_(0), n_(0), myOrder_( 0), lastChildOrder_(0), label_(label) { #ifdef GTSAM_USING_NEW_BOOST_TIMERS timer_.stop(); @@ -153,7 +157,7 @@ const boost::shared_ptr& TimingOutline::child(size_t child, } /* ************************************************************************* */ -void TimingOutline::ticInternal() { +void TimingOutline::tic() { #ifdef GTSAM_USING_NEW_BOOST_TIMERS assert(timer_.is_stopped()); timer_.start(); @@ -169,7 +173,7 @@ void TimingOutline::ticInternal() { } /* ************************************************************************* */ -void TimingOutline::tocInternal() { +void TimingOutline::toc() { #ifdef GTSAM_USING_NEW_BOOST_TIMERS assert(!timer_.is_stopped()); @@ -212,7 +216,6 @@ void TimingOutline::finishedIteration() { } /* ************************************************************************* */ -// Generate or retrieve a unique global ID number that will be used to look up tic_/toc statements size_t getTicTocID(const char *descriptionC) { const std::string description(descriptionC); // Global (static) map from strings to ID numbers and current next ID number @@ -232,37 +235,33 @@ size_t getTicTocID(const char *descriptionC) { } /* ************************************************************************* */ -void ticInternal(size_t id, const char *labelC) { +void tic(size_t id, const char *labelC) { const std::string label(labelC); - if (ISDEBUG("timing-verbose")) - std::cout << "gttic_(" << id << ", " << label << ")" << std::endl; boost::shared_ptr node = // - timingCurrent.lock()->child(id, label, timingCurrent); - timingCurrent = node; - node->ticInternal(); + gCurrentTimer.lock()->child(id, label, gCurrentTimer); + gCurrentTimer = node; + node->tic(); } /* ************************************************************************* */ -void tocInternal(size_t id, const char *label) { - if (ISDEBUG("timing-verbose")) - std::cout << "gttoc(" << id << ", " << label << ")" << std::endl; - boost::shared_ptr current(timingCurrent.lock()); - if (id != current->myId_) { - timingRoot->print(); +void toc(size_t id, const char *label) { + boost::shared_ptr current(gCurrentTimer.lock()); + if (id != current->id_) { + gTimingRoot->print(); throw std::invalid_argument( (boost::format( "gtsam timing: Mismatched tic/toc: gttoc(\"%s\") called when last tic was \"%s\".") % label % current->label_).str()); } if (!current->parent_.lock()) { - timingRoot->print(); + gTimingRoot->print(); throw std::invalid_argument( (boost::format( "gtsam timing: Mismatched tic/toc: extra gttoc(\"%s\"), already at the root") % label).str()); } - current->tocInternal(); - timingCurrent = current->parent_; + current->toc(); + gCurrentTimer = current->parent_; } } // namespace internal diff --git a/gtsam/base/timing.h b/gtsam/base/timing.h index 7a43ef884..a904c5f08 100644 --- a/gtsam/base/timing.h +++ b/gtsam/base/timing.h @@ -17,12 +17,15 @@ */ #pragma once -#include -#include -#include -#include -#include #include +#include + +#include +#include +#include + +#include +#include // This file contains the GTSAM timing instrumentation library, a low-overhead method for // learning at a medium-fine level how much time various components of an algorithm take @@ -125,16 +128,21 @@ namespace gtsam { namespace internal { + // Generate/retrieve a unique global ID number that will be used to look up tic/toc statements GTSAM_EXPORT size_t getTicTocID(const char *description); - GTSAM_EXPORT void ticInternal(size_t id, const char *label); - GTSAM_EXPORT void tocInternal(size_t id, const char *label); + + // Create new TimingOutline child for gCurrentTimer, make it gCurrentTimer, and call tic method + GTSAM_EXPORT void tic(size_t id, const char *label); + + // Call toc on gCurrentTimer and then set gCurrentTimer to the parent of gCurrentTimer + GTSAM_EXPORT void toc(size_t id, const char *label); /** * Timing Entry, arranged in a tree */ class GTSAM_EXPORT TimingOutline { protected: - size_t myId_; + size_t id_; size_t t_; size_t tWall_; double t2_ ; ///< cache the \sum t_i^2 @@ -176,29 +184,38 @@ namespace gtsam { void print2(const std::string& outline = "", const double parentTotal = -1.0) const; const boost::shared_ptr& child(size_t child, const std::string& label, const boost::weak_ptr& thisPtr); - void ticInternal(); - void tocInternal(); + void tic(); + void toc(); void finishedIteration(); - GTSAM_EXPORT friend void tocInternal(size_t id, const char *label); + GTSAM_EXPORT friend void toc(size_t id, const char *label); }; // \TimingOutline /** - * No documentation + * Small class that calls internal::tic at construction, and internol::toc when destroyed */ class AutoTicToc { - private: + private: size_t id_; - const char *label_; + const char* label_; bool isSet_; - public: - AutoTicToc(size_t id, const char* label) : id_(id), label_(label), isSet_(true) { ticInternal(id_, label_); } - void stop() { tocInternal(id_, label_); isSet_ = false; } - ~AutoTicToc() { if(isSet_) stop(); } + + public: + AutoTicToc(size_t id, const char* label) + : id_(id), label_(label), isSet_(true) { + tic(id_, label_); + } + void stop() { + toc(id_, label_); + isSet_ = false; + } + ~AutoTicToc() { + if (isSet_) stop(); + } }; - GTSAM_EXTERN_EXPORT boost::shared_ptr timingRoot; - GTSAM_EXTERN_EXPORT boost::weak_ptr timingCurrent; + GTSAM_EXTERN_EXPORT boost::shared_ptr gTimingRoot; + GTSAM_EXTERN_EXPORT boost::weak_ptr gCurrentTimer; } // Tic and toc functions that are always active (whether or not ENABLE_TIMING is defined) @@ -210,7 +227,7 @@ namespace gtsam { // tic #define gttic_(label) \ static const size_t label##_id_tic = ::gtsam::internal::getTicTocID(#label); \ - ::gtsam::internal::AutoTicToc label##_obj = ::gtsam::internal::AutoTicToc(label##_id_tic, #label) + ::gtsam::internal::AutoTicToc label##_obj(label##_id_tic, #label) // toc #define gttoc_(label) \ @@ -228,26 +245,26 @@ namespace gtsam { // indicate iteration is finished inline void tictoc_finishedIteration_() { - ::gtsam::internal::timingRoot->finishedIteration(); } + ::gtsam::internal::gTimingRoot->finishedIteration(); } // print inline void tictoc_print_() { - ::gtsam::internal::timingRoot->print(); } + ::gtsam::internal::gTimingRoot->print(); } // print mean and standard deviation inline void tictoc_print2_() { - ::gtsam::internal::timingRoot->print2(); } + ::gtsam::internal::gTimingRoot->print2(); } // get a node by label and assign it to variable #define tictoc_getNode(variable, label) \ static const size_t label##_id_getnode = ::gtsam::internal::getTicTocID(#label); \ const boost::shared_ptr variable = \ - ::gtsam::internal::timingCurrent.lock()->child(label##_id_getnode, #label, ::gtsam::internal::timingCurrent); + ::gtsam::internal::gCurrentTimer.lock()->child(label##_id_getnode, #label, ::gtsam::internal::gCurrentTimer); // reset inline void tictoc_reset_() { - ::gtsam::internal::timingRoot.reset(new ::gtsam::internal::TimingOutline("Total", ::gtsam::internal::getTicTocID("Total"))); - ::gtsam::internal::timingCurrent = ::gtsam::internal::timingRoot; } + ::gtsam::internal::gTimingRoot.reset(new ::gtsam::internal::TimingOutline("Total", ::gtsam::internal::getTicTocID("Total"))); + ::gtsam::internal::gCurrentTimer = ::gtsam::internal::gTimingRoot; } #ifdef ENABLE_TIMING #define gttic(label) gttic_(label) From 71caa400950f1c809c72f689b5232ca343065774 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 8 Jun 2015 20:01:56 -0400 Subject: [PATCH 09/47] Use Schur ordering --- timing/timeSFMBAL.cpp | 34 ++++++++++++++++++++++------------ 1 file changed, 22 insertions(+), 12 deletions(-) diff --git a/timing/timeSFMBAL.cpp b/timing/timeSFMBAL.cpp index 49bf23024..98fa3d249 100644 --- a/timing/timeSFMBAL.cpp +++ b/timing/timeSFMBAL.cpp @@ -16,20 +16,20 @@ * @date June 6, 2015 */ -#include -#include +#include +#include #include #include #include -#include -#include -#include #include #include -#include #include -#include -#include +#include +#include +#include +#include + +#include #include #include #include @@ -39,18 +39,19 @@ using namespace gtsam; //#define TERNARY -int main(int argc, char *argv[]) { +int main(int argc, char* argv[]) { typedef GeneralSFMFactor, Point3> sfmFactor; using symbol_shorthand::P; + // Load BAL file (default is tiny) string defaultFilename = findExampleDataFile("dubrovnik-3-7-pre"); SfM_data db; - bool success = readBAL(argc>1 ? argv[1] : defaultFilename, db); + bool success = readBAL(argc > 1 ? argv[1] : defaultFilename, db); if (!success) throw runtime_error("Could not access file!"); + // 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))); @@ -58,9 +59,18 @@ int main(int argc, char *argv[]) { Values initial = initialCamerasAndPointsEstimate(db); - LevenbergMarquardtOptimizer lm(graph, initial); + // Create Schur-complement ordering + vector pointKeys; + for (size_t j = 0; j < db.number_tracks(); j++) pointKeys.push_back(P(j)); + Ordering schurOrdering = Ordering::colamdConstrainedFirst(graph, pointKeys, true); + // Optimize + LevenbergMarquardtParams params; + params.setOrdering(schurOrdering); + LevenbergMarquardtOptimizer lm(graph, initial, params); Values actual = lm.optimize(); + + tictoc_finishedIteration_(); tictoc_print_(); return 0; From 39ffe3ac32f7143de468e6a0f241243a21b4186c Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 10 Jun 2015 15:53:43 -0400 Subject: [PATCH 10/47] Made updateATA a virtual method for a small saving in CPU, but more importantly to allow for custom Jacobian or HessianFactors... --- gtsam/linear/GaussianFactor.h | 10 +++ gtsam/linear/HessianFactor.cpp | 115 ++++++-------------------------- gtsam/linear/HessianFactor.h | 13 +--- gtsam/linear/JacobianFactor.cpp | 43 ++++++++++++ gtsam/linear/JacobianFactor.h | 7 ++ 5 files changed, 85 insertions(+), 103 deletions(-) diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index 6a7d91bc9..bb4b20e58 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -28,6 +28,8 @@ namespace gtsam { // Forward declarations class VectorValues; + class Scatter; + class SymmetricBlockMatrix; /** * An abstract virtual base class for JacobianFactor and HessianFactor. A GaussianFactor has a @@ -119,6 +121,14 @@ namespace gtsam { */ virtual GaussianFactor::shared_ptr negate() const = 0; + /** Update an information matrix by adding the information corresponding to this factor + * (used internally during elimination). + * @param scatter A mapping from variable index to slot index in this HessianFactor + * @param info The information matrix to be updated + */ + virtual void updateATA(const Scatter& scatter, + SymmetricBlockMatrix* info) const = 0; + /// y += alpha * A'*A*x virtual void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const = 0; diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 6df4e7337..9dbc2b52d 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -75,7 +75,7 @@ Scatter::Scatter(const GaussianFactorGraph& gfg, const JacobianFactor* asJacobian = dynamic_cast(factor.get()); if (!asJacobian || asJacobian->cols() > 1) - this->insert( + insert( make_pair(*variable, SlotEntry(none, factor->getDim(variable)))); } } @@ -296,16 +296,8 @@ HessianFactor::HessianFactor(const GaussianFactorGraph& factors, // Form A' * A gttic(update); BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) - { - if(factor) { - if(const HessianFactor* hessian = dynamic_cast(factor.get())) - updateATA(*hessian, *scatter); - else if(const JacobianFactor* jacobian = dynamic_cast(factor.get())) - updateATA(*jacobian, *scatter); - else - throw invalid_argument("GaussianFactor is neither Hessian nor Jacobian"); - } - } + if(factor) + factor->updateATA(*scatter, &info_); gttoc(update); } @@ -313,8 +305,8 @@ HessianFactor::HessianFactor(const GaussianFactorGraph& factors, void HessianFactor::print(const std::string& s, const KeyFormatter& formatter) const { cout << s << "\n"; cout << " keys: "; - for(const_iterator key=this->begin(); key!=this->end(); ++key) - cout << formatter(*key) << "(" << this->getDim(key) << ") "; + for(const_iterator key=begin(); key!=end(); ++key) + cout << formatter(*key) << "(" << getDim(key) << ") "; cout << "\n"; gtsam::print(Matrix(info_.full().selfadjointView()), "Augmented information matrix: "); } @@ -326,7 +318,7 @@ bool HessianFactor::equals(const GaussianFactor& lf, double tol) const { else { if(!Factor::equals(lf, tol)) return false; - Matrix thisMatrix = this->info_.full().selfadjointView(); + Matrix thisMatrix = info_.full().selfadjointView(); thisMatrix(thisMatrix.rows()-1, thisMatrix.cols()-1) = 0.0; Matrix rhsMatrix = static_cast(lf).info_.full().selfadjointView(); rhsMatrix(rhsMatrix.rows()-1, rhsMatrix.cols()-1) = 0.0; @@ -343,7 +335,7 @@ Matrix HessianFactor::augmentedInformation() const /* ************************************************************************* */ Matrix HessianFactor::information() const { - return info_.range(0, this->size(), 0, this->size()).selfadjointView(); + return info_.range(0, size(), 0, size()).selfadjointView(); } /* ************************************************************************* */ @@ -396,97 +388,34 @@ double HessianFactor::error(const VectorValues& c) const { double xtg = 0, xGx = 0; // extract the relevant subset of the VectorValues // NOTE may not be as efficient - const Vector x = c.vector(this->keys()); + const Vector x = c.vector(keys()); xtg = x.dot(linearTerm()); - xGx = x.transpose() * info_.range(0, this->size(), 0, this->size()).selfadjointView() * x; + xGx = x.transpose() * info_.range(0, size(), 0, size()).selfadjointView() * x; return 0.5 * (f - 2.0 * xtg + xGx); } /* ************************************************************************* */ -void HessianFactor::updateATA(const HessianFactor& update, const Scatter& scatter) -{ +void HessianFactor::updateATA(const Scatter& scatter, + SymmetricBlockMatrix* info) const { gttic(updateATA_HessianFactor); - // This function updates 'combined' with the information in 'update'. 'scatter' maps variables in - // the update factor to slots in the combined factor. + // N is number of variables in information matrix, n in HessianFactor + DenseIndex N = info->nBlocks() - 1, n = size(); // First build an array of slots - gttic(slots); - FastVector slots(update.size()); + FastVector slots(n + 1); DenseIndex slot = 0; - BOOST_FOREACH(Key j, update) { - slots[slot] = scatter.at(j).slot; - ++ slot; - } - gttoc(slots); + BOOST_FOREACH (Key key, *this) + slots[slot++] = scatter.at(key).slot; + slots[n] = N; // Apply updates to the upper triangle - gttic(update); - size_t nrInfoBlocks = this->info_.nBlocks(); - for(DenseIndex j2=0; j2 0) { - gttic(whiten); - // Whiten the factor if it has a noise model - boost::optional _whitenedFactor; - const JacobianFactor* whitenedFactor = &update; - const SharedDiagonal& model = update.get_model(); - if (model && !model->isUnit()) { - if (model->isConstrained()) - throw invalid_argument( - "Cannot update HessianFactor from JacobianFactor with constrained noise model"); - _whitenedFactor = update.whiten(); - whitenedFactor = &(*_whitenedFactor); - } - gttoc(whiten); - - // A is the whitened Jacobian matrix A, and we are going to perform I += A'*A below - const VerticalBlockMatrix& Ab = whitenedFactor->matrixObject(); - - // N is number of variables in HessianFactor, n in JacobianFactor - DenseIndex N = this->info_.nBlocks() - 1, n = Ab.nBlocks() - 1; - - // First build an array of slots - gttic(slots); - FastVector slots(n + 1); - DenseIndex slot = 0; - BOOST_FOREACH(Key j, update) - slots[slot++] = scatter.at(j).slot; - slots[n] = N; - gttoc(slots); - - // Apply updates to the upper triangle - gttic(update); - // Loop over blocks of A, including RHS with j==n - for (DenseIndex j = 0; j <= n; ++j) { - DenseIndex J = slots[j]; // get block in Hessian - // Fill off-diagonal blocks with Ai'*Aj - for (DenseIndex i = 0; i < j; ++i) { - DenseIndex I = slots[i]; // get block in Hessian - info_(I, J).knownOffDiagonal() += Ab(i).transpose() * Ab(j); - } - // Fill diagonal block with Aj'*Aj - info_(J, J).selfadjointView().rankUpdate(Ab(j).transpose()); - } - gttoc(update); - } } /* ************************************************************************* */ diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index dbec5bb59..c3cea3f51 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -363,19 +363,12 @@ namespace gtsam { /** Return the full augmented Hessian matrix of this factor as a SymmetricBlockMatrix object. */ const SymmetricBlockMatrix& matrixObject() const { return info_; } - /** Update the factor by adding the information from the JacobianFactor + /** Update an information matrix by adding the information corresponding to this factor * (used internally during elimination). - * @param update The JacobianFactor containing the new information to add * @param scatter A mapping from variable index to slot index in this HessianFactor + * @param info The information matrix to be updated */ - void updateATA(const JacobianFactor& update, const Scatter& scatter); - - /** Update the factor by adding the information from the HessianFactor - * (used internally during elimination). - * @param update The HessianFactor containing the new information to add - * @param scatter A mapping from variable index to slot index in this HessianFactor - */ - void updateATA(const HessianFactor& update, const Scatter& scatter); + void updateATA(const Scatter& scatter, SymmetricBlockMatrix* info) const; /** y += alpha * A'*A*x */ void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const; diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 11025fc0f..bb910aedb 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -497,6 +497,49 @@ map JacobianFactor::hessianBlockDiagonal() const { return blocks; } +/* ************************************************************************* */ +void JacobianFactor::updateATA(const Scatter& scatter, + SymmetricBlockMatrix* info) const { + gttic(updateATA_JacobianFactor); + + if (rows() == 0) return; + + // Whiten the factor if it has a noise model + const SharedDiagonal& model = get_model(); + if (model && !model->isUnit()) { + if (model->isConstrained()) + throw invalid_argument( + "JacobianFactor::updateATA: cannot update information with " + "constrained noise model"); + JacobianFactor whitenedFactor = whiten(); + whitenedFactor.updateATA(scatter, info); + } else { + // Ab_ is the augmented Jacobian matrix A, and we perform I += A'*A below + // N is number of variables in information matrix, n in JacobianFactor + DenseIndex N = info->nBlocks() - 1, n = Ab_.nBlocks() - 1; + + // First build an array of slots + FastVector slots(n + 1); + DenseIndex slot = 0; + BOOST_FOREACH (Key key, *this) + slots[slot++] = scatter.at(key).slot; + slots[n] = N; + + // Apply updates to the upper triangle + // Loop over blocks of A, including RHS with j==n + for (DenseIndex j = 0; j <= n; ++j) { + DenseIndex J = slots[j]; + // Fill off-diagonal blocks with Ai'*Aj + for (DenseIndex i = 0; i < j; ++i) { + DenseIndex I = slots[i]; + (*info)(I, J).knownOffDiagonal() += Ab_(i).transpose() * Ab_(j); + } + // Fill diagonal block with Aj'*Aj + (*info)(J, J).selfadjointView().rankUpdate(Ab_(j).transpose()); + } + } +} + /* ************************************************************************* */ Vector JacobianFactor::operator*(const VectorValues& x) const { Vector Ax = zero(Ab_.rows()); diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 194ba5c67..73f992770 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -273,6 +273,13 @@ namespace gtsam { /** Get a view of the A matrix */ ABlock getA() { return Ab_.range(0, size()); } + /** Update an information matrix by adding the information corresponding to this factor + * (used internally during elimination). + * @param scatter A mapping from variable index to slot index in this HessianFactor + * @param info The information matrix to be updated + */ + void updateATA(const Scatter& scatter, SymmetricBlockMatrix* info) const; + /** Return A*x */ Vector operator*(const VectorValues& x) const; From 7ec26ff3239c33d646832939da6ab905c49ae718 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 10 Jun 2015 18:27:10 -0400 Subject: [PATCH 11/47] Added Whiten --- gtsam/linear/NoiseModel.cpp | 7 +++++-- gtsam/linear/NoiseModel.h | 5 +++++ 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index a8b177b43..06c5fe7fb 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -376,8 +376,11 @@ double Constrained::distance(const Vector& v) const { /* ************************************************************************* */ Matrix Constrained::Whiten(const Matrix& H) const { - // selective scaling - return vector_scale(invsigmas(), H, true); + Matrix A = H; + for (DenseIndex i=0; i<(DenseIndex)dim_; ++i) + if (!constrained(i)) // if constrained, leave row of A as is + A.row(i) *= invsigmas_(i); + return A; } /* ************************************************************************* */ diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 05ed21977..a6c63da50 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -81,6 +81,9 @@ namespace gtsam { /// Whiten an error vector. virtual Vector whiten(const Vector& v) const = 0; + /// Whiten a matrix. + virtual Matrix Whiten(const Matrix& H) const = 0; + /// Unwhiten an error vector. virtual Vector unwhiten(const Vector& v) const = 0; @@ -856,6 +859,8 @@ namespace gtsam { // TODO: functions below are dummy but necessary for the noiseModel::Base inline virtual Vector whiten(const Vector& v) const { Vector r = v; this->WhitenSystem(r); return r; } + inline virtual Matrix Whiten(const Matrix& A) const + { Vector b; Matrix B=A; this->WhitenSystem(B,b); return B; } inline virtual Vector unwhiten(const Vector& /*v*/) const { throw std::invalid_argument("unwhiten is not currently supported for robust noise models."); } inline virtual double distance(const Vector& v) const From 171793aad33f6d0a5966cc6c3b87b53b8f6ae612 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 10 Jun 2015 18:27:34 -0400 Subject: [PATCH 12/47] Prototype (faster?) linearize --- gtsam/slam/tests/testGeneralSFMFactor.cpp | 101 ++++++++++++++++++++-- 1 file changed, 95 insertions(+), 6 deletions(-) diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index 240b19a46..ad25b611c 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -19,13 +19,13 @@ #include #include #include +#include +#include #include -#include #include #include #include -#include -#include +#include #include #include @@ -101,8 +101,8 @@ TEST( GeneralSFMFactor, equals ) /* ************************************************************************* */ TEST( GeneralSFMFactor, error ) { Point2 z(3.,0.); - const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); - boost::shared_ptr factor(new Projection(z, sigma, X(1), L(1))); + const SharedNoiseModel sigma(noiseModel::Unit::Create(2)); + Projection factor(z, sigma, X(1), L(1)); // For the following configuration, the factor predicts 320,240 Values values; Rot3 R; @@ -110,7 +110,7 @@ TEST( GeneralSFMFactor, error ) { Pose3 x1(R,t1); values.insert(X(1), GeneralCamera(x1)); Point3 l1; values.insert(L(1), l1); - EXPECT(assert_equal(((Vector) Vector2(-3.0, 0.0)), factor->unwhitenedError(values))); + EXPECT(assert_equal(((Vector) Vector2(-3.0, 0.0)), factor.unwhitenedError(values))); } static const double baseline = 5.0 ; @@ -430,6 +430,95 @@ TEST(GeneralSFMFactor, CalibratedCameraPoseRange) { EXPECT(assert_equal(expected, actual, 1e-4)); } +/* ************************************************************************* */ + +static const int DimC = 11, DimL = 3; + +/// Linearize using fixed-size matrices +boost::shared_ptr linearize(const Projection& factor, + const Values& values) { + // Only linearize if the factor is active + if (!factor.active(values)) return boost::shared_ptr(); + + const Key key1 = factor.key1(), key2 = factor.key2(); + Eigen::Matrix H1; + Eigen::Matrix H2; + Vector2 b; + try { + const GeneralCamera& camera = values.at(key1); + const Point3& point = values.at(key2); + Point2 reprojError(camera.project2(point, H1, H2) - factor.measured()); + b = -reprojError.vector(); + } catch (CheiralityException& e) { + // TODO warn if verbose output asked for + return boost::make_shared(); + } + + // Whiten the system if needed + const SharedNoiseModel& noiseModel = factor.get_noiseModel(); + if (noiseModel && !noiseModel->isUnit()) { + // TODO: implement WhitenSystem for fixed size matrices and include above + H1 = noiseModel->Whiten(H1); + H2 = noiseModel->Whiten(H2); + b = noiseModel->Whiten(b); + } + + if (noiseModel && noiseModel->isConstrained()) { + using noiseModel::Constrained; + return boost::make_shared( + key1, H1, key2, H2, b, + boost::static_pointer_cast(noiseModel)->unit()); + } else { + return boost::make_shared(key1, H1, key2, H2, b); + } +} + +/* ************************************************************************* */ +TEST(GeneralSFMFactor, Linearize) { + Point2 z(3.,0.); + + // Create Values + Values values; + Rot3 R; + Point3 t1(0,0,-6); + Pose3 x1(R,t1); + values.insert(X(1), GeneralCamera(x1)); + Point3 l1; values.insert(L(1), l1); + + // Test with Empty Model + { + const SharedNoiseModel model; + Projection factor(z, model, X(1), L(1)); + GaussianFactor::shared_ptr expected = factor.NoiseModelFactor::linearize(values); + GaussianFactor::shared_ptr actual = linearize(factor, values); + EXPECT(assert_equal(*expected,*actual,1e-9)); + } + // Test with Unit Model + { + const SharedNoiseModel model(noiseModel::Unit::Create(2)); + Projection factor(z, model, X(1), L(1)); + GaussianFactor::shared_ptr expected = factor.linearize(values); + GaussianFactor::shared_ptr actual = linearize(factor, values); + EXPECT(assert_equal(*expected,*actual,1e-9)); + } + // Test with Isotropic noise + { + const SharedNoiseModel model(noiseModel::Isotropic::Sigma(2,0.5)); + Projection factor(z, model, X(1), L(1)); + GaussianFactor::shared_ptr expected = factor.linearize(values); + GaussianFactor::shared_ptr actual = linearize(factor, values); + EXPECT(assert_equal(*expected,*actual,1e-9)); + } + // Test with Constrained Model + { + const SharedNoiseModel model(noiseModel::Constrained::All(2)); + Projection factor(z, model, X(1), L(1)); + GaussianFactor::shared_ptr expected = factor.linearize(values); + GaussianFactor::shared_ptr actual = linearize(factor, values); + EXPECT(assert_equal(*expected,*actual,1e-9)); + } +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ From 5dc149fe232f81ab82952d4c97c677c76451b8e3 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 10 Jun 2015 18:37:09 -0400 Subject: [PATCH 13/47] Faster linearize now in class --- gtsam/slam/GeneralSFMFactor.h | 43 +++++++++++++++-- gtsam/slam/tests/testGeneralSFMFactor.cpp | 57 +++-------------------- 2 files changed, 47 insertions(+), 53 deletions(-) diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index 2516b2bcc..ba3d27202 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -116,7 +116,6 @@ namespace gtsam { /** h(x)-z */ Vector evaluateError(const CAMERA& camera, const LANDMARK& point, boost::optional H1=boost::none, boost::optional H2=boost::none) const { - try { Point2 reprojError(camera.project2(point,H1,H2) - measured_); return reprojError.vector(); @@ -124,12 +123,50 @@ namespace gtsam { catch( CheiralityException& e) { if (H1) *H1 = zeros(2, DimC); if (H2) *H2 = zeros(2, DimL); - std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) - << " behind Camera " << DefaultKeyFormatter(this->key1()) << std::endl; + // TODO warn if verbose output asked for return zero(2); } } + /// Linearize using fixed-size matrices + boost::shared_ptr linearize(const Values& values) { + // Only linearize if the factor is active + if (!this->active(values)) return boost::shared_ptr(); + + const Key key1 = this->key1(), key2 = this->key2(); + Eigen::Matrix H1; + Eigen::Matrix H2; + Vector2 b; + try { + const CAMERA& camera = values.at(key1); + const LANDMARK& point = values.at(key2); + Point2 reprojError(camera.project2(point, H1, H2) - measured()); + b = -reprojError.vector(); + } catch (CheiralityException& e) { + // TODO warn if verbose output asked for + return boost::make_shared(); + } + + // Whiten the system if needed + const SharedNoiseModel& noiseModel = this->get_noiseModel(); + if (noiseModel && !noiseModel->isUnit()) { + // TODO: implement WhitenSystem for fixed size matrices and include + // above + H1 = noiseModel->Whiten(H1); + H2 = noiseModel->Whiten(H2); + b = noiseModel->Whiten(b); + } + + if (noiseModel && noiseModel->isConstrained()) { + using noiseModel::Constrained; + return boost::make_shared( + key1, H1, key2, H2, b, + boost::static_pointer_cast(noiseModel)->unit()); + } else { + return boost::make_shared(key1, H1, key2, H2, b); + } + } + /** return the measured */ inline const Point2 measured() const { return measured_; diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index ad25b611c..a83db3f1d 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -430,49 +430,6 @@ TEST(GeneralSFMFactor, CalibratedCameraPoseRange) { EXPECT(assert_equal(expected, actual, 1e-4)); } -/* ************************************************************************* */ - -static const int DimC = 11, DimL = 3; - -/// Linearize using fixed-size matrices -boost::shared_ptr linearize(const Projection& factor, - const Values& values) { - // Only linearize if the factor is active - if (!factor.active(values)) return boost::shared_ptr(); - - const Key key1 = factor.key1(), key2 = factor.key2(); - Eigen::Matrix H1; - Eigen::Matrix H2; - Vector2 b; - try { - const GeneralCamera& camera = values.at(key1); - const Point3& point = values.at(key2); - Point2 reprojError(camera.project2(point, H1, H2) - factor.measured()); - b = -reprojError.vector(); - } catch (CheiralityException& e) { - // TODO warn if verbose output asked for - return boost::make_shared(); - } - - // Whiten the system if needed - const SharedNoiseModel& noiseModel = factor.get_noiseModel(); - if (noiseModel && !noiseModel->isUnit()) { - // TODO: implement WhitenSystem for fixed size matrices and include above - H1 = noiseModel->Whiten(H1); - H2 = noiseModel->Whiten(H2); - b = noiseModel->Whiten(b); - } - - if (noiseModel && noiseModel->isConstrained()) { - using noiseModel::Constrained; - return boost::make_shared( - key1, H1, key2, H2, b, - boost::static_pointer_cast(noiseModel)->unit()); - } else { - return boost::make_shared(key1, H1, key2, H2, b); - } -} - /* ************************************************************************* */ TEST(GeneralSFMFactor, Linearize) { Point2 z(3.,0.); @@ -490,31 +447,31 @@ TEST(GeneralSFMFactor, Linearize) { const SharedNoiseModel model; Projection factor(z, model, X(1), L(1)); GaussianFactor::shared_ptr expected = factor.NoiseModelFactor::linearize(values); - GaussianFactor::shared_ptr actual = linearize(factor, values); + GaussianFactor::shared_ptr actual = factor.linearize(values); EXPECT(assert_equal(*expected,*actual,1e-9)); } // Test with Unit Model { const SharedNoiseModel model(noiseModel::Unit::Create(2)); Projection factor(z, model, X(1), L(1)); - GaussianFactor::shared_ptr expected = factor.linearize(values); - GaussianFactor::shared_ptr actual = linearize(factor, values); + GaussianFactor::shared_ptr expected = factor.NoiseModelFactor::linearize(values); + GaussianFactor::shared_ptr actual = factor.linearize(values); EXPECT(assert_equal(*expected,*actual,1e-9)); } // Test with Isotropic noise { const SharedNoiseModel model(noiseModel::Isotropic::Sigma(2,0.5)); Projection factor(z, model, X(1), L(1)); - GaussianFactor::shared_ptr expected = factor.linearize(values); - GaussianFactor::shared_ptr actual = linearize(factor, values); + GaussianFactor::shared_ptr expected = factor.NoiseModelFactor::linearize(values); + GaussianFactor::shared_ptr actual = factor.linearize(values); EXPECT(assert_equal(*expected,*actual,1e-9)); } // Test with Constrained Model { const SharedNoiseModel model(noiseModel::Constrained::All(2)); Projection factor(z, model, X(1), L(1)); - GaussianFactor::shared_ptr expected = factor.linearize(values); - GaussianFactor::shared_ptr actual = linearize(factor, values); + GaussianFactor::shared_ptr expected = factor.NoiseModelFactor::linearize(values); + GaussianFactor::shared_ptr actual = factor.linearize(values); EXPECT(assert_equal(*expected,*actual,1e-9)); } } From 304cc61decdfded28412c1e4fbfaa5365193a6db Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 10 Jun 2015 23:47:53 -0700 Subject: [PATCH 14/47] Specialized fixed-size matrix --- gtsam/slam/GeneralSFMFactor.h | 69 +++++++++++++++++++++++++++++++---- 1 file changed, 61 insertions(+), 8 deletions(-) diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index ba3d27202..e44576e5f 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -25,13 +25,16 @@ #include #include #include +#include #include #include #include #include +#include #include #include #include +#include #include #include @@ -61,6 +64,8 @@ namespace gtsam { static const int DimC = FixedDimension::value; static const int DimL = FixedDimension::value; + typedef Eigen::Matrix JacobianC; + typedef Eigen::Matrix JacobianL; protected: @@ -121,21 +126,69 @@ namespace gtsam { return reprojError.vector(); } catch( CheiralityException& e) { - if (H1) *H1 = zeros(2, DimC); - if (H2) *H2 = zeros(2, DimL); + if (H1) *H1 = JacobianC::Zero(); + if (H2) *H2 = JacobianL::Zero(); // TODO warn if verbose output asked for return zero(2); } } + class LinearizedFactor : public JacobianFactor { + // Fixed size matrices + // TODO: implement generic BinaryJacobianFactor next to + // JacobianFactor + JacobianC AC_; + JacobianL AL_; + Vector2 b_; + + public: + /// Constructor + LinearizedFactor(Key i1, const JacobianC& A1, Key i2, const JacobianL& A2, + const Vector2& b, + const SharedDiagonal& model = SharedDiagonal()) + : JacobianFactor(i1, A1, i2, A2, b, model), AC_(A1), AL_(A2), b_(b) {} + + // Fixed-size matrix update + void updateATA(const Scatter& scatter, SymmetricBlockMatrix* info) const { + gttic(updateATA_LinearizedFactor); + + // Whiten the factor if it has a noise model + const SharedDiagonal& model = get_model(); + if (model && !model->isUnit()) { + if (model->isConstrained()) + throw std::invalid_argument( + "JacobianFactor::updateATA: cannot update information with " + "constrained noise model"); + JacobianFactor whitenedFactor = whiten(); + whitenedFactor.updateATA(scatter, info); + } else { + // N is number of variables in information matrix + DenseIndex N = info->nBlocks() - 1; + + // First build an array of slots + DenseIndex slotC = scatter.at(this->keys().front()).slot; + DenseIndex slotL = scatter.at(this->keys().back()).slot; + DenseIndex slotB = N; + + // We perform I += A'*A to the upper triangle + (*info)(slotC, slotC).knownOffDiagonal() += AC_.transpose() * AC_; + (*info)(slotC, slotL).knownOffDiagonal() += AC_.transpose() * AL_; + (*info)(slotC, slotB).knownOffDiagonal() += AC_.transpose() * b_; + (*info)(slotL, slotL).knownOffDiagonal() += AL_.transpose() * AL_; + (*info)(slotL, slotB).knownOffDiagonal() += AL_.transpose() * b_; + (*info)(slotB, slotB).knownOffDiagonal() += b_.transpose() * b_; + } + } + }; + /// Linearize using fixed-size matrices - boost::shared_ptr linearize(const Values& values) { + boost::shared_ptr linearize(const Values& values) const { // Only linearize if the factor is active - if (!this->active(values)) return boost::shared_ptr(); + if (!this->active(values)) return boost::shared_ptr(); const Key key1 = this->key1(), key2 = this->key2(); - Eigen::Matrix H1; - Eigen::Matrix H2; + JacobianC H1; + JacobianL H2; Vector2 b; try { const CAMERA& camera = values.at(key1); @@ -159,11 +212,11 @@ namespace gtsam { if (noiseModel && noiseModel->isConstrained()) { using noiseModel::Constrained; - return boost::make_shared( + return boost::make_shared( key1, H1, key2, H2, b, boost::static_pointer_cast(noiseModel)->unit()); } else { - return boost::make_shared(key1, H1, key2, H2, b); + return boost::make_shared(key1, H1, key2, H2, b); } } From 05a120d94c5f066356135ec77ebbd955cc37b47f Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 11 Jun 2015 09:44:23 -0700 Subject: [PATCH 15/47] Hardcode ordering and add verbosity --- timing/timeSFMBAL.cpp | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/timing/timeSFMBAL.cpp b/timing/timeSFMBAL.cpp index 98fa3d249..568c3a756 100644 --- a/timing/timeSFMBAL.cpp +++ b/timing/timeSFMBAL.cpp @@ -59,14 +59,22 @@ int main(int argc, char* argv[]) { Values initial = initialCamerasAndPointsEstimate(db); - // Create Schur-complement ordering +// Create Schur-complement ordering +#ifdef CCOLAMD vector pointKeys; for (size_t j = 0; j < db.number_tracks(); j++) pointKeys.push_back(P(j)); - Ordering schurOrdering = Ordering::colamdConstrainedFirst(graph, pointKeys, true); + 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); +#endif // Optimize LevenbergMarquardtParams params; - params.setOrdering(schurOrdering); + params.setOrdering(ordering); + params.setVerbosity("ERROR"); + params.setVerbosityLM("TRYLAMBDA"); LevenbergMarquardtOptimizer lm(graph, initial, params); Values actual = lm.optimize(); From 635a2a079256b73002c8fdabcd631b0b26e47ce6 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 11 Jun 2015 13:22:26 -0700 Subject: [PATCH 16/47] Fix bug that could disconnect graph --- gtsam/slam/GeneralSFMFactor.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index e44576e5f..be20ee3fa 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -196,8 +196,10 @@ namespace gtsam { Point2 reprojError(camera.project2(point, H1, H2) - measured()); b = -reprojError.vector(); } catch (CheiralityException& e) { + H1.setZero(); + H2.setZero(); + b.setZero(); // TODO warn if verbose output asked for - return boost::make_shared(); } // Whiten the system if needed From e140538a48b34375201f08a7750e1dab6cb5bfaa Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 11 Jun 2015 13:22:38 -0700 Subject: [PATCH 17/47] Use diagonal damping by default --- gtsam/nonlinear/LevenbergMarquardtOptimizer.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h index b3cc3746d..4e4479cd6 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h @@ -62,7 +62,7 @@ public: LevenbergMarquardtParams() : lambdaInitial(1e-4), lambdaFactor(2.0), lambdaUpperBound(1e5), lambdaLowerBound( 0.0), verbosityLM(SILENT), minModelFidelity(1e-3), - diagonalDamping(false), reuse_diagonal_(false), useFixedLambdaFactor_(true), + diagonalDamping(true), reuse_diagonal_(false), useFixedLambdaFactor_(true), min_diagonal_(1e-6), max_diagonal_(1e32) { } virtual ~LevenbergMarquardtParams() { From 29c2b47ace48b3b3b87ddea153db3f3bd994e1ba Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 11 Jun 2015 14:09:00 -0700 Subject: [PATCH 18/47] Fixed comments, added TODO --- gtsam/nonlinear/LevenbergMarquardtOptimizer.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h index 4e4479cd6..04b2d9e8d 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h @@ -46,7 +46,7 @@ public: public: - double lambdaInitial; ///< The initial Levenberg-Marquardt damping term (default: 1e-5) + double lambdaInitial; ///< The initial Levenberg-Marquardt damping term (default: 1e-4) double lambdaFactor; ///< The amount by which to multiply or divide lambda when adjusting lambda (default: 10.0) double lambdaUpperBound; ///< The maximum lambda to try before assuming the optimization has failed (default: 1e5) double lambdaLowerBound; ///< The minimum lambda used in LM (default: 0) @@ -54,7 +54,7 @@ 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 (related to efficiency) + 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) From 257060e8dda5793796c409e6c8e6a5dba2aaa4ad Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 11 Jun 2015 21:21:01 -0700 Subject: [PATCH 19/47] Scatter class in separate compilation unit --- gtsam/linear/HessianFactor.cpp | 49 --------------- gtsam/linear/HessianFactor.h | 28 +-------- gtsam/linear/JacobianFactor.cpp | 2 +- gtsam/linear/Scatter.cpp | 77 ++++++++++++++++++++++++ gtsam/linear/Scatter.h | 56 +++++++++++++++++ gtsam/linear/tests/testHessianFactor.cpp | 2 +- gtsam/linear/tests/testScatter.cpp | 63 +++++++++++++++++++ 7 files changed, 200 insertions(+), 77 deletions(-) create mode 100644 gtsam/linear/Scatter.cpp create mode 100644 gtsam/linear/Scatter.h create mode 100644 gtsam/linear/tests/testScatter.cpp diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 9dbc2b52d..a5b3e4a9a 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -53,55 +53,6 @@ namespace br { using namespace boost::range; using namespace boost::adaptors; } namespace gtsam { -/* ************************************************************************* */ -string SlotEntry::toString() const { - ostringstream oss; - oss << "SlotEntry: slot=" << slot << ", dim=" << dimension; - return oss.str(); -} - -/* ************************************************************************* */ -Scatter::Scatter(const GaussianFactorGraph& gfg, - boost::optional ordering) { - gttic(Scatter_Constructor); - static const size_t 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++); - } - } - - // 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++); - } -} - /* ************************************************************************* */ HessianFactor::HessianFactor() : info_(cref_list_of<1>(1)) diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index c3cea3f51..50a81b579 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -18,10 +18,10 @@ #pragma once +#include +#include #include #include -#include -#include #include @@ -41,30 +41,6 @@ namespace gtsam { GTSAM_EXPORT std::pair, boost::shared_ptr > EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys); - /** - * One SlotEntry stores the slot index for a variable, as well its dimension. - */ - struct GTSAM_EXPORT SlotEntry { - size_t slot, dimension; - SlotEntry(size_t _slot, size_t _dimension) - : slot(_slot), dimension(_dimension) {} - std::string toString() const; - }; - - /** - * 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 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 { - public: - Scatter() { - } - Scatter(const GaussianFactorGraph& gfg, - boost::optional ordering = boost::none); - }; - /** * @brief A Gaussian factor using the canonical parameters (information form) * diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index bb910aedb..9d0917919 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -20,7 +20,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/gtsam/linear/Scatter.cpp b/gtsam/linear/Scatter.cpp new file mode 100644 index 000000000..3efbc2004 --- /dev/null +++ b/gtsam/linear/Scatter.cpp @@ -0,0 +1,77 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file HessianFactor.cpp + * @author Richard Roberts + * @date Dec 8, 2010 + */ + +#include +#include +#include + +#include + +using namespace std; + +namespace gtsam { + +/* ************************************************************************* */ +string SlotEntry::toString() const { + ostringstream oss; + oss << "SlotEntry: slot=" << slot << ", dim=" << dimension; + return oss.str(); +} + +/* ************************************************************************* */ +Scatter::Scatter(const GaussianFactorGraph& gfg, + boost::optional ordering) { + gttic(Scatter_Constructor); + static const size_t 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++); + } + } + + // 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++); + } +} + +} // gtsam diff --git a/gtsam/linear/Scatter.h b/gtsam/linear/Scatter.h new file mode 100644 index 000000000..e212c5867 --- /dev/null +++ b/gtsam/linear/Scatter.h @@ -0,0 +1,56 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Scatter.h + * @brief Maps global variable indices to slot indices + * @author Richard Roberts + * @author Frank Dellaert + * @date Dec 8, 2010 + */ + +#pragma once + +#include +#include +#include + +#include + +namespace gtsam { + +class GaussianFactorGraph; +class Ordering; + +/** + * One SlotEntry stores the slot index for a variable, as well its dimension. + */ +struct GTSAM_EXPORT SlotEntry { + size_t slot, dimension; + SlotEntry(size_t _slot, size_t _dimension) + : slot(_slot), dimension(_dimension) {} + std::string toString() const; +}; + +/** + * 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 + * 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 { + public: + Scatter(const GaussianFactorGraph& gfg, + boost::optional ordering = boost::none); +}; + +} // \ namespace gtsam diff --git a/gtsam/linear/tests/testHessianFactor.cpp b/gtsam/linear/tests/testHessianFactor.cpp index 3a2cd0fd4..557ba3f36 100644 --- a/gtsam/linear/tests/testHessianFactor.cpp +++ b/gtsam/linear/tests/testHessianFactor.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file testCholeskyFactor.cpp + * @file testHessianFactor.cpp * @author Richard Roberts * @date Dec 15, 2010 */ diff --git a/gtsam/linear/tests/testScatter.cpp b/gtsam/linear/tests/testScatter.cpp new file mode 100644 index 000000000..19d099616 --- /dev/null +++ b/gtsam/linear/tests/testScatter.cpp @@ -0,0 +1,63 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testScatter.cpp + * @author Frank Dellaert + * @date June, 2015 + */ + +#include +#include +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +TEST(HessianFactor, CombineAndEliminate) { + static const size_t m = 3, n = 3; + Matrix A01 = + (Matrix(m, n) << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0).finished(); + Vector3 b0(1.5, 1.5, 1.5); + Vector3 s0(1.6, 1.6, 1.6); + + Matrix A10 = + (Matrix(m, n) << 2.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 2.0).finished(); + Matrix A11 = (Matrix(m, n) << -2.0, 0.0, 0.0, 0.0, -2.0, 0.0, 0.0, 0.0, -2.0) + .finished(); + Vector3 b1(2.5, 2.5, 2.5); + Vector3 s1(2.6, 2.6, 2.6); + + Matrix A21 = + (Matrix(m, n) << 3.0, 0.0, 0.0, 0.0, 3.0, 0.0, 0.0, 0.0, 3.0).finished(); + Vector3 b2(3.5, 3.5, 3.5); + 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)); + + 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_LONGS_EQUAL(n, scatter.at(0).dimension); + EXPECT_LONGS_EQUAL(n, scatter.at(1).dimension); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ From 5f41529ffffe4a090ae830c34ec5ffcba86bc196 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 11 Jun 2015 21:45:01 -0700 Subject: [PATCH 20/47] Added required method --- gtsam/slam/RegularImplicitSchurFactor.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/gtsam/slam/RegularImplicitSchurFactor.h b/gtsam/slam/RegularImplicitSchurFactor.h index b1490d465..f2fc4e819 100644 --- a/gtsam/slam/RegularImplicitSchurFactor.h +++ b/gtsam/slam/RegularImplicitSchurFactor.h @@ -115,6 +115,11 @@ public: return D; } + virtual void updateATA(const Scatter& scatter, + SymmetricBlockMatrix* info) const { + throw std::runtime_error( + "RegularImplicitSchurFactor::updateATA non implemented"); + } virtual Matrix augmentedJacobian() const { throw std::runtime_error( "RegularImplicitSchurFactor::augmentedJacobian non implemented"); From f89ffdc81cd6a09465cb3779e86478c84595c1bf Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 11 Jun 2015 23:11:24 -0700 Subject: [PATCH 21/47] Restored defaults (test failed) --- gtsam/nonlinear/LevenbergMarquardtOptimizer.h | 72 +++++++------------ 1 file changed, 26 insertions(+), 46 deletions(-) diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h index 04b2d9e8d..315e95512 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h @@ -59,60 +59,40 @@ public: 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-4), lambdaFactor(2.0), lambdaUpperBound(1e5), lambdaLowerBound( - 0.0), verbosityLM(SILENT), minModelFidelity(1e-3), - diagonalDamping(true), reuse_diagonal_(false), useFixedLambdaFactor_(true), - min_diagonal_(1e-6), max_diagonal_(1e32) { - } - virtual ~LevenbergMarquardtParams() { - } + LevenbergMarquardtParams() + : lambdaInitial(1e-5), + lambdaFactor(10.0), + lambdaUpperBound(1e5), + lambdaLowerBound(0.0), + verbosityLM(SILENT), + minModelFidelity(1e-3), + diagonalDamping(false), + reuse_diagonal_(false), + useFixedLambdaFactor_(true), + min_diagonal_(1e-6), + max_diagonal_(1e32) {} + 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 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; - } + inline std::string getLogFile() const { return logFile; } + inline bool getDiagonalDamping() const { return diagonalDamping; } - 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) { + 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 setLogFile(const std::string& s) { logFile = s; } + inline void setDiagonalDamping(bool flag) { diagonalDamping = flag; } inline void setUseFixedLambdaFactor(bool flag) { useFixedLambdaFactor_ = flag; } From 6ed82459ba8abe3f860455b4b97e7ffbbc378b66 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 11 Jun 2015 23:11:35 -0700 Subject: [PATCH 22/47] Set params to be like ceres --- timing/timeSFMBAL.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/timing/timeSFMBAL.cpp b/timing/timeSFMBAL.cpp index 568c3a756..154a72dc9 100644 --- a/timing/timeSFMBAL.cpp +++ b/timing/timeSFMBAL.cpp @@ -71,10 +71,14 @@ int main(int argc, char* argv[]) { #endif // Optimize + // Set parameters to be similar to ceres LevenbergMarquardtParams params; 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 63d918ed7a4876eabb7bbbcd08559f1ca9488627 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 11 Jun 2015 23:38:29 -0700 Subject: [PATCH 23/47] Now FastVector --- gtsam/inference/Ordering.h | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/gtsam/inference/Ordering.h b/gtsam/inference/Ordering.h index 98c369fcb..e2e7fac4c 100644 --- a/gtsam/inference/Ordering.h +++ b/gtsam/inference/Ordering.h @@ -22,17 +22,18 @@ #include #include -#include #include #include #include #include +#include +#include namespace gtsam { - class Ordering : public std::vector { + class Ordering : public FastVector { protected: - typedef std::vector Base; + typedef FastVector Base; public: From 692ddd8f361ad957f4adf7738a19ef41acdcbf47 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 11 Jun 2015 23:38:57 -0700 Subject: [PATCH 24/47] Made node keys ordered to avoid copy constructor in eliminate --- gtsam/inference/ClusterTree.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/inference/ClusterTree.h b/gtsam/inference/ClusterTree.h index 5a412a79e..09bec7452 100644 --- a/gtsam/inference/ClusterTree.h +++ b/gtsam/inference/ClusterTree.h @@ -11,7 +11,7 @@ #include #include -#include +#include namespace gtsam { @@ -37,7 +37,7 @@ namespace gtsam typedef typename FactorGraphType::Eliminate Eliminate; ///< Typedef for an eliminate subroutine struct Cluster { - typedef FastVector Keys; + typedef Ordering Keys; typedef FastVector Factors; typedef FastVector > Children; From 15d55428096082e23082dd32f7bec527adf6ab46 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 11 Jun 2015 23:48:18 -0700 Subject: [PATCH 25/47] Renamed variable --- gtsam/inference/ClusterTree-inst.h | 4 ++-- gtsam/inference/ClusterTree.h | 2 +- gtsam/inference/JunctionTree-inst.h | 17 ++++++++++++----- .../symbolic/tests/testSymbolicJunctionTree.cpp | 4 ++-- 4 files changed, 17 insertions(+), 10 deletions(-) diff --git a/gtsam/inference/ClusterTree-inst.h b/gtsam/inference/ClusterTree-inst.h index 13130bf2e..a94baaf6c 100644 --- a/gtsam/inference/ClusterTree-inst.h +++ b/gtsam/inference/ClusterTree-inst.h @@ -99,7 +99,7 @@ namespace gtsam // Do dense elimination step std::pair, boost::shared_ptr > eliminationResult = - eliminationFunction(gatheredFactors, Ordering(node->keys)); + eliminationFunction(gatheredFactors, Ordering(node->orderedFrontalKeys)); // Store conditional in BayesTree clique, and in the case of ISAM2Clique also store the remaining factor myData.bayesTreeNode->setEliminationResult(eliminationResult); @@ -123,7 +123,7 @@ namespace gtsam const std::string& s, const KeyFormatter& keyFormatter) const { std::cout << s; - BOOST_FOREACH(Key j, keys) + BOOST_FOREACH(Key j, orderedFrontalKeys) std::cout << j << " "; std::cout << "problemSize = " << problemSize_ << std::endl; } diff --git a/gtsam/inference/ClusterTree.h b/gtsam/inference/ClusterTree.h index 09bec7452..435631b21 100644 --- a/gtsam/inference/ClusterTree.h +++ b/gtsam/inference/ClusterTree.h @@ -41,7 +41,7 @@ namespace gtsam typedef FastVector Factors; typedef FastVector > Children; - Keys keys; ///< Frontal keys of this node + Keys orderedFrontalKeys; ///< Frontal keys of this node Factors factors; ///< Factors associated with this node Children children; ///< sub-trees int problemSize_; diff --git a/gtsam/inference/JunctionTree-inst.h b/gtsam/inference/JunctionTree-inst.h index 9d96c5b9c..f12e5afd2 100644 --- a/gtsam/inference/JunctionTree-inst.h +++ b/gtsam/inference/JunctionTree-inst.h @@ -49,7 +49,7 @@ namespace gtsam { // 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->keys.push_back(node->key); + 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; @@ -101,13 +101,20 @@ namespace gtsam { const typename JunctionTree::Node& childToMerge = *myData.myJTNode->children[child - nrMergedChildren]; // Merge keys, factors, and children. - myData.myJTNode->keys.insert(myData.myJTNode->keys.begin(), childToMerge.keys.begin(), childToMerge.keys.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()); + 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.keys.size(); + myNrFrontals += childToMerge.orderedFrontalKeys.size(); // Remove child from list. myData.myJTNode->children.erase(myData.myJTNode->children.begin() + (child - nrMergedChildren)); // Increment number of merged children diff --git a/gtsam/symbolic/tests/testSymbolicJunctionTree.cpp b/gtsam/symbolic/tests/testSymbolicJunctionTree.cpp index 49b14bc07..f2d891827 100644 --- a/gtsam/symbolic/tests/testSymbolicJunctionTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicJunctionTree.cpp @@ -46,10 +46,10 @@ TEST( JunctionTree, constructor ) frontal1 = list_of(2)(3), frontal2 = list_of(0)(1), sep1, sep2 = list_of(2); - EXPECT(assert_container_equality(frontal1, actual.roots().front()->keys)); + EXPECT(assert_container_equality(frontal1, actual.roots().front()->orderedFrontalKeys)); //EXPECT(assert_equal(sep1, actual.roots().front()->separator)); LONGS_EQUAL(1, (long)actual.roots().front()->factors.size()); - EXPECT(assert_container_equality(frontal2, actual.roots().front()->children.front()->keys)); + EXPECT(assert_container_equality(frontal2, actual.roots().front()->children.front()->orderedFrontalKeys)); //EXPECT(assert_equal(sep2, actual.roots().front()->children.front()->separator)); LONGS_EQUAL(2, (long)actual.roots().front()->children.front()->factors.size()); EXPECT(assert_equal(*simpleChain[2], *actual.roots().front()->factors[0])); From bcc34d1c127d746d8aff845133cfd53d51438e9a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 11 Jun 2015 23:49:28 -0700 Subject: [PATCH 26/47] No more copy constructor --- gtsam/inference/ClusterTree-inst.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/inference/ClusterTree-inst.h b/gtsam/inference/ClusterTree-inst.h index a94baaf6c..ad7ab0063 100644 --- a/gtsam/inference/ClusterTree-inst.h +++ b/gtsam/inference/ClusterTree-inst.h @@ -99,7 +99,7 @@ namespace gtsam // Do dense elimination step std::pair, boost::shared_ptr > eliminationResult = - eliminationFunction(gatheredFactors, Ordering(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); From c9910625c2754b982ff2a01959b177a5c337de98 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 11 Jun 2015 23:51:35 -0700 Subject: [PATCH 27/47] Fixed headers --- gtsam/inference/Ordering.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/inference/Ordering.h b/gtsam/inference/Ordering.h index e2e7fac4c..3e7828944 100644 --- a/gtsam/inference/Ordering.h +++ b/gtsam/inference/Ordering.h @@ -18,10 +18,6 @@ #pragma once -#include -#include -#include - #include #include #include @@ -29,6 +25,10 @@ #include #include +#include +#include +#include + namespace gtsam { class Ordering : public FastVector { From 41a0146b051d5f412245911adcb5e93f4b9b26d3 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 12 Jun 2015 00:20:37 -0700 Subject: [PATCH 28/47] changed updateATA -> updateHessian. Much clearer --- gtsam/linear/GaussianFactor.h | 2 +- gtsam/linear/HessianFactor.cpp | 4 ++-- gtsam/linear/HessianFactor.h | 2 +- gtsam/linear/JacobianFactor.cpp | 6 +++--- gtsam/linear/JacobianFactor.h | 2 +- gtsam/slam/GeneralSFMFactor.h | 6 +++--- gtsam/slam/RegularImplicitSchurFactor.h | 4 ++-- 7 files changed, 13 insertions(+), 13 deletions(-) diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index bb4b20e58..bc14cc670 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -126,7 +126,7 @@ namespace gtsam { * @param scatter A mapping from variable index to slot index in this HessianFactor * @param info The information matrix to be updated */ - virtual void updateATA(const Scatter& scatter, + virtual void updateHessian(const Scatter& scatter, SymmetricBlockMatrix* info) const = 0; /// y += alpha * A'*A*x diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index a5b3e4a9a..0cb813b01 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -248,7 +248,7 @@ HessianFactor::HessianFactor(const GaussianFactorGraph& factors, gttic(update); BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) if(factor) - factor->updateATA(*scatter, &info_); + factor->updateHessian(*scatter, &info_); gttoc(update); } @@ -346,7 +346,7 @@ double HessianFactor::error(const VectorValues& c) const { } /* ************************************************************************* */ -void HessianFactor::updateATA(const Scatter& scatter, +void HessianFactor::updateHessian(const Scatter& scatter, SymmetricBlockMatrix* info) const { gttic(updateATA_HessianFactor); // N is number of variables in information matrix, n in HessianFactor diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index 50a81b579..160d05b15 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -344,7 +344,7 @@ namespace gtsam { * @param scatter A mapping from variable index to slot index in this HessianFactor * @param info The information matrix to be updated */ - void updateATA(const Scatter& scatter, SymmetricBlockMatrix* info) const; + void updateHessian(const Scatter& scatter, SymmetricBlockMatrix* info) const; /** y += alpha * A'*A*x */ void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const; diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 9d0917919..c960dca04 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -498,7 +498,7 @@ map JacobianFactor::hessianBlockDiagonal() const { } /* ************************************************************************* */ -void JacobianFactor::updateATA(const Scatter& scatter, +void JacobianFactor::updateHessian(const Scatter& scatter, SymmetricBlockMatrix* info) const { gttic(updateATA_JacobianFactor); @@ -509,10 +509,10 @@ void JacobianFactor::updateATA(const Scatter& scatter, if (model && !model->isUnit()) { if (model->isConstrained()) throw invalid_argument( - "JacobianFactor::updateATA: cannot update information with " + "JacobianFactor::updateHessian: cannot update information with " "constrained noise model"); JacobianFactor whitenedFactor = whiten(); - whitenedFactor.updateATA(scatter, info); + whitenedFactor.updateHessian(scatter, info); } else { // Ab_ is the augmented Jacobian matrix A, and we perform I += A'*A below // N is number of variables in information matrix, n in JacobianFactor diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 73f992770..00a9b5488 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -278,7 +278,7 @@ namespace gtsam { * @param scatter A mapping from variable index to slot index in this HessianFactor * @param info The information matrix to be updated */ - void updateATA(const Scatter& scatter, SymmetricBlockMatrix* info) const; + void updateHessian(const Scatter& scatter, SymmetricBlockMatrix* info) const; /** Return A*x */ Vector operator*(const VectorValues& x) const; diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index be20ee3fa..9a8d613ad 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -149,7 +149,7 @@ namespace gtsam { : JacobianFactor(i1, A1, i2, A2, b, model), AC_(A1), AL_(A2), b_(b) {} // Fixed-size matrix update - void updateATA(const Scatter& scatter, SymmetricBlockMatrix* info) const { + void updateHessian(const Scatter& scatter, SymmetricBlockMatrix* info) const { gttic(updateATA_LinearizedFactor); // Whiten the factor if it has a noise model @@ -157,10 +157,10 @@ namespace gtsam { if (model && !model->isUnit()) { if (model->isConstrained()) throw std::invalid_argument( - "JacobianFactor::updateATA: cannot update information with " + "JacobianFactor::updateHessian: cannot update information with " "constrained noise model"); JacobianFactor whitenedFactor = whiten(); - whitenedFactor.updateATA(scatter, info); + whitenedFactor.updateHessian(scatter, info); } else { // N is number of variables in information matrix DenseIndex N = info->nBlocks() - 1; diff --git a/gtsam/slam/RegularImplicitSchurFactor.h b/gtsam/slam/RegularImplicitSchurFactor.h index f2fc4e819..87d78911d 100644 --- a/gtsam/slam/RegularImplicitSchurFactor.h +++ b/gtsam/slam/RegularImplicitSchurFactor.h @@ -115,10 +115,10 @@ public: return D; } - virtual void updateATA(const Scatter& scatter, + virtual void updateHessian(const Scatter& scatter, SymmetricBlockMatrix* info) const { throw std::runtime_error( - "RegularImplicitSchurFactor::updateATA non implemented"); + "RegularImplicitSchurFactor::updateHessian non implemented"); } virtual Matrix augmentedJacobian() const { throw std::runtime_error( From b712a65c21c145c7662d220eb9b0f7922740a00c Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 12 Jun 2015 00:28:15 -0700 Subject: [PATCH 29/47] Updated gttic strings as well --- gtsam/linear/HessianFactor.cpp | 2 +- gtsam/linear/JacobianFactor.cpp | 2 +- gtsam/slam/GeneralSFMFactor.h | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 0cb813b01..27bd61fbd 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -348,7 +348,7 @@ double HessianFactor::error(const VectorValues& c) const { /* ************************************************************************* */ void HessianFactor::updateHessian(const Scatter& scatter, SymmetricBlockMatrix* info) const { - gttic(updateATA_HessianFactor); + gttic(updateHessian_HessianFactor); // N is number of variables in information matrix, n in HessianFactor DenseIndex N = info->nBlocks() - 1, n = size(); diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index c960dca04..ff5431432 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -500,7 +500,7 @@ map JacobianFactor::hessianBlockDiagonal() const { /* ************************************************************************* */ void JacobianFactor::updateHessian(const Scatter& scatter, SymmetricBlockMatrix* info) const { - gttic(updateATA_JacobianFactor); + gttic(updateHessian_JacobianFactor); if (rows() == 0) return; diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index 9a8d613ad..ec779cbd4 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -150,7 +150,7 @@ namespace gtsam { // Fixed-size matrix update void updateHessian(const Scatter& scatter, SymmetricBlockMatrix* info) const { - gttic(updateATA_LinearizedFactor); + gttic(updateHessian_LinearizedFactor); // Whiten the factor if it has a noise model const SharedDiagonal& model = get_model(); From 6664699c4c124d5c82bb72c491b7d9d2401b42ab Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 12 Jun 2015 09:33:55 -0700 Subject: [PATCH 30/47] getSlots method --- gtsam/linear/HessianFactor.cpp | 12 +++-------- gtsam/linear/JacobianFactor.cpp | 20 +++++++----------- gtsam/linear/Scatter.cpp | 37 +++++++++++++++++++++++---------- gtsam/linear/Scatter.h | 22 +++++++++++++------- gtsam/slam/GeneralSFMFactor.h | 5 +++-- 5 files changed, 54 insertions(+), 42 deletions(-) diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 27bd61fbd..a038a7ff8 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -347,19 +347,13 @@ double HessianFactor::error(const VectorValues& c) const { /* ************************************************************************* */ void HessianFactor::updateHessian(const Scatter& scatter, - SymmetricBlockMatrix* info) const { + SymmetricBlockMatrix* info) const { gttic(updateHessian_HessianFactor); - // N is number of variables in information matrix, n in HessianFactor - DenseIndex N = info->nBlocks() - 1, n = size(); - // First build an array of slots - FastVector slots(n + 1); - DenseIndex slot = 0; - BOOST_FOREACH (Key key, *this) - slots[slot++] = scatter.at(key).slot; - slots[n] = N; + FastVector slots = scatter.getSlotsForKeys(keys_); // Apply updates to the upper triangle + DenseIndex n = size(); for (DenseIndex j = 0; j <= n; ++j) { DenseIndex J = slots[j]; for (DenseIndex i = 0; i <= j; ++i) { diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index ff5431432..3d42db1ca 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -499,7 +499,7 @@ map JacobianFactor::hessianBlockDiagonal() const { /* ************************************************************************* */ void JacobianFactor::updateHessian(const Scatter& scatter, - SymmetricBlockMatrix* info) const { + SymmetricBlockMatrix* info) const { gttic(updateHessian_JacobianFactor); if (rows() == 0) return; @@ -514,24 +514,20 @@ void JacobianFactor::updateHessian(const Scatter& scatter, JacobianFactor whitenedFactor = whiten(); whitenedFactor.updateHessian(scatter, info); } else { - // Ab_ is the augmented Jacobian matrix A, and we perform I += A'*A below - // N is number of variables in information matrix, n in JacobianFactor - DenseIndex N = info->nBlocks() - 1, n = Ab_.nBlocks() - 1; - // First build an array of slots - FastVector slots(n + 1); - DenseIndex slot = 0; - BOOST_FOREACH (Key key, *this) - slots[slot++] = scatter.at(key).slot; - slots[n] = N; + FastVector slots = scatter.getSlotsForKeys(keys_); + + // Ab_ is the augmented Jacobian matrix A, and we perform I += A'*A below + DenseIndex n = Ab_.nBlocks() - 1; // Apply updates to the upper triangle // Loop over blocks of A, including RHS with j==n + // BOOST_FOREACH(DenseIndex J, slots) for (DenseIndex j = 0; j <= n; ++j) { - DenseIndex J = slots[j]; + const DenseIndex J = slots[j]; // Fill off-diagonal blocks with Ai'*Aj for (DenseIndex i = 0; i < j; ++i) { - DenseIndex I = slots[i]; + const DenseIndex I = slots[i]; (*info)(I, J).knownOffDiagonal() += Ab_(i).transpose() * Ab_(j); } // Fill diagonal block with Aj'*Aj diff --git a/gtsam/linear/Scatter.cpp b/gtsam/linear/Scatter.cpp index 3efbc2004..21d20c14c 100644 --- a/gtsam/linear/Scatter.cpp +++ b/gtsam/linear/Scatter.cpp @@ -34,16 +34,17 @@ string SlotEntry::toString() const { /* ************************************************************************* */ Scatter::Scatter(const GaussianFactorGraph& gfg, - boost::optional ordering) { + boost::optional ordering) { gttic(Scatter_Constructor); - static const size_t none = std::numeric_limits::max(); + static const DenseIndex none = std::numeric_limits::max(); // First do the set union. - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, gfg) { + 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 + 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) @@ -56,22 +57,36 @@ Scatter::Scatter(const GaussianFactorGraph& gfg, // If we have an ordering, pre-fill the ordered variables first size_t slot = 0; if (ordering) { - BOOST_FOREACH(Key key, *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."); + "contained extra variables that did not appear in the factors to " + "combine."); at(key).slot = (slot++); } } // 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++); + BOOST_FOREACH (value_type& var_slot, *this) { + if (var_slot.second.slot == none) var_slot.second.slot = (slot++); } } -} // gtsam +/* ************************************************************************* */ +FastVector Scatter::getSlotsForKeys( + const FastVector& keys) const { + gttic(getSlotsForKeys); + FastVector slots(keys.size() + 1); + DenseIndex slot = 0; + BOOST_FOREACH (Key key, keys) + slots[slot++] = at(key).slot; + slots.back() = size(); + return slots; +} + +/* ************************************************************************* */ + +} // gtsam diff --git a/gtsam/linear/Scatter.h b/gtsam/linear/Scatter.h index e212c5867..1d6c546b8 100644 --- a/gtsam/linear/Scatter.h +++ b/gtsam/linear/Scatter.h @@ -30,12 +30,11 @@ namespace gtsam { class GaussianFactorGraph; class Ordering; -/** - * One SlotEntry stores the slot index for a variable, as well its dimension. - */ +/// One SlotEntry stores the slot index for a variable, as well its dimension. struct GTSAM_EXPORT SlotEntry { - size_t slot, dimension; - SlotEntry(size_t _slot, size_t _dimension) + DenseIndex slot; + size_t dimension; + SlotEntry(DenseIndex _slot, size_t _dimension) : slot(_slot), dimension(_dimension) {} std::string toString() const; }; @@ -43,14 +42,21 @@ struct GTSAM_EXPORT SlotEntry { /** * 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 - * from - * global variable indices to slot indices in the union of involved variables. - * We also include the dimensionality of the variable. + * 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 { public: Scatter(const GaussianFactorGraph& gfg, boost::optional ordering = boost::none); + + DenseIndex slot(Key key) const { return at(key).slot; } + + /** + * For the subset of keys given, return the slots in the same order, + * terminated by the a RHS slot equal to N, the size of the Scatter + */ + FastVector getSlotsForKeys(const FastVector& keys) const; }; } // \ namespace gtsam diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index ec779cbd4..c469bcada 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -166,8 +166,9 @@ namespace gtsam { DenseIndex N = info->nBlocks() - 1; // First build an array of slots - DenseIndex slotC = scatter.at(this->keys().front()).slot; - DenseIndex slotL = scatter.at(this->keys().back()).slot; + FastVector slots = scatter.getSlotsForKeys(keys_); + DenseIndex slotC = scatter.slot(keys_.front()); + DenseIndex slotL = scatter.slot(keys_.back()); DenseIndex slotB = N; // We perform I += A'*A to the upper triangle From c8cff296fbe24606811b567c65004a8552811e28 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 13 Jun 2015 09:01:13 -0700 Subject: [PATCH 31/47] Don't bother making an array --- gtsam/linear/HessianFactor.cpp | 9 +++------ gtsam/linear/JacobianFactor.cpp | 10 +++------- gtsam/slam/GeneralSFMFactor.h | 1 - 3 files changed, 6 insertions(+), 14 deletions(-) diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index a038a7ff8..c74d53476 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -349,15 +349,12 @@ double HessianFactor::error(const VectorValues& c) const { void HessianFactor::updateHessian(const Scatter& scatter, SymmetricBlockMatrix* info) const { gttic(updateHessian_HessianFactor); - // First build an array of slots - FastVector slots = scatter.getSlotsForKeys(keys_); - // Apply updates to the upper triangle - DenseIndex n = size(); + DenseIndex n = size(), N = info->nBlocks()-1; for (DenseIndex j = 0; j <= n; ++j) { - DenseIndex J = slots[j]; + const DenseIndex J = j==n ? N : scatter.slot(keys_[j]); for (DenseIndex i = 0; i <= j; ++i) { - DenseIndex I = slots[i]; + const DenseIndex I = i==n ? N : scatter.slot(keys_[i]); (*info)(I, J) += info_(i, j); } } diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 3d42db1ca..1e3433268 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -514,20 +514,16 @@ void JacobianFactor::updateHessian(const Scatter& scatter, JacobianFactor whitenedFactor = whiten(); whitenedFactor.updateHessian(scatter, info); } else { - // First build an array of slots - FastVector slots = scatter.getSlotsForKeys(keys_); - // Ab_ is the augmented Jacobian matrix A, and we perform I += A'*A below - DenseIndex n = Ab_.nBlocks() - 1; + DenseIndex n = Ab_.nBlocks() - 1, N = info->nBlocks() - 1; // Apply updates to the upper triangle // Loop over blocks of A, including RHS with j==n - // BOOST_FOREACH(DenseIndex J, slots) for (DenseIndex j = 0; j <= n; ++j) { - const DenseIndex J = slots[j]; + const DenseIndex J = j==n ? N : scatter.slot(keys_[j]); // Fill off-diagonal blocks with Ai'*Aj for (DenseIndex i = 0; i < j; ++i) { - const DenseIndex I = slots[i]; + const DenseIndex I = scatter.slot(keys_[i]); (*info)(I, J).knownOffDiagonal() += Ab_(i).transpose() * Ab_(j); } // Fill diagonal block with Aj'*Aj diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index c469bcada..4425d63d0 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -166,7 +166,6 @@ namespace gtsam { DenseIndex N = info->nBlocks() - 1; // First build an array of slots - FastVector slots = scatter.getSlotsForKeys(keys_); DenseIndex slotC = scatter.slot(keys_.front()); DenseIndex slotL = scatter.slot(keys_.back()); DenseIndex slotB = N; From 08f30966dd4c4aaef6dd2c9f2f276b6ccfd3a3fe Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 13 Jun 2015 11:03:12 -0700 Subject: [PATCH 32/47] Got rid of obsolete getSlots method --- gtsam/linear/Scatter.cpp | 12 ------------ gtsam/linear/Scatter.h | 11 +++++------ 2 files changed, 5 insertions(+), 18 deletions(-) diff --git a/gtsam/linear/Scatter.cpp b/gtsam/linear/Scatter.cpp index 21d20c14c..2602e08ba 100644 --- a/gtsam/linear/Scatter.cpp +++ b/gtsam/linear/Scatter.cpp @@ -75,18 +75,6 @@ Scatter::Scatter(const GaussianFactorGraph& gfg, } } -/* ************************************************************************* */ -FastVector Scatter::getSlotsForKeys( - const FastVector& keys) const { - gttic(getSlotsForKeys); - FastVector slots(keys.size() + 1); - DenseIndex slot = 0; - BOOST_FOREACH (Key key, keys) - slots[slot++] = at(key).slot; - slots.back() = size(); - return slots; -} - /* ************************************************************************* */ } // gtsam diff --git a/gtsam/linear/Scatter.h b/gtsam/linear/Scatter.h index 1d6c546b8..e1df2d658 100644 --- a/gtsam/linear/Scatter.h +++ b/gtsam/linear/Scatter.h @@ -30,7 +30,7 @@ namespace gtsam { class GaussianFactorGraph; class Ordering; -/// One SlotEntry stores the slot index for a variable, as well its dimension. +/// One SlotEntry stores the slot index for a variable, as well its dim. struct GTSAM_EXPORT SlotEntry { DenseIndex slot; size_t dimension; @@ -47,16 +47,15 @@ struct GTSAM_EXPORT SlotEntry { */ class Scatter : public FastMap { 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; } - /** - * For the subset of keys given, return the slots in the same order, - * terminated by the a RHS slot equal to N, the size of the Scatter - */ - FastVector getSlotsForKeys(const FastVector& keys) const; + /// Get the dimension corresponding to the given key + DenseIndex dim(Key key) const { return at(key).dimension; } }; } // \ namespace gtsam From f6575323d6dbd26bc8ac5ca23e57db65df62224a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 13 Jun 2015 12:06:13 -0700 Subject: [PATCH 33/47] Sidestep Scatter altogether and just use HessianFactor keys_. In theory, n^3 lookup cost, but in practice (as keys is contiguous memory) just as fast as map. --- gtsam/linear/GaussianFactor.h | 10 ++++++++-- gtsam/linear/HessianFactor.cpp | 8 ++++---- gtsam/linear/HessianFactor.h | 2 +- gtsam/linear/JacobianFactor.cpp | 8 ++++---- gtsam/linear/JacobianFactor.h | 2 +- gtsam/linear/tests/testHessianFactor.cpp | 18 +++++++++++++++--- gtsam/slam/GeneralSFMFactor.h | 13 +++++-------- gtsam/slam/RegularImplicitSchurFactor.h | 2 +- 8 files changed, 39 insertions(+), 24 deletions(-) diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index bc14cc670..7f9c5ea3c 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -126,7 +126,7 @@ namespace gtsam { * @param scatter A mapping from variable index to slot index in this HessianFactor * @param info The information matrix to be updated */ - virtual void updateHessian(const Scatter& scatter, + virtual void updateHessian(const FastVector& keys, SymmetricBlockMatrix* info) const = 0; /// y += alpha * A'*A*x @@ -141,6 +141,12 @@ namespace gtsam { /// Gradient wrt a key at any values virtual Vector gradient(Key key, const VectorValues& x) const = 0; + // Determine position of a given key + template + static DenseIndex Slot(const CONTAINER& keys, Key key) { + return std::find(keys.begin(), keys.end(), key) - keys.begin(); + } + private: /** Serialization function */ friend class boost::serialization::access; @@ -150,7 +156,7 @@ namespace gtsam { } }; // GaussianFactor - + /// traits template<> struct traits : public Testable { diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index c74d53476..c071f8daa 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -248,7 +248,7 @@ HessianFactor::HessianFactor(const GaussianFactorGraph& factors, gttic(update); BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) if(factor) - factor->updateHessian(*scatter, &info_); + factor->updateHessian(keys_, &info_); gttoc(update); } @@ -346,15 +346,15 @@ double HessianFactor::error(const VectorValues& c) const { } /* ************************************************************************* */ -void HessianFactor::updateHessian(const Scatter& scatter, +void HessianFactor::updateHessian(const FastVector& infoKeys, SymmetricBlockMatrix* info) const { gttic(updateHessian_HessianFactor); // Apply updates to the upper triangle DenseIndex n = size(), N = info->nBlocks()-1; for (DenseIndex j = 0; j <= n; ++j) { - const DenseIndex J = j==n ? N : scatter.slot(keys_[j]); + const DenseIndex J = (j==n) ? N : Slot(infoKeys, keys_[j]); for (DenseIndex i = 0; i <= j; ++i) { - const DenseIndex I = i==n ? N : scatter.slot(keys_[i]); + const DenseIndex I = (i==n) ? N : Slot(infoKeys, keys_[i]); (*info)(I, J) += info_(i, j); } } diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index 160d05b15..b74d557ea 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -344,7 +344,7 @@ namespace gtsam { * @param scatter A mapping from variable index to slot index in this HessianFactor * @param info The information matrix to be updated */ - void updateHessian(const Scatter& scatter, SymmetricBlockMatrix* info) const; + void updateHessian(const FastVector& keys, SymmetricBlockMatrix* info) const; /** y += alpha * A'*A*x */ void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const; diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 1e3433268..5b90d913d 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -498,7 +498,7 @@ map JacobianFactor::hessianBlockDiagonal() const { } /* ************************************************************************* */ -void JacobianFactor::updateHessian(const Scatter& scatter, +void JacobianFactor::updateHessian(const FastVector& infoKeys, SymmetricBlockMatrix* info) const { gttic(updateHessian_JacobianFactor); @@ -512,7 +512,7 @@ void JacobianFactor::updateHessian(const Scatter& scatter, "JacobianFactor::updateHessian: cannot update information with " "constrained noise model"); JacobianFactor whitenedFactor = whiten(); - whitenedFactor.updateHessian(scatter, info); + whitenedFactor.updateHessian(infoKeys, info); } else { // Ab_ is the augmented Jacobian matrix A, and we perform I += A'*A below DenseIndex n = Ab_.nBlocks() - 1, N = info->nBlocks() - 1; @@ -520,10 +520,10 @@ void JacobianFactor::updateHessian(const Scatter& scatter, // Apply updates to the upper triangle // Loop over blocks of A, including RHS with j==n for (DenseIndex j = 0; j <= n; ++j) { - const DenseIndex J = j==n ? N : scatter.slot(keys_[j]); + const DenseIndex J = (j==n) ? N : Slot(infoKeys, keys_[j]); // Fill off-diagonal blocks with Ai'*Aj for (DenseIndex i = 0; i < j; ++i) { - const DenseIndex I = scatter.slot(keys_[i]); + const DenseIndex I = Slot(infoKeys, keys_[i]); (*info)(I, J).knownOffDiagonal() += Ab_(i).transpose() * Ab_(j); } // Fill diagonal block with Aj'*Aj diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 00a9b5488..ff7310d9c 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -278,7 +278,7 @@ namespace gtsam { * @param scatter A mapping from variable index to slot index in this HessianFactor * @param info The information matrix to be updated */ - void updateHessian(const Scatter& scatter, SymmetricBlockMatrix* info) const; + void updateHessian(const FastVector& keys, SymmetricBlockMatrix* info) const; /** Return A*x */ Vector operator*(const VectorValues& x) const; diff --git a/gtsam/linear/tests/testHessianFactor.cpp b/gtsam/linear/tests/testHessianFactor.cpp index 557ba3f36..96e61aa33 100644 --- a/gtsam/linear/tests/testHessianFactor.cpp +++ b/gtsam/linear/tests/testHessianFactor.cpp @@ -38,6 +38,16 @@ using namespace gtsam; const double tol = 1e-5; +/* ************************************************************************* */ +TEST(HessianFactor, Slot) +{ + FastVector keys = list_of(2)(4)(1); + EXPECT_LONGS_EQUAL(0, GaussianFactor::Slot(keys,2)); + EXPECT_LONGS_EQUAL(1, GaussianFactor::Slot(keys,4)); + EXPECT_LONGS_EQUAL(2, GaussianFactor::Slot(keys,1)); + EXPECT_LONGS_EQUAL(3, GaussianFactor::Slot(keys,5)); // does not exist +} + /* ************************************************************************* */ TEST(HessianFactor, emptyConstructor) { @@ -302,15 +312,17 @@ TEST(HessianFactor, CombineAndEliminate) gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true)); gfg.add(1, A21, b2, noiseModel::Diagonal::Sigmas(s2, true)); - Matrix zero3x3 = zeros(3,3); - Matrix A0 = gtsam::stack(3, &A10, &zero3x3, &zero3x3); - Matrix A1 = gtsam::stack(3, &A11, &A01, &A21); + Matrix93 A0; A0 << A10, Z_3x3, Z_3x3; + Matrix93 A1; A1 << A11, A01, A21; Vector9 b; b << b1, b0, b2; Vector9 sigmas; sigmas << s1, s0, s2; // create a full, uneliminated version of the factor JacobianFactor expectedFactor(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true)); + // Make sure combining works + EXPECT(assert_equal(HessianFactor(expectedFactor), HessianFactor(gfg), 1e-6)); + // perform elimination on jacobian GaussianConditional::shared_ptr expectedConditional; JacobianFactor::shared_ptr expectedRemainingFactor; diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index 4425d63d0..d969f08a2 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -149,7 +149,7 @@ namespace gtsam { : JacobianFactor(i1, A1, i2, A2, b, model), AC_(A1), AL_(A2), b_(b) {} // Fixed-size matrix update - void updateHessian(const Scatter& scatter, SymmetricBlockMatrix* info) const { + void updateHessian(const FastVector& infoKeys, SymmetricBlockMatrix* info) const { gttic(updateHessian_LinearizedFactor); // Whiten the factor if it has a noise model @@ -160,15 +160,12 @@ namespace gtsam { "JacobianFactor::updateHessian: cannot update information with " "constrained noise model"); JacobianFactor whitenedFactor = whiten(); - whitenedFactor.updateHessian(scatter, info); + whitenedFactor.updateHessian(infoKeys, info); } else { - // N is number of variables in information matrix - DenseIndex N = info->nBlocks() - 1; - // First build an array of slots - DenseIndex slotC = scatter.slot(keys_.front()); - DenseIndex slotL = scatter.slot(keys_.back()); - DenseIndex slotB = N; + DenseIndex slotC = Slot(infoKeys, keys_.front()); + DenseIndex slotL = Slot(infoKeys, keys_.back()); + DenseIndex slotB = info->nBlocks() - 1; // We perform I += A'*A to the upper triangle (*info)(slotC, slotC).knownOffDiagonal() += AC_.transpose() * AC_; diff --git a/gtsam/slam/RegularImplicitSchurFactor.h b/gtsam/slam/RegularImplicitSchurFactor.h index 87d78911d..71944c670 100644 --- a/gtsam/slam/RegularImplicitSchurFactor.h +++ b/gtsam/slam/RegularImplicitSchurFactor.h @@ -115,7 +115,7 @@ public: return D; } - virtual void updateHessian(const Scatter& scatter, + virtual void updateHessian(const FastVector& keys, SymmetricBlockMatrix* info) const { throw std::runtime_error( "RegularImplicitSchurFactor::updateHessian non implemented"); From d0775faebaa732f2565484f41d7fe24374dfc959 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 13 Jun 2015 12:26:10 -0700 Subject: [PATCH 34/47] Save slots to bring cost down from O(n^3) to O(n^2) - again, in theory. In practice, it did seem to help for larger HessianFactors (as expected). --- gtsam/linear/HessianFactor.cpp | 8 +++++--- gtsam/linear/JacobianFactor.cpp | 6 ++++-- 2 files changed, 9 insertions(+), 5 deletions(-) diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index c071f8daa..7f3929488 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -350,11 +350,13 @@ void HessianFactor::updateHessian(const FastVector& infoKeys, SymmetricBlockMatrix* info) const { gttic(updateHessian_HessianFactor); // Apply updates to the upper triangle - DenseIndex n = size(), N = info->nBlocks()-1; + DenseIndex n = size(), N = info->nBlocks() - 1; + vector slots(n + 1); for (DenseIndex j = 0; j <= n; ++j) { - const DenseIndex J = (j==n) ? N : Slot(infoKeys, keys_[j]); + const DenseIndex J = (j == n) ? N : Slot(infoKeys, keys_[j]); + slots[j] = J; for (DenseIndex i = 0; i <= j; ++i) { - const DenseIndex I = (i==n) ? N : Slot(infoKeys, keys_[i]); + const DenseIndex I = slots[i]; // because i<=j, slots[i] is valid. (*info)(I, J) += info_(i, j); } } diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 5b90d913d..8214294b2 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -519,11 +519,13 @@ void JacobianFactor::updateHessian(const FastVector& infoKeys, // Apply updates to the upper triangle // Loop over blocks of A, including RHS with j==n + vector slots(n+1); for (DenseIndex j = 0; j <= n; ++j) { - const DenseIndex J = (j==n) ? N : Slot(infoKeys, keys_[j]); + const DenseIndex J = (j == n) ? N : Slot(infoKeys, keys_[j]); + slots[j] = J; // Fill off-diagonal blocks with Ai'*Aj for (DenseIndex i = 0; i < j; ++i) { - const DenseIndex I = Slot(infoKeys, keys_[i]); + const DenseIndex I = slots[i]; // because i Date: Sat, 13 Jun 2015 20:19:44 -0700 Subject: [PATCH 35/47] Reverted back to vector to avoid troubles w TBB --- gtsam/inference/Ordering.h | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/gtsam/inference/Ordering.h b/gtsam/inference/Ordering.h index 3e7828944..9de3fb66a 100644 --- a/gtsam/inference/Ordering.h +++ b/gtsam/inference/Ordering.h @@ -23,7 +23,6 @@ #include #include #include -#include #include #include @@ -31,9 +30,9 @@ namespace gtsam { - class Ordering : public FastVector { + class Ordering : public std::vector { protected: - typedef FastVector Base; + typedef std::vector Base; public: From 4909fef21a5a2f98e643e6315953f0a996804707 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 13 Jun 2015 20:20:33 -0700 Subject: [PATCH 36/47] Fixed issue --- gtsam/slam/GeneralSFMFactor.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index d969f08a2..0ad635d0e 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -168,12 +168,12 @@ namespace gtsam { DenseIndex slotB = info->nBlocks() - 1; // We perform I += A'*A to the upper triangle - (*info)(slotC, slotC).knownOffDiagonal() += AC_.transpose() * AC_; + (*info)(slotC, slotC).selfadjointView().rankUpdate(AC_.transpose()); (*info)(slotC, slotL).knownOffDiagonal() += AC_.transpose() * AL_; (*info)(slotC, slotB).knownOffDiagonal() += AC_.transpose() * b_; - (*info)(slotL, slotL).knownOffDiagonal() += AL_.transpose() * AL_; + (*info)(slotL, slotL).selfadjointView().rankUpdate(AL_.transpose()); (*info)(slotL, slotB).knownOffDiagonal() += AL_.transpose() * b_; - (*info)(slotB, slotB).knownOffDiagonal() += b_.transpose() * b_; + (*info)(slotB, slotB).selfadjointView().rankUpdate(b_.transpose()); } } }; From 2c99f68ed7379b2896554be0bfdbce9e3dc31cb7 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 14 Jun 2015 10:56:22 -0700 Subject: [PATCH 37/47] Some formatting/cleanup before fixing bug --- gtsam/slam/GeneralSFMFactor.h | 532 +++++++++--------- gtsam/slam/tests/testGeneralSFMFactor.cpp | 370 ++++++------ .../testGeneralSFMFactor_Cal3Bundler.cpp | 288 +++++----- 3 files changed, 607 insertions(+), 583 deletions(-) diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index 0ad635d0e..fbf64de6c 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -51,295 +51,295 @@ class access; namespace gtsam { - /** - * Non-linear factor for a constraint derived from a 2D measurement. - * The calibration is unknown here compared to GenericProjectionFactor - * @addtogroup SLAM - */ - template - class GeneralSFMFactor: public NoiseModelFactor2 { +/** + * Non-linear factor for a constraint derived from a 2D measurement. + * The calibration is unknown here compared to GenericProjectionFactor + * @addtogroup SLAM + */ +template +class GeneralSFMFactor: public NoiseModelFactor2 { - GTSAM_CONCEPT_MANIFOLD_TYPE(CAMERA) - GTSAM_CONCEPT_MANIFOLD_TYPE(LANDMARK) + GTSAM_CONCEPT_MANIFOLD_TYPE(CAMERA); + GTSAM_CONCEPT_MANIFOLD_TYPE(LANDMARK); - static const int DimC = FixedDimension::value; - static const int DimL = FixedDimension::value; - typedef Eigen::Matrix JacobianC; - typedef Eigen::Matrix JacobianL; + static const int DimC = FixedDimension::value; + static const int DimL = FixedDimension::value; + typedef Eigen::Matrix JacobianC; + typedef Eigen::Matrix JacobianL; - protected: +protected: - Point2 measured_; ///< the 2D measurement + Point2 measured_; ///< the 2D measurement - public: +public: - typedef GeneralSFMFactor This; ///< typedef for this object - typedef NoiseModelFactor2 Base; ///< typedef for the base class + typedef GeneralSFMFactor This;///< typedef for this object + typedef NoiseModelFactor2 Base;///< typedef for the base class - // shorthand for a smart pointer to a factor - typedef boost::shared_ptr shared_ptr; - - /** - * Constructor - * @param measured is the 2 dimensional location of point in image (the measurement) - * @param model is the standard deviation of the measurements - * @param cameraKey is the index of the camera - * @param landmarkKey is the index of the landmark - */ - GeneralSFMFactor(const Point2& measured, const SharedNoiseModel& model, Key cameraKey, Key landmarkKey) : - Base(model, cameraKey, landmarkKey), measured_(measured) {} - - GeneralSFMFactor():measured_(0.0,0.0) {} ///< default constructor - GeneralSFMFactor(const Point2 & p):measured_(p) {} ///< constructor that takes a Point2 - GeneralSFMFactor(double x, double y):measured_(x,y) {} ///< constructor that takes doubles x,y to make a Point2 - - virtual ~GeneralSFMFactor() {} ///< destructor - - /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { - return boost::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); } - - /** - * print - * @param s optional string naming the factor - * @param keyFormatter optional formatter for printing out Symbols - */ - void print(const std::string& s = "SFMFactor", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - Base::print(s, keyFormatter); - measured_.print(s + ".z"); - } - - /** - * equals - */ - bool equals(const NonlinearFactor &p, double tol = 1e-9) const { - const This* e = dynamic_cast(&p); - return e && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol) ; - } - - /** h(x)-z */ - Vector evaluateError(const CAMERA& camera, const LANDMARK& point, - boost::optional H1=boost::none, boost::optional H2=boost::none) const { - try { - Point2 reprojError(camera.project2(point,H1,H2) - measured_); - return reprojError.vector(); - } - catch( CheiralityException& e) { - if (H1) *H1 = JacobianC::Zero(); - if (H2) *H2 = JacobianL::Zero(); - // TODO warn if verbose output asked for - return zero(2); - } - } - - class LinearizedFactor : public JacobianFactor { - // Fixed size matrices - // TODO: implement generic BinaryJacobianFactor next to - // JacobianFactor - JacobianC AC_; - JacobianL AL_; - Vector2 b_; - - public: - /// Constructor - LinearizedFactor(Key i1, const JacobianC& A1, Key i2, const JacobianL& A2, - const Vector2& b, - const SharedDiagonal& model = SharedDiagonal()) - : JacobianFactor(i1, A1, i2, A2, b, model), AC_(A1), AL_(A2), b_(b) {} - - // Fixed-size matrix update - void updateHessian(const FastVector& infoKeys, SymmetricBlockMatrix* info) const { - gttic(updateHessian_LinearizedFactor); - - // Whiten the factor if it has a noise model - const SharedDiagonal& model = get_model(); - if (model && !model->isUnit()) { - if (model->isConstrained()) - throw std::invalid_argument( - "JacobianFactor::updateHessian: cannot update information with " - "constrained noise model"); - JacobianFactor whitenedFactor = whiten(); - whitenedFactor.updateHessian(infoKeys, info); - } else { - // First build an array of slots - DenseIndex slotC = Slot(infoKeys, keys_.front()); - DenseIndex slotL = Slot(infoKeys, keys_.back()); - DenseIndex slotB = info->nBlocks() - 1; - - // We perform I += A'*A to the upper triangle - (*info)(slotC, slotC).selfadjointView().rankUpdate(AC_.transpose()); - (*info)(slotC, slotL).knownOffDiagonal() += AC_.transpose() * AL_; - (*info)(slotC, slotB).knownOffDiagonal() += AC_.transpose() * b_; - (*info)(slotL, slotL).selfadjointView().rankUpdate(AL_.transpose()); - (*info)(slotL, slotB).knownOffDiagonal() += AL_.transpose() * b_; - (*info)(slotB, slotB).selfadjointView().rankUpdate(b_.transpose()); - } - } - }; - - /// Linearize using fixed-size matrices - boost::shared_ptr linearize(const Values& values) const { - // Only linearize if the factor is active - if (!this->active(values)) return boost::shared_ptr(); - - const Key key1 = this->key1(), key2 = this->key2(); - JacobianC H1; - JacobianL H2; - Vector2 b; - try { - const CAMERA& camera = values.at(key1); - const LANDMARK& point = values.at(key2); - Point2 reprojError(camera.project2(point, H1, H2) - measured()); - b = -reprojError.vector(); - } catch (CheiralityException& e) { - H1.setZero(); - H2.setZero(); - b.setZero(); - // TODO warn if verbose output asked for - } - - // Whiten the system if needed - const SharedNoiseModel& noiseModel = this->get_noiseModel(); - if (noiseModel && !noiseModel->isUnit()) { - // TODO: implement WhitenSystem for fixed size matrices and include - // above - H1 = noiseModel->Whiten(H1); - H2 = noiseModel->Whiten(H2); - b = noiseModel->Whiten(b); - } - - if (noiseModel && noiseModel->isConstrained()) { - using noiseModel::Constrained; - return boost::make_shared( - key1, H1, key2, H2, b, - boost::static_pointer_cast(noiseModel)->unit()); - } else { - return boost::make_shared(key1, H1, key2, H2, b); - } - } - - /** return the measured */ - inline const Point2 measured() const { - return measured_; - } - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int /*version*/) { - ar & boost::serialization::make_nvp("NoiseModelFactor2", - boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(measured_); - } - }; - - template - struct traits > : Testable< - GeneralSFMFactor > { - }; + // shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; /** - * Non-linear factor for a constraint derived from a 2D measurement. - * Compared to GeneralSFMFactor, it is a ternary-factor because the calibration is isolated from camera.. + * Constructor + * @param measured is the 2 dimensional location of point in image (the measurement) + * @param model is the standard deviation of the measurements + * @param cameraKey is the index of the camera + * @param landmarkKey is the index of the landmark */ - template - class GeneralSFMFactor2: public NoiseModelFactor3 { + GeneralSFMFactor(const Point2& measured, const SharedNoiseModel& model, Key cameraKey, Key landmarkKey) : + Base(model, cameraKey, landmarkKey), measured_(measured) {} - GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION) - static const int DimK = FixedDimension::value; + GeneralSFMFactor():measured_(0.0,0.0) {} ///< default constructor + GeneralSFMFactor(const Point2 & p):measured_(p) {} ///< constructor that takes a Point2 + GeneralSFMFactor(double x, double y):measured_(x,y) {} ///< constructor that takes doubles x,y to make a Point2 - protected: + virtual ~GeneralSFMFactor() {} ///< destructor - Point2 measured_; ///< the 2D measurement + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this)));} - public: + /** + * print + * @param s optional string naming the factor + * @param keyFormatter optional formatter for printing out Symbols + */ + void print(const std::string& s = "SFMFactor", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + Base::print(s, keyFormatter); + measured_.print(s + ".z"); + } - typedef GeneralSFMFactor2 This; - typedef PinholeCamera Camera; ///< typedef for camera type - typedef NoiseModelFactor3 Base; ///< typedef for the base class + /** + * equals + */ + bool equals(const NonlinearFactor &p, double tol = 1e-9) const { + const This* e = dynamic_cast(&p); + return e && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol); + } - // shorthand for a smart pointer to a factor - typedef boost::shared_ptr shared_ptr; - - /** - * Constructor - * @param measured is the 2 dimensional location of point in image (the measurement) - * @param model is the standard deviation of the measurements - * @param poseKey is the index of the camera - * @param landmarkKey is the index of the landmark - * @param calibKey is the index of the calibration - */ - GeneralSFMFactor2(const Point2& measured, const SharedNoiseModel& model, Key poseKey, Key landmarkKey, Key calibKey) : - Base(model, poseKey, landmarkKey, calibKey), measured_(measured) {} - GeneralSFMFactor2():measured_(0.0,0.0) {} ///< default constructor - - virtual ~GeneralSFMFactor2() {} ///< destructor - - /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { - return boost::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); } - - /** - * print - * @param s optional string naming the factor - * @param keyFormatter optional formatter useful for printing Symbols - */ - void print(const std::string& s = "SFMFactor2", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - Base::print(s, keyFormatter); - measured_.print(s + ".z"); + /** h(x)-z */ + Vector evaluateError(const CAMERA& camera, const LANDMARK& point, + boost::optional H1=boost::none, boost::optional H2=boost::none) const { + try { + Point2 reprojError(camera.project2(point,H1,H2) - measured_); + return reprojError.vector(); } - - /** - * equals - */ - bool equals(const NonlinearFactor &p, double tol = 1e-9) const { - const This* e = dynamic_cast(&p); - return e && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol) ; - } - - /** h(x)-z */ - Vector evaluateError(const Pose3& pose3, const Point3& point, const CALIBRATION &calib, - boost::optional H1=boost::none, - boost::optional H2=boost::none, - boost::optional H3=boost::none) const - { - try { - Camera camera(pose3,calib); - Point2 reprojError(camera.project(point, H1, H2, H3) - measured_); - return reprojError.vector(); - } - catch( CheiralityException& e) { - if (H1) *H1 = zeros(2, 6); - if (H2) *H2 = zeros(2, 3); - if (H3) *H3 = zeros(2, DimK); - std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) - << " behind Camera " << DefaultKeyFormatter(this->key1()) << std::endl; - } + catch( CheiralityException& e) { + if (H1) *H1 = JacobianC::Zero(); + if (H2) *H2 = JacobianL::Zero(); + // TODO warn if verbose output asked for return zero(2); } + } - /** return the measured */ - inline const Point2 measured() const { - return measured_; - } + class LinearizedFactor : public JacobianFactor { + // Fixed size matrices + // TODO: implement generic BinaryJacobianFactor next to + // JacobianFactor + JacobianC AC_; + JacobianL AL_; + Vector2 b_; - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int /*version*/) { - ar & boost::serialization::make_nvp("NoiseModelFactor3", - boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(measured_); + public: + /// Constructor + LinearizedFactor(Key i1, const JacobianC& A1, Key i2, const JacobianL& A2, + const Vector2& b, + const SharedDiagonal& model = SharedDiagonal()) + : JacobianFactor(i1, A1, i2, A2, b, model), AC_(A1), AL_(A2), b_(b) {} + + // Fixed-size matrix update + void updateHessian(const FastVector& infoKeys, SymmetricBlockMatrix* info) const { + gttic(updateHessian_LinearizedFactor); + + // Whiten the factor if it has a noise model + const SharedDiagonal& model = get_model(); + if (model && !model->isUnit()) { + if (model->isConstrained()) + throw std::invalid_argument( + "JacobianFactor::updateHessian: cannot update information with " + "constrained noise model"); + JacobianFactor whitenedFactor = whiten(); + whitenedFactor.updateHessian(infoKeys, info); + } else { + // First build an array of slots + DenseIndex slotC = Slot(infoKeys, keys_.front()); + DenseIndex slotL = Slot(infoKeys, keys_.back()); + DenseIndex slotB = info->nBlocks() - 1; + + // We perform I += A'*A to the upper triangle + (*info)(slotC, slotC).selfadjointView().rankUpdate(AC_.transpose()); + (*info)(slotC, slotL).knownOffDiagonal() += AC_.transpose() * AL_; + (*info)(slotC, slotB).knownOffDiagonal() += AC_.transpose() * b_; + (*info)(slotL, slotL).selfadjointView().rankUpdate(AL_.transpose()); + (*info)(slotL, slotB).knownOffDiagonal() += AL_.transpose() * b_; + (*info)(slotB, slotB).selfadjointView().rankUpdate(b_.transpose()); + } } }; - template - struct traits > : Testable< - GeneralSFMFactor2 > { - }; + /// Linearize using fixed-size matrices + boost::shared_ptr linearize(const Values& values) const { + // Only linearize if the factor is active + if (!this->active(values)) return boost::shared_ptr(); + + const Key key1 = this->key1(), key2 = this->key2(); + JacobianC H1; + JacobianL H2; + Vector2 b; + try { + const CAMERA& camera = values.at(key1); + const LANDMARK& point = values.at(key2); + Point2 reprojError(camera.project2(point, H1, H2) - measured()); + b = -reprojError.vector(); + } catch (CheiralityException& e) { + H1.setZero(); + H2.setZero(); + b.setZero(); + // TODO warn if verbose output asked for + } + + // Whiten the system if needed + const SharedNoiseModel& noiseModel = this->get_noiseModel(); + if (noiseModel && !noiseModel->isUnit()) { + // TODO: implement WhitenSystem for fixed size matrices and include + // above + H1 = noiseModel->Whiten(H1); + H2 = noiseModel->Whiten(H2); + b = noiseModel->Whiten(b); + } + + if (noiseModel && noiseModel->isConstrained()) { + using noiseModel::Constrained; + return boost::make_shared( + key1, H1, key2, H2, b, + boost::static_pointer_cast(noiseModel)->unit()); + } else { + return boost::make_shared(key1, H1, key2, H2, b); + } + } + + /** return the measured */ + inline const Point2 measured() const { + return measured_; + } + +private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int /*version*/) { + ar & boost::serialization::make_nvp("NoiseModelFactor2", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(measured_); + } +}; + +template +struct traits > : Testable< + GeneralSFMFactor > { +}; + +/** + * Non-linear factor for a constraint derived from a 2D measurement. + * Compared to GeneralSFMFactor, it is a ternary-factor because the calibration is isolated from camera.. + */ +template +class GeneralSFMFactor2: public NoiseModelFactor3 { + + GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION); + static const int DimK = FixedDimension::value; + +protected: + + Point2 measured_; ///< the 2D measurement + +public: + + typedef GeneralSFMFactor2 This; + typedef PinholeCamera Camera;///< typedef for camera type + typedef NoiseModelFactor3 Base;///< typedef for the base class + + // shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; + + /** + * Constructor + * @param measured is the 2 dimensional location of point in image (the measurement) + * @param model is the standard deviation of the measurements + * @param poseKey is the index of the camera + * @param landmarkKey is the index of the landmark + * @param calibKey is the index of the calibration + */ + GeneralSFMFactor2(const Point2& measured, const SharedNoiseModel& model, Key poseKey, Key landmarkKey, Key calibKey) : + Base(model, poseKey, landmarkKey, calibKey), measured_(measured) {} + GeneralSFMFactor2():measured_(0.0,0.0) {} ///< default constructor + + virtual ~GeneralSFMFactor2() {} ///< destructor + + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this)));} + + /** + * print + * @param s optional string naming the factor + * @param keyFormatter optional formatter useful for printing Symbols + */ + void print(const std::string& s = "SFMFactor2", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + Base::print(s, keyFormatter); + measured_.print(s + ".z"); + } + + /** + * equals + */ + bool equals(const NonlinearFactor &p, double tol = 1e-9) const { + const This* e = dynamic_cast(&p); + return e && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol); + } + + /** h(x)-z */ + Vector evaluateError(const Pose3& pose3, const Point3& point, const CALIBRATION &calib, + boost::optional H1=boost::none, + boost::optional H2=boost::none, + boost::optional H3=boost::none) const + { + try { + Camera camera(pose3,calib); + Point2 reprojError(camera.project(point, H1, H2, H3) - measured_); + return reprojError.vector(); + } + catch( CheiralityException& e) { + if (H1) *H1 = zeros(2, 6); + if (H2) *H2 = zeros(2, 3); + if (H3) *H3 = zeros(2, DimK); + std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) + << " behind Camera " << DefaultKeyFormatter(this->key1()) << std::endl; + } + return zero(2); + } + + /** return the measured */ + inline const Point2 measured() const { + return measured_; + } + +private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int /*version*/) { + ar & boost::serialization::make_nvp("NoiseModelFactor3", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(measured_); + } +}; + +template +struct traits > : Testable< + GeneralSFMFactor2 > { +}; } //namespace diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index a83db3f1d..7da6cdbdf 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -49,7 +49,8 @@ typedef NonlinearEquality Point3Constraint; class Graph: public NonlinearFactorGraph { public: - void addMeasurement(int i, int j, const Point2& z, const SharedNoiseModel& model) { + void addMeasurement(int i, int j, const Point2& z, + const SharedNoiseModel& model) { push_back(boost::make_shared(z, model, X(i), L(j))); } @@ -65,98 +66,100 @@ public: }; -static double getGaussian() -{ - double S,V1,V2; - // Use Box-Muller method to create gauss noise from uniform noise - do - { - double U1 = rand() / (double)(RAND_MAX); - double U2 = rand() / (double)(RAND_MAX); - V1 = 2 * U1 - 1; /* V1=[-1,1] */ - V2 = 2 * U2 - 1; /* V2=[-1,1] */ - S = V1 * V1 + V2 * V2; - } while(S>=1); - return sqrt(-2.0f * (double)log(S) / S) * V1; +static double getGaussian() { + double S, V1, V2; + // Use Box-Muller method to create gauss noise from uniform noise + do { + double U1 = rand() / (double) (RAND_MAX); + double U2 = rand() / (double) (RAND_MAX); + V1 = 2 * U1 - 1; /* V1=[-1,1] */ + V2 = 2 * U2 - 1; /* V2=[-1,1] */ + S = V1 * V1 + V2 * V2; + } while (S >= 1); + return sqrt(-2.f * (double) log(S) / S) * V1; } static const SharedNoiseModel sigma1(noiseModel::Unit::Create(2)); /* ************************************************************************* */ -TEST( GeneralSFMFactor, equals ) -{ +TEST( GeneralSFMFactor, equals ) { // Create two identical factors and make sure they're equal - Point2 z(323.,240.); - const Symbol cameraFrameNumber('x',1), landmarkNumber('l',1); + Point2 z(323., 240.); + const Symbol cameraFrameNumber('x', 1), landmarkNumber('l', 1); const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); - boost::shared_ptr - factor1(new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); + boost::shared_ptr factor1( + new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); - boost::shared_ptr - factor2(new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); + boost::shared_ptr factor2( + new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); EXPECT(assert_equal(*factor1, *factor2)); } /* ************************************************************************* */ TEST( GeneralSFMFactor, error ) { - Point2 z(3.,0.); + Point2 z(3., 0.); const SharedNoiseModel sigma(noiseModel::Unit::Create(2)); Projection factor(z, sigma, X(1), L(1)); // For the following configuration, the factor predicts 320,240 Values values; Rot3 R; - Point3 t1(0,0,-6); - Pose3 x1(R,t1); + Point3 t1(0, 0, -6); + Pose3 x1(R, t1); values.insert(X(1), GeneralCamera(x1)); - Point3 l1; values.insert(L(1), l1); - EXPECT(assert_equal(((Vector) Vector2(-3.0, 0.0)), factor.unwhitenedError(values))); + Point3 l1; + values.insert(L(1), l1); + EXPECT( + assert_equal(((Vector ) Vector2(-3., 0.)), + factor.unwhitenedError(values))); } -static const double baseline = 5.0 ; +static const double baseline = 5.; /* ************************************************************************* */ static vector genPoint3() { const double z = 5; - vector landmarks ; - landmarks.push_back(Point3 (-1.0,-1.0, z)); - landmarks.push_back(Point3 (-1.0, 1.0, z)); - landmarks.push_back(Point3 ( 1.0, 1.0, z)); - landmarks.push_back(Point3 ( 1.0,-1.0, z)); - landmarks.push_back(Point3 (-1.5,-1.5, 1.5*z)); - landmarks.push_back(Point3 (-1.5, 1.5, 1.5*z)); - landmarks.push_back(Point3 ( 1.5, 1.5, 1.5*z)); - landmarks.push_back(Point3 ( 1.5,-1.5, 1.5*z)); - landmarks.push_back(Point3 (-2.0,-2.0, 2*z)); - landmarks.push_back(Point3 (-2.0, 2.0, 2*z)); - landmarks.push_back(Point3 ( 2.0, 2.0, 2*z)); - landmarks.push_back(Point3 ( 2.0,-2.0, 2*z)); - return landmarks ; + vector landmarks; + landmarks.push_back(Point3(-1., -1., z)); + landmarks.push_back(Point3(-1., 1., z)); + landmarks.push_back(Point3(1., 1., z)); + landmarks.push_back(Point3(1., -1., z)); + landmarks.push_back(Point3(-1.5, -1.5, 1.5 * z)); + landmarks.push_back(Point3(-1.5, 1.5, 1.5 * z)); + landmarks.push_back(Point3(1.5, 1.5, 1.5 * z)); + landmarks.push_back(Point3(1.5, -1.5, 1.5 * z)); + landmarks.push_back(Point3(-2., -2., 2 * z)); + landmarks.push_back(Point3(-2., 2., 2 * z)); + landmarks.push_back(Point3(2., 2., 2 * z)); + landmarks.push_back(Point3(2., -2., 2 * z)); + return landmarks; } static vector genCameraDefaultCalibration() { - vector X ; - X.push_back(GeneralCamera(Pose3(eye(3),Point3(-baseline/2.0, 0.0, 0.0)))); - X.push_back(GeneralCamera(Pose3(eye(3),Point3( baseline/2.0, 0.0, 0.0)))); - return X ; + vector X; + X.push_back(GeneralCamera(Pose3(eye(3), Point3(-baseline / 2., 0., 0.)))); + X.push_back(GeneralCamera(Pose3(eye(3), Point3(baseline / 2., 0., 0.)))); + return X; } static vector genCameraVariableCalibration() { - const Cal3_S2 K(640,480,0.01,320,240); - vector X ; - X.push_back(GeneralCamera(Pose3(eye(3),Point3(-baseline/2.0, 0.0, 0.0)), K)); - X.push_back(GeneralCamera(Pose3(eye(3),Point3( baseline/2.0, 0.0, 0.0)), K)); - return X ; + const Cal3_S2 K(640, 480, 0.1, 320, 240); + vector X; + X.push_back(GeneralCamera(Pose3(eye(3), Point3(-baseline / 2., 0., 0.)), K)); + X.push_back(GeneralCamera(Pose3(eye(3), Point3(baseline / 2., 0., 0.)), K)); + return X; } -static boost::shared_ptr getOrdering(const vector& cameras, const vector& landmarks) { +static boost::shared_ptr getOrdering( + const vector& cameras, const vector& landmarks) { boost::shared_ptr ordering(new Ordering); - for ( size_t i = 0 ; i < landmarks.size() ; ++i ) ordering->push_back(L(i)) ; - for ( size_t i = 0 ; i < cameras.size() ; ++i ) ordering->push_back(X(i)) ; - return ordering ; + for (size_t i = 0; i < landmarks.size(); ++i) + ordering->push_back(L(i)); + for (size_t i = 0; i < cameras.size(); ++i) + ordering->push_back(X(i)); + return ordering; } - /* ************************************************************************* */ TEST( GeneralSFMFactor, optimize_defaultK ) { @@ -165,32 +168,32 @@ TEST( GeneralSFMFactor, optimize_defaultK ) { // add measurement with noise Graph graph; - for ( size_t j = 0 ; j < cameras.size() ; ++j) { - for ( size_t i = 0 ; i < landmarks.size() ; ++i) { - Point2 pt = cameras[j].project(landmarks[i]) ; + for (size_t j = 0; j < cameras.size(); ++j) { + for (size_t i = 0; i < landmarks.size(); ++i) { + Point2 pt = cameras[j].project(landmarks[i]); graph.addMeasurement(j, i, pt, sigma1); } } - const size_t nMeasurements = cameras.size()*landmarks.size() ; + const size_t nMeasurements = cameras.size() * landmarks.size(); // add initial - const double noise = baseline*0.1; + const double noise = baseline * 0.1; Values values; - for ( size_t i = 0 ; i < cameras.size() ; ++i ) - values.insert(X(i), cameras[i]) ; + for (size_t i = 0; i < cameras.size(); ++i) + values.insert(X(i), cameras[i]); - for ( size_t i = 0 ; i < landmarks.size() ; ++i ) { - Point3 pt(landmarks[i].x()+noise*getGaussian(), - landmarks[i].y()+noise*getGaussian(), - landmarks[i].z()+noise*getGaussian()); - values.insert(L(i), pt) ; + for (size_t i = 0; i < landmarks.size(); ++i) { + Point3 pt(landmarks[i].x() + noise * getGaussian(), + landmarks[i].y() + noise * getGaussian(), + landmarks[i].z() + noise * getGaussian()); + values.insert(L(i), pt); } graph.addCameraConstraint(0, cameras[0]); // Create an ordering of the variables - Ordering ordering = *getOrdering(cameras,landmarks); + Ordering ordering = *getOrdering(cameras, landmarks); LevenbergMarquardtOptimizer optimizer(graph, values, ordering); Values final = optimizer.optimize(); EXPECT(optimizer.error() < 0.5 * 1e-5 * nMeasurements); @@ -202,38 +205,37 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) { vector cameras = genCameraVariableCalibration(); // add measurement with noise Graph graph; - for ( size_t j = 0 ; j < cameras.size() ; ++j) { - for ( size_t i = 0 ; i < landmarks.size() ; ++i) { - Point2 pt = cameras[j].project(landmarks[i]) ; + for (size_t j = 0; j < cameras.size(); ++j) { + for (size_t i = 0; i < landmarks.size(); ++i) { + Point2 pt = cameras[j].project(landmarks[i]); graph.addMeasurement(j, i, pt, sigma1); } } - const size_t nMeasurements = cameras.size()*landmarks.size() ; + const size_t nMeasurements = cameras.size() * landmarks.size(); // add initial - const double noise = baseline*0.1; + const double noise = baseline * 0.1; Values values; - for ( size_t i = 0 ; i < cameras.size() ; ++i ) - values.insert(X(i), cameras[i]) ; + for (size_t i = 0; i < cameras.size(); ++i) + values.insert(X(i), cameras[i]); // add noise only to the first landmark - for ( size_t i = 0 ; i < landmarks.size() ; ++i ) { - if ( i == 0 ) { - Point3 pt(landmarks[i].x()+noise*getGaussian(), - landmarks[i].y()+noise*getGaussian(), - landmarks[i].z()+noise*getGaussian()); - values.insert(L(i), pt) ; - } - else { - values.insert(L(i), landmarks[i]) ; + for (size_t i = 0; i < landmarks.size(); ++i) { + if (i == 0) { + Point3 pt(landmarks[i].x() + noise * getGaussian(), + landmarks[i].y() + noise * getGaussian(), + landmarks[i].z() + noise * getGaussian()); + values.insert(L(i), pt); + } else { + values.insert(L(i), landmarks[i]); } } graph.addCameraConstraint(0, cameras[0]); const double reproj_error = 1e-5; - Ordering ordering = *getOrdering(cameras,landmarks); + Ordering ordering = *getOrdering(cameras, landmarks); LevenbergMarquardtOptimizer optimizer(graph, values, ordering); Values final = optimizer.optimize(); EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); @@ -246,35 +248,35 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) { vector cameras = genCameraVariableCalibration(); // add measurement with noise - const double noise = baseline*0.1; + const double noise = baseline * 0.1; Graph graph; - for ( size_t j = 0 ; j < cameras.size() ; ++j) { - for ( size_t i = 0 ; i < landmarks.size() ; ++i) { - Point2 pt = cameras[j].project(landmarks[i]) ; + for (size_t j = 0; j < cameras.size(); ++j) { + for (size_t i = 0; i < landmarks.size(); ++i) { + Point2 pt = cameras[j].project(landmarks[i]); graph.addMeasurement(j, i, pt, sigma1); } } - const size_t nMeasurements = landmarks.size()*cameras.size(); + const size_t nMeasurements = landmarks.size() * cameras.size(); Values values; - for ( size_t i = 0 ; i < cameras.size() ; ++i ) - values.insert(X(i), cameras[i]) ; + for (size_t i = 0; i < cameras.size(); ++i) + values.insert(X(i), cameras[i]); - for ( size_t i = 0 ; i < landmarks.size() ; ++i ) { - Point3 pt(landmarks[i].x()+noise*getGaussian(), - landmarks[i].y()+noise*getGaussian(), - landmarks[i].z()+noise*getGaussian()); + for (size_t i = 0; i < landmarks.size(); ++i) { + Point3 pt(landmarks[i].x() + noise * getGaussian(), + landmarks[i].y() + noise * getGaussian(), + landmarks[i].z() + noise * getGaussian()); //Point3 pt(landmarks[i].x(), landmarks[i].y(), landmarks[i].z()); - values.insert(L(i), pt) ; + values.insert(L(i), pt); } - for ( size_t i = 0 ; i < cameras.size() ; ++i ) + for (size_t i = 0; i < cameras.size(); ++i) graph.addCameraConstraint(i, cameras[i]); - const double reproj_error = 1e-5 ; + const double reproj_error = 1e-5; - Ordering ordering = *getOrdering(cameras,landmarks); + Ordering ordering = *getOrdering(cameras, landmarks); LevenbergMarquardtOptimizer optimizer(graph, values, ordering); Values final = optimizer.optimize(); EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); @@ -288,50 +290,45 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) { // add measurement with noise Graph graph; - for ( size_t j = 0 ; j < cameras.size() ; ++j) { - for ( size_t i = 0 ; i < landmarks.size() ; ++i) { - Point2 pt = cameras[j].project(landmarks[i]) ; + for (size_t j = 0; j < cameras.size(); ++j) { + for (size_t i = 0; i < landmarks.size(); ++i) { + Point2 pt = cameras[j].project(landmarks[i]); graph.addMeasurement(j, i, pt, sigma1); } } - const size_t nMeasurements = landmarks.size()*cameras.size(); + const size_t nMeasurements = landmarks.size() * cameras.size(); Values values; - for ( size_t i = 0 ; i < cameras.size() ; ++i ) { - const double - rot_noise = 1e-5, - trans_noise = 1e-3, - focal_noise = 1, - skew_noise = 1e-5; - if ( i == 0 ) { - values.insert(X(i), cameras[i]) ; - } - else { + for (size_t i = 0; i < cameras.size(); ++i) { + const double rot_noise = 1e-5, trans_noise = 1e-3, focal_noise = 1, + skew_noise = 1e-5; + if (i == 0) { + values.insert(X(i), cameras[i]); + } else { - Vector delta = (Vector(11) << - rot_noise, rot_noise, rot_noise, // rotation - trans_noise, trans_noise, trans_noise, // translation - focal_noise, focal_noise, // f_x, f_y - skew_noise, // s - trans_noise, trans_noise // ux, uy + Vector delta = (Vector(11) << rot_noise, rot_noise, rot_noise, // rotation + trans_noise, trans_noise, trans_noise, // translation + focal_noise, focal_noise, // f_x, f_y + skew_noise, // s + trans_noise, trans_noise // ux, uy ).finished(); - values.insert(X(i), cameras[i].retract(delta)) ; + values.insert(X(i), cameras[i].retract(delta)); } } - for ( size_t i = 0 ; i < landmarks.size() ; ++i ) { - values.insert(L(i), landmarks[i]) ; + for (size_t i = 0; i < landmarks.size(); ++i) { + values.insert(L(i), landmarks[i]); } // fix X0 and all landmarks, allow only the cameras[1] to move graph.addCameraConstraint(0, cameras[0]); - for ( size_t i = 0 ; i < landmarks.size() ; ++i ) + for (size_t i = 0; i < landmarks.size(); ++i) graph.addPoint3Constraint(i, landmarks[i]); - const double reproj_error = 1e-5 ; + const double reproj_error = 1e-5; - Ordering ordering = *getOrdering(cameras,landmarks); + Ordering ordering = *getOrdering(cameras, landmarks); LevenbergMarquardtOptimizer optimizer(graph, values, ordering); Values final = optimizer.optimize(); EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); @@ -344,38 +341,40 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) { // add measurement with noise Graph graph; - for ( size_t j = 0 ; j < cameras.size() ; ++j) { - for ( size_t i = 0 ; i < landmarks.size() ; ++i) { - Point2 pt = cameras[j].project(landmarks[i]) ; + for (size_t j = 0; j < cameras.size(); ++j) { + for (size_t i = 0; i < landmarks.size(); ++i) { + Point2 pt = cameras[j].project(landmarks[i]); graph.addMeasurement(j, i, pt, sigma1); } } - const size_t nMeasurements = cameras.size()*landmarks.size() ; + const size_t nMeasurements = cameras.size() * landmarks.size(); // add initial - const double noise = baseline*0.1; + const double noise = baseline * 0.1; Values values; - for ( size_t i = 0 ; i < cameras.size() ; ++i ) - values.insert(X(i), cameras[i]) ; + for (size_t i = 0; i < cameras.size(); ++i) + values.insert(X(i), cameras[i]); // add noise only to the first landmark - for ( size_t i = 0 ; i < landmarks.size() ; ++i ) { - Point3 pt(landmarks[i].x()+noise*getGaussian(), - landmarks[i].y()+noise*getGaussian(), - landmarks[i].z()+noise*getGaussian()); - values.insert(L(i), pt) ; + for (size_t i = 0; i < landmarks.size(); ++i) { + Point3 pt(landmarks[i].x() + noise * getGaussian(), + landmarks[i].y() + noise * getGaussian(), + landmarks[i].z() + noise * getGaussian()); + values.insert(L(i), pt); } // Constrain position of system with the first camera constrained to the origin graph.addCameraConstraint(0, cameras[0]); // Constrain the scale of the problem with a soft range factor of 1m between the cameras - graph.push_back(RangeFactor(X(0), X(1), 2.0, noiseModel::Isotropic::Sigma(1, 10.0))); + graph.push_back( + RangeFactor(X(0), X(1), 2., + noiseModel::Isotropic::Sigma(1, 10.))); - const double reproj_error = 1e-5 ; + const double reproj_error = 1e-5; - Ordering ordering = *getOrdering(cameras,landmarks); + Ordering ordering = *getOrdering(cameras, landmarks); LevenbergMarquardtOptimizer optimizer(graph, values, ordering); Values final = optimizer.optimize(); EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); @@ -386,17 +385,21 @@ TEST(GeneralSFMFactor, GeneralCameraPoseRange) { // Tests range factor between a GeneralCamera and a Pose3 Graph graph; graph.addCameraConstraint(0, GeneralCamera()); - graph.push_back(RangeFactor(X(0), X(1), 2.0, noiseModel::Isotropic::Sigma(1, 1.0))); - graph.push_back(PriorFactor(X(1), Pose3(Rot3(), Point3(1.0, 0.0, 0.0)), noiseModel::Isotropic::Sigma(6, 1.0))); + graph.push_back( + RangeFactor(X(0), X(1), 2., + noiseModel::Isotropic::Sigma(1, 1.))); + graph.push_back( + PriorFactor(X(1), Pose3(Rot3(), Point3(1., 0., 0.)), + noiseModel::Isotropic::Sigma(6, 1.))); Values init; init.insert(X(0), GeneralCamera()); - init.insert(X(1), Pose3(Rot3(), Point3(1.0,1.0,1.0))); + init.insert(X(1), Pose3(Rot3(), Point3(1., 1., 1.))); // The optimal value between the 2m range factor and 1m prior is 1.5m Values expected; expected.insert(X(0), GeneralCamera()); - expected.insert(X(1), Pose3(Rot3(), Point3(1.5,0.0,0.0))); + expected.insert(X(1), Pose3(Rot3(), Point3(1.5, 0., 0.))); LevenbergMarquardtParams params; params.absoluteErrorTol = 1e-9; @@ -410,16 +413,23 @@ TEST(GeneralSFMFactor, GeneralCameraPoseRange) { TEST(GeneralSFMFactor, CalibratedCameraPoseRange) { // Tests range factor between a CalibratedCamera and a Pose3 NonlinearFactorGraph graph; - graph.push_back(PriorFactor(X(0), CalibratedCamera(), noiseModel::Isotropic::Sigma(6, 1.0))); - graph.push_back(RangeFactor(X(0), X(1), 2.0, noiseModel::Isotropic::Sigma(1, 1.0))); - graph.push_back(PriorFactor(X(1), Pose3(Rot3(), Point3(1.0, 0.0, 0.0)), noiseModel::Isotropic::Sigma(6, 1.0))); + graph.push_back( + PriorFactor(X(0), CalibratedCamera(), + noiseModel::Isotropic::Sigma(6, 1.))); + graph.push_back( + RangeFactor(X(0), X(1), 2., + noiseModel::Isotropic::Sigma(1, 1.))); + graph.push_back( + PriorFactor(X(1), Pose3(Rot3(), Point3(1., 0., 0.)), + noiseModel::Isotropic::Sigma(6, 1.))); Values init; init.insert(X(0), CalibratedCamera()); - init.insert(X(1), Pose3(Rot3(), Point3(1.0,1.0,1.0))); + init.insert(X(1), Pose3(Rot3(), Point3(1., 1., 1.))); Values expected; - expected.insert(X(0), CalibratedCamera(Pose3(Rot3(), Point3(-0.333333333333, 0, 0)))); + expected.insert(X(0), + CalibratedCamera(Pose3(Rot3(), Point3(-0.333333333333, 0, 0)))); expected.insert(X(1), Pose3(Rot3(), Point3(1.333333333333, 0, 0))); LevenbergMarquardtParams params; @@ -432,50 +442,58 @@ TEST(GeneralSFMFactor, CalibratedCameraPoseRange) { /* ************************************************************************* */ TEST(GeneralSFMFactor, Linearize) { - Point2 z(3.,0.); + Point2 z(3., 0.); // Create Values Values values; Rot3 R; - Point3 t1(0,0,-6); - Pose3 x1(R,t1); + Point3 t1(0, 0, -6); + Pose3 x1(R, t1); values.insert(X(1), GeneralCamera(x1)); - Point3 l1; values.insert(L(1), l1); + Point3 l1; + values.insert(L(1), l1); // Test with Empty Model { - const SharedNoiseModel model; - Projection factor(z, model, X(1), L(1)); - GaussianFactor::shared_ptr expected = factor.NoiseModelFactor::linearize(values); - GaussianFactor::shared_ptr actual = factor.linearize(values); - EXPECT(assert_equal(*expected,*actual,1e-9)); + const SharedNoiseModel model; + Projection factor(z, model, X(1), L(1)); + GaussianFactor::shared_ptr expected = factor.NoiseModelFactor::linearize( + values); + GaussianFactor::shared_ptr actual = factor.linearize(values); + EXPECT(assert_equal(*expected, *actual, 1e-9)); } // Test with Unit Model { - const SharedNoiseModel model(noiseModel::Unit::Create(2)); - Projection factor(z, model, X(1), L(1)); - GaussianFactor::shared_ptr expected = factor.NoiseModelFactor::linearize(values); - GaussianFactor::shared_ptr actual = factor.linearize(values); - EXPECT(assert_equal(*expected,*actual,1e-9)); + const SharedNoiseModel model(noiseModel::Unit::Create(2)); + Projection factor(z, model, X(1), L(1)); + GaussianFactor::shared_ptr expected = factor.NoiseModelFactor::linearize( + values); + GaussianFactor::shared_ptr actual = factor.linearize(values); + EXPECT(assert_equal(*expected, *actual, 1e-9)); } // Test with Isotropic noise { - const SharedNoiseModel model(noiseModel::Isotropic::Sigma(2,0.5)); - Projection factor(z, model, X(1), L(1)); - GaussianFactor::shared_ptr expected = factor.NoiseModelFactor::linearize(values); - GaussianFactor::shared_ptr actual = factor.linearize(values); - EXPECT(assert_equal(*expected,*actual,1e-9)); + const SharedNoiseModel model(noiseModel::Isotropic::Sigma(2, 0.5)); + Projection factor(z, model, X(1), L(1)); + GaussianFactor::shared_ptr expected = factor.NoiseModelFactor::linearize( + values); + GaussianFactor::shared_ptr actual = factor.linearize(values); + EXPECT(assert_equal(*expected, *actual, 1e-9)); } // Test with Constrained Model { - const SharedNoiseModel model(noiseModel::Constrained::All(2)); - Projection factor(z, model, X(1), L(1)); - GaussianFactor::shared_ptr expected = factor.NoiseModelFactor::linearize(values); - GaussianFactor::shared_ptr actual = factor.linearize(values); - EXPECT(assert_equal(*expected,*actual,1e-9)); + const SharedNoiseModel model(noiseModel::Constrained::All(2)); + Projection factor(z, model, X(1), L(1)); + GaussianFactor::shared_ptr expected = factor.NoiseModelFactor::linearize( + values); + GaussianFactor::shared_ptr actual = factor.linearize(values); + EXPECT(assert_equal(*expected, *actual, 1e-9)); } } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ diff --git a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp index df56f5260..f812cd308 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp @@ -49,7 +49,8 @@ typedef NonlinearEquality Point3Constraint; /* ************************************************************************* */ class Graph: public NonlinearFactorGraph { public: - void addMeasurement(const int& i, const int& j, const Point2& z, const SharedNoiseModel& model) { + void addMeasurement(const int& i, const int& j, const Point2& z, + const SharedNoiseModel& model) { push_back(boost::make_shared(z, model, X(i), L(j))); } @@ -65,97 +66,101 @@ public: }; -static double getGaussian() -{ - double S,V1,V2; - // Use Box-Muller method to create gauss noise from uniform noise - do - { - double U1 = rand() / (double)(RAND_MAX); - double U2 = rand() / (double)(RAND_MAX); - V1 = 2 * U1 - 1; /* V1=[-1,1] */ - V2 = 2 * U2 - 1; /* V2=[-1,1] */ - S = V1 * V1 + V2 * V2; - } while(S>=1); - return sqrt(-2.0f * (double)log(S) / S) * V1; +static double getGaussian() { + double S, V1, V2; + // Use Box-Muller method to create gauss noise from uniform noise + do { + double U1 = rand() / (double) (RAND_MAX); + double U2 = rand() / (double) (RAND_MAX); + V1 = 2 * U1 - 1; /* V1=[-1,1] */ + V2 = 2 * U2 - 1; /* V2=[-1,1] */ + S = V1 * V1 + V2 * V2; + } while (S >= 1); + return sqrt(-2.f * (double) log(S) / S) * V1; } static const SharedNoiseModel sigma1(noiseModel::Unit::Create(2)); /* ************************************************************************* */ -TEST( GeneralSFMFactor_Cal3Bundler, equals ) -{ +TEST( GeneralSFMFactor_Cal3Bundler, equals ) { // Create two identical factors and make sure they're equal - Point2 z(323.,240.); - const Symbol cameraFrameNumber('x',1), landmarkNumber('l',1); + Point2 z(323., 240.); + const Symbol cameraFrameNumber('x', 1), landmarkNumber('l', 1); const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); - boost::shared_ptr - factor1(new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); + boost::shared_ptr factor1( + new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); - boost::shared_ptr - factor2(new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); + boost::shared_ptr factor2( + new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); EXPECT(assert_equal(*factor1, *factor2)); } /* ************************************************************************* */ TEST( GeneralSFMFactor_Cal3Bundler, error ) { - Point2 z(3.,0.); + Point2 z(3., 0.); const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); - boost::shared_ptr - factor(new Projection(z, sigma, X(1), L(1))); + boost::shared_ptr factor(new Projection(z, sigma, X(1), L(1))); // For the following configuration, the factor predicts 320,240 Values values; Rot3 R; - Point3 t1(0,0,-6); - Pose3 x1(R,t1); + Point3 t1(0, 0, -6); + Pose3 x1(R, t1); values.insert(X(1), GeneralCamera(x1)); - Point3 l1; values.insert(L(1), l1); - EXPECT(assert_equal(Vector2(-3.0, 0.0), factor->unwhitenedError(values))); + Point3 l1; + values.insert(L(1), l1); + EXPECT(assert_equal(Vector2(-3., 0.), factor->unwhitenedError(values))); } - -static const double baseline = 5.0 ; +static const double baseline = 5.; /* ************************************************************************* */ static vector genPoint3() { const double z = 5; - vector landmarks ; - landmarks.push_back(Point3 (-1.0,-1.0, z)); - landmarks.push_back(Point3 (-1.0, 1.0, z)); - landmarks.push_back(Point3 ( 1.0, 1.0, z)); - landmarks.push_back(Point3 ( 1.0,-1.0, z)); - landmarks.push_back(Point3 (-1.5,-1.5, 1.5*z)); - landmarks.push_back(Point3 (-1.5, 1.5, 1.5*z)); - landmarks.push_back(Point3 ( 1.5, 1.5, 1.5*z)); - landmarks.push_back(Point3 ( 1.5,-1.5, 1.5*z)); - landmarks.push_back(Point3 (-2.0,-2.0, 2*z)); - landmarks.push_back(Point3 (-2.0, 2.0, 2*z)); - landmarks.push_back(Point3 ( 2.0, 2.0, 2*z)); - landmarks.push_back(Point3 ( 2.0,-2.0, 2*z)); - return landmarks ; + vector landmarks; + landmarks.push_back(Point3(-1., -1., z)); + landmarks.push_back(Point3(-1., 1., z)); + landmarks.push_back(Point3(1., 1., z)); + landmarks.push_back(Point3(1., -1., z)); + landmarks.push_back(Point3(-1.5, -1.5, 1.5 * z)); + landmarks.push_back(Point3(-1.5, 1.5, 1.5 * z)); + landmarks.push_back(Point3(1.5, 1.5, 1.5 * z)); + landmarks.push_back(Point3(1.5, -1.5, 1.5 * z)); + landmarks.push_back(Point3(-2., -2., 2 * z)); + landmarks.push_back(Point3(-2., 2., 2 * z)); + landmarks.push_back(Point3(2., 2., 2 * z)); + landmarks.push_back(Point3(2., -2., 2 * z)); + return landmarks; } static vector genCameraDefaultCalibration() { - vector cameras ; - cameras.push_back(GeneralCamera(Pose3(eye(3),Point3(-baseline/2.0, 0.0, 0.0)))); - cameras.push_back(GeneralCamera(Pose3(eye(3),Point3( baseline/2.0, 0.0, 0.0)))); - return cameras ; + vector cameras; + cameras.push_back( + GeneralCamera(Pose3(Rot3(), Point3(-baseline / 2., 0., 0.)))); + cameras.push_back( + GeneralCamera(Pose3(Rot3(), Point3(baseline / 2., 0., 0.)))); + return cameras; } static vector genCameraVariableCalibration() { - const Cal3Bundler K(500,1e-3,1e-3); - vector cameras ; - cameras.push_back(GeneralCamera(Pose3(eye(3),Point3(-baseline/2.0, 0.0, 0.0)), K)); - cameras.push_back(GeneralCamera(Pose3(eye(3),Point3( baseline/2.0, 0.0, 0.0)), K)); - return cameras ; + const Cal3Bundler K(500, 1e-3, 1e-3); + vector cameras; + cameras.push_back( + GeneralCamera(Pose3(Rot3(), Point3(-baseline / 2., 0., 0.)), K)); + cameras.push_back( + GeneralCamera(Pose3(Rot3(), Point3(baseline / 2., 0., 0.)), K)); + return cameras; } -static boost::shared_ptr getOrdering(const std::vector& cameras, const std::vector& landmarks) { +static boost::shared_ptr getOrdering( + const std::vector& cameras, + const std::vector& landmarks) { boost::shared_ptr ordering(new Ordering); - for ( size_t i = 0 ; i < landmarks.size() ; ++i ) ordering->push_back(L(i)) ; - for ( size_t i = 0 ; i < cameras.size() ; ++i ) ordering->push_back(X(i)) ; - return ordering ; + for (size_t i = 0; i < landmarks.size(); ++i) + ordering->push_back(L(i)); + for (size_t i = 0; i < cameras.size(); ++i) + ordering->push_back(X(i)); + return ordering; } /* ************************************************************************* */ @@ -166,32 +171,32 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_defaultK ) { // add measurement with noise Graph graph; - for ( size_t j = 0 ; j < cameras.size() ; ++j) { - for ( size_t i = 0 ; i < landmarks.size() ; ++i) { - Point2 pt = cameras[j].project(landmarks[i]) ; + for (size_t j = 0; j < cameras.size(); ++j) { + for (size_t i = 0; i < landmarks.size(); ++i) { + Point2 pt = cameras[j].project(landmarks[i]); graph.addMeasurement(j, i, pt, sigma1); } } - const size_t nMeasurements = cameras.size()*landmarks.size() ; + const size_t nMeasurements = cameras.size() * landmarks.size(); // add initial - const double noise = baseline*0.1; + const double noise = baseline * 0.1; Values values; - for ( size_t i = 0 ; i < cameras.size() ; ++i ) - values.insert(X(i), cameras[i]) ; + for (size_t i = 0; i < cameras.size(); ++i) + values.insert(X(i), cameras[i]); - for ( size_t i = 0 ; i < landmarks.size() ; ++i ) { - Point3 pt(landmarks[i].x()+noise*getGaussian(), - landmarks[i].y()+noise*getGaussian(), - landmarks[i].z()+noise*getGaussian()); - values.insert(L(i), pt) ; + for (size_t i = 0; i < landmarks.size(); ++i) { + Point3 pt(landmarks[i].x() + noise * getGaussian(), + landmarks[i].y() + noise * getGaussian(), + landmarks[i].z() + noise * getGaussian()); + values.insert(L(i), pt); } graph.addCameraConstraint(0, cameras[0]); // Create an ordering of the variables - Ordering ordering = *getOrdering(cameras,landmarks); + Ordering ordering = *getOrdering(cameras, landmarks); LevenbergMarquardtOptimizer optimizer(graph, values, ordering); Values final = optimizer.optimize(); EXPECT(optimizer.error() < 0.5 * 1e-5 * nMeasurements); @@ -203,38 +208,37 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_SingleMeasurementError ) { vector cameras = genCameraVariableCalibration(); // add measurement with noise Graph graph; - for ( size_t j = 0 ; j < cameras.size() ; ++j) { - for ( size_t i = 0 ; i < landmarks.size() ; ++i) { - Point2 pt = cameras[j].project(landmarks[i]) ; + for (size_t j = 0; j < cameras.size(); ++j) { + for (size_t i = 0; i < landmarks.size(); ++i) { + Point2 pt = cameras[j].project(landmarks[i]); graph.addMeasurement(j, i, pt, sigma1); } } - const size_t nMeasurements = cameras.size()*landmarks.size() ; + const size_t nMeasurements = cameras.size() * landmarks.size(); // add initial - const double noise = baseline*0.1; + const double noise = baseline * 0.1; Values values; - for ( size_t i = 0 ; i < cameras.size() ; ++i ) - values.insert(X(i), cameras[i]) ; + for (size_t i = 0; i < cameras.size(); ++i) + values.insert(X(i), cameras[i]); // add noise only to the first landmark - for ( size_t i = 0 ; i < landmarks.size() ; ++i ) { - if ( i == 0 ) { - Point3 pt(landmarks[i].x()+noise*getGaussian(), - landmarks[i].y()+noise*getGaussian(), - landmarks[i].z()+noise*getGaussian()); - values.insert(L(i), pt) ; - } - else { - values.insert(L(i), landmarks[i]) ; + for (size_t i = 0; i < landmarks.size(); ++i) { + if (i == 0) { + Point3 pt(landmarks[i].x() + noise * getGaussian(), + landmarks[i].y() + noise * getGaussian(), + landmarks[i].z() + noise * getGaussian()); + values.insert(L(i), pt); + } else { + values.insert(L(i), landmarks[i]); } } graph.addCameraConstraint(0, cameras[0]); const double reproj_error = 1e-5; - Ordering ordering = *getOrdering(cameras,landmarks); + Ordering ordering = *getOrdering(cameras, landmarks); LevenbergMarquardtOptimizer optimizer(graph, values, ordering); Values final = optimizer.optimize(); EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); @@ -247,35 +251,35 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_FixCameras ) { vector cameras = genCameraVariableCalibration(); // add measurement with noise - const double noise = baseline*0.1; + const double noise = baseline * 0.1; Graph graph; - for ( size_t j = 0 ; j < cameras.size() ; ++j) { - for ( size_t i = 0 ; i < landmarks.size() ; ++i) { - Point2 pt = cameras[j].project(landmarks[i]) ; + for (size_t j = 0; j < cameras.size(); ++j) { + for (size_t i = 0; i < landmarks.size(); ++i) { + Point2 pt = cameras[j].project(landmarks[i]); graph.addMeasurement(j, i, pt, sigma1); } } - const size_t nMeasurements = landmarks.size()*cameras.size(); + const size_t nMeasurements = landmarks.size() * cameras.size(); Values values; - for ( size_t i = 0 ; i < cameras.size() ; ++i ) - values.insert(X(i), cameras[i]) ; + for (size_t i = 0; i < cameras.size(); ++i) + values.insert(X(i), cameras[i]); - for ( size_t i = 0 ; i < landmarks.size() ; ++i ) { - Point3 pt(landmarks[i].x()+noise*getGaussian(), - landmarks[i].y()+noise*getGaussian(), - landmarks[i].z()+noise*getGaussian()); + for (size_t i = 0; i < landmarks.size(); ++i) { + Point3 pt(landmarks[i].x() + noise * getGaussian(), + landmarks[i].y() + noise * getGaussian(), + landmarks[i].z() + noise * getGaussian()); //Point3 pt(landmarks[i].x(), landmarks[i].y(), landmarks[i].z()); - values.insert(L(i), pt) ; + values.insert(L(i), pt); } - for ( size_t i = 0 ; i < cameras.size() ; ++i ) + for (size_t i = 0; i < cameras.size(); ++i) graph.addCameraConstraint(i, cameras[i]); - const double reproj_error = 1e-5 ; + const double reproj_error = 1e-5; - Ordering ordering = *getOrdering(cameras,landmarks); + Ordering ordering = *getOrdering(cameras, landmarks); LevenbergMarquardtOptimizer optimizer(graph, values, ordering); Values final = optimizer.optimize(); EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); @@ -289,46 +293,43 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_FixLandmarks ) { // add measurement with noise Graph graph; - for ( size_t j = 0 ; j < cameras.size() ; ++j) { - for ( size_t i = 0 ; i < landmarks.size() ; ++i) { - Point2 pt = cameras[j].project(landmarks[i]) ; + for (size_t j = 0; j < cameras.size(); ++j) { + for (size_t i = 0; i < landmarks.size(); ++i) { + Point2 pt = cameras[j].project(landmarks[i]); graph.addMeasurement(j, i, pt, sigma1); } } - const size_t nMeasurements = landmarks.size()*cameras.size(); + const size_t nMeasurements = landmarks.size() * cameras.size(); Values values; - for ( size_t i = 0 ; i < cameras.size() ; ++i ) { - const double - rot_noise = 1e-5, trans_noise = 1e-3, - focal_noise = 1, distort_noise = 1e-3; - if ( i == 0 ) { - values.insert(X(i), cameras[i]) ; - } - else { + for (size_t i = 0; i < cameras.size(); ++i) { + const double rot_noise = 1e-5, trans_noise = 1e-3, focal_noise = 1, + distort_noise = 1e-3; + if (i == 0) { + values.insert(X(i), cameras[i]); + } else { - Vector delta = (Vector(9) << - rot_noise, rot_noise, rot_noise, // rotation - trans_noise, trans_noise, trans_noise, // translation - focal_noise, distort_noise, distort_noise // f, k1, k2 + Vector delta = (Vector(9) << rot_noise, rot_noise, rot_noise, // rotation + trans_noise, trans_noise, trans_noise, // translation + focal_noise, distort_noise, distort_noise // f, k1, k2 ).finished(); - values.insert(X(i), cameras[i].retract(delta)) ; + values.insert(X(i), cameras[i].retract(delta)); } } - for ( size_t i = 0 ; i < landmarks.size() ; ++i ) { - values.insert(L(i), landmarks[i]) ; + for (size_t i = 0; i < landmarks.size(); ++i) { + values.insert(L(i), landmarks[i]); } // fix X0 and all landmarks, allow only the cameras[1] to move graph.addCameraConstraint(0, cameras[0]); - for ( size_t i = 0 ; i < landmarks.size() ; ++i ) + for (size_t i = 0; i < landmarks.size(); ++i) graph.addPoint3Constraint(i, landmarks[i]); - const double reproj_error = 1e-5 ; + const double reproj_error = 1e-5; - Ordering ordering = *getOrdering(cameras,landmarks); + Ordering ordering = *getOrdering(cameras, landmarks); LevenbergMarquardtOptimizer optimizer(graph, values, ordering); Values final = optimizer.optimize(); EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); @@ -341,43 +342,48 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_BA ) { // add measurement with noise Graph graph; - for ( size_t j = 0 ; j < cameras.size() ; ++j) { - for ( size_t i = 0 ; i < landmarks.size() ; ++i) { - Point2 pt = cameras[j].project(landmarks[i]) ; + for (size_t j = 0; j < cameras.size(); ++j) { + for (size_t i = 0; i < landmarks.size(); ++i) { + Point2 pt = cameras[j].project(landmarks[i]); graph.addMeasurement(j, i, pt, sigma1); } } - const size_t nMeasurements = cameras.size()*landmarks.size() ; + const size_t nMeasurements = cameras.size() * landmarks.size(); // add initial - const double noise = baseline*0.1; + const double noise = baseline * 0.1; Values values; - for ( size_t i = 0 ; i < cameras.size() ; ++i ) - values.insert(X(i), cameras[i]) ; + for (size_t i = 0; i < cameras.size(); ++i) + values.insert(X(i), cameras[i]); // add noise only to the first landmark - for ( size_t i = 0 ; i < landmarks.size() ; ++i ) { - Point3 pt(landmarks[i].x()+noise*getGaussian(), - landmarks[i].y()+noise*getGaussian(), - landmarks[i].z()+noise*getGaussian()); - values.insert(L(i), pt) ; + for (size_t i = 0; i < landmarks.size(); ++i) { + Point3 pt(landmarks[i].x() + noise * getGaussian(), + landmarks[i].y() + noise * getGaussian(), + landmarks[i].z() + noise * getGaussian()); + values.insert(L(i), pt); } // Constrain position of system with the first camera constrained to the origin graph.addCameraConstraint(0, cameras[0]); // Constrain the scale of the problem with a soft range factor of 1m between the cameras - graph.push_back(RangeFactor(X(0), X(1), 2.0, noiseModel::Isotropic::Sigma(1, 10.0))); + graph.push_back( + RangeFactor(X(0), X(1), 2., + noiseModel::Isotropic::Sigma(1, 10.))); - const double reproj_error = 1e-5 ; + const double reproj_error = 1e-5; - Ordering ordering = *getOrdering(cameras,landmarks); + Ordering ordering = *getOrdering(cameras, landmarks); LevenbergMarquardtOptimizer optimizer(graph, values, ordering); Values final = optimizer.optimize(); EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements); } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ From 850501ed52370cec5196125d391bafebd12e6e44 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 14 Jun 2015 11:16:54 -0700 Subject: [PATCH 38/47] BORG Formatting --- gtsam/linear/HessianFactor.cpp | 294 +++++++++++++++++---------------- 1 file changed, 149 insertions(+), 145 deletions(-) diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 7f3929488..bbc684a10 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -49,183 +49,185 @@ using namespace std; using namespace boost::assign; -namespace br { using namespace boost::range; using namespace boost::adaptors; } +namespace br { +using namespace boost::range; +using namespace boost::adaptors; +} namespace gtsam { /* ************************************************************************* */ HessianFactor::HessianFactor() : - info_(cref_list_of<1>(1)) -{ + info_(cref_list_of<1>(1)) { linearTerm().setZero(); constantTerm() = 0.0; } /* ************************************************************************* */ HessianFactor::HessianFactor(Key j, const Matrix& G, const Vector& g, double f) : - GaussianFactor(cref_list_of<1>(j)), info_(cref_list_of<2>(G.cols())(1)) -{ - if(G.rows() != G.cols() || G.rows() != g.size()) throw invalid_argument( - "Attempting to construct HessianFactor with inconsistent matrix and/or vector dimensions"); - info_(0,0) = G; - info_(0,1) = g; - info_(1,1)(0,0) = f; + GaussianFactor(cref_list_of<1>(j)), info_(cref_list_of<2>(G.cols())(1)) { + if (G.rows() != G.cols() || G.rows() != g.size()) + throw invalid_argument( + "Attempting to construct HessianFactor with inconsistent matrix and/or vector dimensions"); + info_(0, 0) = G; + info_(0, 1) = g; + info_(1, 1)(0, 0) = f; } /* ************************************************************************* */ // error is 0.5*(x-mu)'*inv(Sigma)*(x-mu) = 0.5*(x'*G*x - 2*x'*G*mu + mu'*G*mu) // where G = inv(Sigma), g = G*mu, f = mu'*G*mu = mu'*g HessianFactor::HessianFactor(Key j, const Vector& mu, const Matrix& Sigma) : - GaussianFactor(cref_list_of<1>(j)), - info_(cref_list_of<2> (Sigma.cols()) (1) ) -{ - if (Sigma.rows() != Sigma.cols() || Sigma.rows() != mu.size()) throw invalid_argument( - "Attempting to construct HessianFactor with inconsistent matrix and/or vector dimensions"); - info_(0,0) = Sigma.inverse(); // G - info_(0,1) = info_(0,0).selfadjointView() * mu; // g - info_(1,1)(0,0) = mu.dot(info_(0,1).knownOffDiagonal().col(0)); // f + GaussianFactor(cref_list_of<1>(j)), info_(cref_list_of<2>(Sigma.cols())(1)) { + if (Sigma.rows() != Sigma.cols() || Sigma.rows() != mu.size()) + throw invalid_argument( + "Attempting to construct HessianFactor with inconsistent matrix and/or vector dimensions"); + info_(0, 0) = Sigma.inverse(); // G + info_(0, 1) = info_(0, 0).selfadjointView() * mu; // g + info_(1, 1)(0, 0) = mu.dot(info_(0, 1).knownOffDiagonal().col(0)); // f } /* ************************************************************************* */ -HessianFactor::HessianFactor(Key j1, Key j2, - const Matrix& G11, const Matrix& G12, const Vector& g1, - const Matrix& G22, const Vector& g2, double f) : - GaussianFactor(cref_list_of<2>(j1)(j2)), - info_(cref_list_of<3> (G11.cols()) (G22.cols()) (1) ) -{ - info_(0,0) = G11; - info_(0,1) = G12; - info_(0,2) = g1; - info_(1,1) = G22; - info_(1,2) = g2; - info_(2,2)(0,0) = f; +HessianFactor::HessianFactor(Key j1, Key j2, const Matrix& G11, + const Matrix& G12, const Vector& g1, const Matrix& G22, const Vector& g2, + double f) : + GaussianFactor(cref_list_of<2>(j1)(j2)), info_( + cref_list_of<3>(G11.cols())(G22.cols())(1)) { + info_(0, 0) = G11; + info_(0, 1) = G12; + info_(0, 2) = g1; + info_(1, 1) = G22; + info_(1, 2) = g2; + info_(2, 2)(0, 0) = f; } /* ************************************************************************* */ -HessianFactor::HessianFactor(Key j1, Key j2, Key j3, - const Matrix& G11, const Matrix& G12, const Matrix& G13, const Vector& g1, - const Matrix& G22, const Matrix& G23, const Vector& g2, - const Matrix& G33, const Vector& g3, double f) : - GaussianFactor(cref_list_of<3>(j1)(j2)(j3)), - info_(cref_list_of<4> (G11.cols()) (G22.cols()) (G33.cols()) (1) ) -{ - if(G11.rows() != G11.cols() || G11.rows() != G12.rows() || G11.rows() != G13.rows() || G11.rows() != g1.size() || - G22.cols() != G12.cols() || G33.cols() != G13.cols() || G22.cols() != g2.size() || G33.cols() != g3.size()) - throw invalid_argument("Inconsistent matrix and/or vector dimensions in HessianFactor constructor"); - info_(0,0) = G11; - info_(0,1) = G12; - info_(0,2) = G13; - info_(0,3) = g1; - info_(1,1) = G22; - info_(1,2) = G23; - info_(1,3) = g2; - info_(2,2) = G33; - info_(2,3) = g3; - info_(3,3)(0,0) = f; +HessianFactor::HessianFactor(Key j1, Key j2, Key j3, const Matrix& G11, + const Matrix& G12, const Matrix& G13, const Vector& g1, const Matrix& G22, + const Matrix& G23, const Vector& g2, const Matrix& G33, const Vector& g3, + double f) : + GaussianFactor(cref_list_of<3>(j1)(j2)(j3)), info_( + cref_list_of<4>(G11.cols())(G22.cols())(G33.cols())(1)) { + if (G11.rows() != G11.cols() || G11.rows() != G12.rows() + || G11.rows() != G13.rows() || G11.rows() != g1.size() + || G22.cols() != G12.cols() || G33.cols() != G13.cols() + || G22.cols() != g2.size() || G33.cols() != g3.size()) + throw invalid_argument( + "Inconsistent matrix and/or vector dimensions in HessianFactor constructor"); + info_(0, 0) = G11; + info_(0, 1) = G12; + info_(0, 2) = G13; + info_(0, 3) = g1; + info_(1, 1) = G22; + info_(1, 2) = G23; + info_(1, 3) = g2; + info_(2, 2) = G33; + info_(2, 3) = g3; + info_(3, 3)(0, 0) = f; } /* ************************************************************************* */ namespace { -DenseIndex _getSizeHF(const Vector& m) { return m.size(); } +DenseIndex _getSizeHF(const Vector& m) { + return m.size(); +} } /* ************************************************************************* */ -HessianFactor::HessianFactor(const std::vector& js, const std::vector& Gs, - const std::vector& gs, double f) : - GaussianFactor(js), info_(gs | br::transformed(&_getSizeHF), true) -{ +HessianFactor::HessianFactor(const std::vector& js, + const std::vector& Gs, const std::vector& gs, double f) : + GaussianFactor(js), info_(gs | br::transformed(&_getSizeHF), true) { // Get the number of variables size_t variable_count = js.size(); // Verify the provided number of entries in the vectors are consistent - if(gs.size() != variable_count || Gs.size() != (variable_count*(variable_count+1))/2) - throw invalid_argument("Inconsistent number of entries between js, Gs, and gs in HessianFactor constructor.\nThe number of keys provided \ + if (gs.size() != variable_count + || Gs.size() != (variable_count * (variable_count + 1)) / 2) + throw invalid_argument( + "Inconsistent number of entries between js, Gs, and gs in HessianFactor constructor.\nThe number of keys provided \ in js must match the number of linear vector pieces in gs. The number of upper-diagonal blocks in Gs must be n*(n+1)/2"); // Verify the dimensions of each provided matrix are consistent // Note: equations for calculating the indices derived from the "sum of an arithmetic sequence" formula - for(size_t i = 0; i < variable_count; ++i){ + for (size_t i = 0; i < variable_count; ++i) { DenseIndex block_size = gs[i].size(); // Check rows - for(size_t j = 0; j < variable_count-i; ++j){ - size_t index = i*(2*variable_count - i + 1)/2 + j; - if(Gs[index].rows() != block_size){ - throw invalid_argument("Inconsistent matrix and/or vector dimensions in HessianFactor constructor"); + for (size_t j = 0; j < variable_count - i; ++j) { + size_t index = i * (2 * variable_count - i + 1) / 2 + j; + if (Gs[index].rows() != block_size) { + throw invalid_argument( + "Inconsistent matrix and/or vector dimensions in HessianFactor constructor"); } } // Check cols - for(size_t j = 0; j <= i; ++j){ - size_t index = j*(2*variable_count - j + 1)/2 + (i-j); - if(Gs[index].cols() != block_size){ - throw invalid_argument("Inconsistent matrix and/or vector dimensions in HessianFactor constructor"); + for (size_t j = 0; j <= i; ++j) { + size_t index = j * (2 * variable_count - j + 1) / 2 + (i - j); + if (Gs[index].cols() != block_size) { + throw invalid_argument( + "Inconsistent matrix and/or vector dimensions in HessianFactor constructor"); } } } // Fill in the blocks size_t index = 0; - for(size_t i = 0; i < variable_count; ++i){ - for(size_t j = i; j < variable_count; ++j){ + for (size_t i = 0; i < variable_count; ++i) { + for (size_t j = i; j < variable_count; ++j) { info_(i, j) = Gs[index++]; } info_(i, variable_count) = gs[i]; } - info_(variable_count, variable_count)(0,0) = f; + info_(variable_count, variable_count)(0, 0) = f; } /* ************************************************************************* */ namespace { -void _FromJacobianHelper(const JacobianFactor& jf, SymmetricBlockMatrix& info) -{ +void _FromJacobianHelper(const JacobianFactor& jf, SymmetricBlockMatrix& info) { gttic(HessianFactor_fromJacobian); const SharedDiagonal& jfModel = jf.get_model(); - if(jfModel) - { - if(jf.get_model()->isConstrained()) - throw invalid_argument("Cannot construct HessianFactor from JacobianFactor with constrained noise model"); - info.full().triangularView() = jf.matrixObject().full().transpose() * - (jfModel->invsigmas().array() * jfModel->invsigmas().array()).matrix().asDiagonal() * - jf.matrixObject().full(); + if (jfModel) { + if (jf.get_model()->isConstrained()) + throw invalid_argument( + "Cannot construct HessianFactor from JacobianFactor with constrained noise model"); + info.full().triangularView() = + jf.matrixObject().full().transpose() + * (jfModel->invsigmas().array() * jfModel->invsigmas().array()).matrix().asDiagonal() + * jf.matrixObject().full(); } else { - info.full().triangularView() = jf.matrixObject().full().transpose() * jf.matrixObject().full(); + info.full().triangularView() = jf.matrixObject().full().transpose() + * jf.matrixObject().full(); } } } /* ************************************************************************* */ HessianFactor::HessianFactor(const JacobianFactor& jf) : - GaussianFactor(jf), info_(SymmetricBlockMatrix::LikeActiveViewOf(jf.matrixObject())) -{ + GaussianFactor(jf), info_( + SymmetricBlockMatrix::LikeActiveViewOf(jf.matrixObject())) { _FromJacobianHelper(jf, info_); } /* ************************************************************************* */ HessianFactor::HessianFactor(const GaussianFactor& gf) : - GaussianFactor(gf) -{ + GaussianFactor(gf) { // Copy the matrix data depending on what type of factor we're copying from - if(const JacobianFactor* jf = dynamic_cast(&gf)) - { + if (const JacobianFactor* jf = dynamic_cast(&gf)) { info_ = SymmetricBlockMatrix::LikeActiveViewOf(jf->matrixObject()); _FromJacobianHelper(*jf, info_); - } - else if(const HessianFactor* hf = dynamic_cast(&gf)) - { + } else if (const HessianFactor* hf = dynamic_cast(&gf)) { info_ = hf->info_; - } - else - { - throw std::invalid_argument("In HessianFactor(const GaussianFactor& gf), gf is neither a JacobianFactor nor a HessianFactor"); + } else { + throw std::invalid_argument( + "In HessianFactor(const GaussianFactor& gf), gf is neither a JacobianFactor nor a HessianFactor"); } } /* ************************************************************************* */ HessianFactor::HessianFactor(const GaussianFactorGraph& factors, - boost::optional scatter) -{ + boost::optional scatter) { gttic(HessianFactor_MergeConstructor); boost::optional computedScatter; - if(!scatter) { + if (!scatter) { computedScatter = Scatter(factors); scatter = computedScatter; } @@ -247,45 +249,46 @@ HessianFactor::HessianFactor(const GaussianFactorGraph& factors, // Form A' * A gttic(update); BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) - if(factor) + if (factor) factor->updateHessian(keys_, &info_); gttoc(update); } /* ************************************************************************* */ -void HessianFactor::print(const std::string& s, const KeyFormatter& formatter) const { +void HessianFactor::print(const std::string& s, + const KeyFormatter& formatter) const { cout << s << "\n"; cout << " keys: "; - for(const_iterator key=begin(); key!=end(); ++key) + for (const_iterator key = begin(); key != end(); ++key) cout << formatter(*key) << "(" << getDim(key) << ") "; cout << "\n"; - gtsam::print(Matrix(info_.full().selfadjointView()), "Augmented information matrix: "); + gtsam::print(Matrix(info_.full().selfadjointView()), + "Augmented information matrix: "); } /* ************************************************************************* */ bool HessianFactor::equals(const GaussianFactor& lf, double tol) const { - if(!dynamic_cast(&lf)) + if (!dynamic_cast(&lf)) return false; else { - if(!Factor::equals(lf, tol)) + if (!Factor::equals(lf, tol)) return false; Matrix thisMatrix = info_.full().selfadjointView(); - thisMatrix(thisMatrix.rows()-1, thisMatrix.cols()-1) = 0.0; - Matrix rhsMatrix = static_cast(lf).info_.full().selfadjointView(); - rhsMatrix(rhsMatrix.rows()-1, rhsMatrix.cols()-1) = 0.0; + thisMatrix(thisMatrix.rows() - 1, thisMatrix.cols() - 1) = 0.0; + Matrix rhsMatrix = + static_cast(lf).info_.full().selfadjointView(); + rhsMatrix(rhsMatrix.rows() - 1, rhsMatrix.cols() - 1) = 0.0; return equal_with_abs_tol(thisMatrix, rhsMatrix, tol); } } /* ************************************************************************* */ -Matrix HessianFactor::augmentedInformation() const -{ +Matrix HessianFactor::augmentedInformation() const { return info_.full().selfadjointView(); } /* ************************************************************************* */ -Matrix HessianFactor::information() const -{ +Matrix HessianFactor::information() const { return info_.range(0, size(), 0, size()).selfadjointView(); } @@ -293,10 +296,10 @@ Matrix HessianFactor::information() const VectorValues HessianFactor::hessianDiagonal() const { VectorValues d; // Loop over all variables - for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) { + for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { // Get the diagonal block, and insert its diagonal Matrix B = info_(j, j).selfadjointView(); - d.insert(keys_[j],B.diagonal()); + d.insert(keys_[j], B.diagonal()); } return d; } @@ -309,26 +312,24 @@ void HessianFactor::hessianDiagonal(double* d) const { } /* ************************************************************************* */ -map HessianFactor::hessianBlockDiagonal() const { - map blocks; +map HessianFactor::hessianBlockDiagonal() const { + map blocks; // Loop over all variables - for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) { + for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { // Get the diagonal block, and insert it Matrix B = info_(j, j).selfadjointView(); - blocks.insert(make_pair(keys_[j],B)); + blocks.insert(make_pair(keys_[j], B)); } return blocks; } /* ************************************************************************* */ -Matrix HessianFactor::augmentedJacobian() const -{ +Matrix HessianFactor::augmentedJacobian() const { return JacobianFactor(*this).augmentedJacobian(); } /* ************************************************************************* */ -std::pair HessianFactor::jacobian() const -{ +std::pair HessianFactor::jacobian() const { return JacobianFactor(*this).jacobian(); } @@ -341,13 +342,13 @@ double HessianFactor::error(const VectorValues& c) const { // NOTE may not be as efficient const Vector x = c.vector(keys()); xtg = x.dot(linearTerm()); - xGx = x.transpose() * info_.range(0, size(), 0, size()).selfadjointView() * x; - return 0.5 * (f - 2.0 * xtg + xGx); + xGx = x.transpose() * info_.range(0, size(), 0, size()).selfadjointView() * x; + return 0.5 * (f - 2.0 * xtg + xGx); } /* ************************************************************************* */ void HessianFactor::updateHessian(const FastVector& infoKeys, - SymmetricBlockMatrix* info) const { + SymmetricBlockMatrix* info) const { gttic(updateHessian_HessianFactor); // Apply updates to the upper triangle DenseIndex n = size(), N = info->nBlocks() - 1; @@ -356,17 +357,17 @@ void HessianFactor::updateHessian(const FastVector& infoKeys, const DenseIndex J = (j == n) ? N : Slot(infoKeys, keys_[j]); slots[j] = J; for (DenseIndex i = 0; i <= j; ++i) { - const DenseIndex I = slots[i]; // because i<=j, slots[i] is valid. + const DenseIndex I = slots[i]; // because i<=j, slots[i] is valid. (*info)(I, J) += info_(i, j); } } } /* ************************************************************************* */ -GaussianFactor::shared_ptr HessianFactor::negate() const -{ +GaussianFactor::shared_ptr HessianFactor::negate() const { shared_ptr result = boost::make_shared(*this); - result->info_.full().triangularView() = -result->info_.full().triangularView().nestedExpression(); // Negate the information matrix of the result + result->info_.full().triangularView() = + -result->info_.full().triangularView().nestedExpression(); // Negate the information matrix of the result return result; } @@ -383,7 +384,7 @@ void HessianFactor::multiplyHessianAdd(double alpha, const VectorValues& x, // Accessing the VectorValues one by one is expensive // So we will loop over columns to access x only once per column // And fill the above temporary y values, to be added into yvalues after - for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) { + for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { // xj is the input vector Vector xj = x.at(keys_[j]); DenseIndex i = 0; @@ -392,13 +393,13 @@ void HessianFactor::multiplyHessianAdd(double alpha, const VectorValues& x, // blocks on the diagonal are only half y[i] += info_(j, j).selfadjointView() * xj; // for below diagonal, we take transpose block from upper triangular part - for (i = j + 1; i < (DenseIndex)size(); ++i) + for (i = j + 1; i < (DenseIndex) size(); ++i) y[i] += info_(i, j).knownOffDiagonal() * xj; } // copy to yvalues - for(DenseIndex i = 0; i < (DenseIndex)size(); ++i) { - bool didNotExist; + for (DenseIndex i = 0; i < (DenseIndex) size(); ++i) { + bool didNotExist; VectorValues::iterator it; boost::tie(it, didNotExist) = yvalues.tryInsert(keys_[i], Vector()); if (didNotExist) @@ -413,7 +414,7 @@ VectorValues HessianFactor::gradientAtZero() const { VectorValues g; size_t n = size(); for (size_t j = 0; j < n; ++j) - g.insert(keys_[j], -info_(j,n).knownOffDiagonal()); + g.insert(keys_[j], -info_(j, n).knownOffDiagonal()); return g; } @@ -436,8 +437,7 @@ Vector HessianFactor::gradient(Key key, const VectorValues& x) const { if (i > j) { Matrix Gji = info(j, i); Gij = Gji.transpose(); - } - else { + } else { Gij = info(i, j); } // Accumulate Gij*xj to gradf @@ -449,30 +449,34 @@ Vector HessianFactor::gradient(Key key, const VectorValues& x) const { } /* ************************************************************************* */ -std::pair, boost::shared_ptr > -EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys) -{ +std::pair, + boost::shared_ptr > EliminateCholesky( + const GaussianFactorGraph& factors, const Ordering& keys) { gttic(EliminateCholesky); // Build joint factor HessianFactor::shared_ptr jointFactor; try { - jointFactor = boost::make_shared(factors, Scatter(factors, keys)); - } catch(std::invalid_argument&) { + jointFactor = boost::make_shared(factors, + Scatter(factors, keys)); + } catch (std::invalid_argument&) { throw InvalidDenseElimination( "EliminateCholesky was called with a request to eliminate variables that are not\n" - "involved in the provided factors."); + "involved in the provided factors."); } // Do dense elimination GaussianConditional::shared_ptr conditional; try { size_t numberOfKeysToEliminate = keys.size(); - VerticalBlockMatrix Ab = jointFactor->info_.choleskyPartial(numberOfKeysToEliminate); - conditional = boost::make_shared(jointFactor->keys(), numberOfKeysToEliminate, Ab); + VerticalBlockMatrix Ab = jointFactor->info_.choleskyPartial( + numberOfKeysToEliminate); + conditional = boost::make_shared(jointFactor->keys(), + numberOfKeysToEliminate, Ab); // Erase the eliminated keys in the remaining factor - jointFactor->keys_.erase(jointFactor->begin(), jointFactor->begin() + numberOfKeysToEliminate); - } catch(CholeskyFailed&) { + jointFactor->keys_.erase(jointFactor->begin(), + jointFactor->begin() + numberOfKeysToEliminate); + } catch (CholeskyFailed&) { throw IndeterminantLinearSystemException(keys.front()); } @@ -481,9 +485,9 @@ EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys) } /* ************************************************************************* */ -std::pair, boost::shared_ptr > -EliminatePreferCholesky(const GaussianFactorGraph& factors, const Ordering& keys) -{ +std::pair, + boost::shared_ptr > EliminatePreferCholesky( + const GaussianFactorGraph& factors, const Ordering& keys) { gttic(EliminatePreferCholesky); // If any JacobianFactors have constrained noise models, we have to convert From e045a5e1f77d09da943e85d9750946e560b7dfcd Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 14 Jun 2015 12:53:20 -0700 Subject: [PATCH 39/47] Added more powerful tests on updateHessian --- gtsam/slam/tests/testGeneralSFMFactor.cpp | 67 ++++++++++++----------- 1 file changed, 34 insertions(+), 33 deletions(-) diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index 7da6cdbdf..d14847e52 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include #include @@ -28,9 +29,10 @@ #include #include +#include #include #include -using namespace boost; +using namespace boost::assign; #include #include @@ -441,7 +443,8 @@ TEST(GeneralSFMFactor, CalibratedCameraPoseRange) { } /* ************************************************************************* */ -TEST(GeneralSFMFactor, Linearize) { +// Frank created these tests after switching to a custom LinearizedFactor +TEST(GeneralSFMFactor, LinearizedFactor) { Point2 z(3., 0.); // Create Values @@ -453,44 +456,42 @@ TEST(GeneralSFMFactor, Linearize) { Point3 l1; values.insert(L(1), l1); - // Test with Empty Model + vector models; { - const SharedNoiseModel model; - Projection factor(z, model, X(1), L(1)); - GaussianFactor::shared_ptr expected = factor.NoiseModelFactor::linearize( - values); - GaussianFactor::shared_ptr actual = factor.linearize(values); - EXPECT(assert_equal(*expected, *actual, 1e-9)); + // Create various noise-models to test all cases + using namespace noiseModel; + Rot2 R = Rot2::fromAngle(0.3); + Matrix2 cov = R.matrix() * R.matrix().transpose(); + models += SharedNoiseModel(), Unit::Create(2), // + Isotropic::Sigma(2, 0.5), Constrained::All(2), Gaussian::Covariance(cov); } - // Test with Unit Model - { - const SharedNoiseModel model(noiseModel::Unit::Create(2)); + + // Now loop over all these noise models + BOOST_FOREACH(SharedNoiseModel model, models) { Projection factor(z, model, X(1), L(1)); - GaussianFactor::shared_ptr expected = factor.NoiseModelFactor::linearize( - values); - GaussianFactor::shared_ptr actual = factor.linearize(values); - EXPECT(assert_equal(*expected, *actual, 1e-9)); - } - // Test with Isotropic noise - { - const SharedNoiseModel model(noiseModel::Isotropic::Sigma(2, 0.5)); - Projection factor(z, model, X(1), L(1)); - GaussianFactor::shared_ptr expected = factor.NoiseModelFactor::linearize( - values); - GaussianFactor::shared_ptr actual = factor.linearize(values); - EXPECT(assert_equal(*expected, *actual, 1e-9)); - } - // Test with Constrained Model - { - const SharedNoiseModel model(noiseModel::Constrained::All(2)); - Projection factor(z, model, X(1), L(1)); - GaussianFactor::shared_ptr expected = factor.NoiseModelFactor::linearize( - values); + + // Test linearize + GaussianFactor::shared_ptr expected = // + factor.NoiseModelFactor::linearize(values); GaussianFactor::shared_ptr actual = factor.linearize(values); EXPECT(assert_equal(*expected, *actual, 1e-9)); + + // Test methods that rely on updateHessian + if (model && !model->isConstrained()) { + // Construct HessianFactor from single JacobianFactor + HessianFactor expectedHessian(*expected), actualHessian(*actual); + EXPECT(assert_equal(expectedHessian, actualHessian, 1e-9)); + + // Construct from GaussianFactorGraph + GaussianFactorGraph gfg1; + gfg1 += expected; + GaussianFactorGraph gfg2; + gfg2 += actual; + HessianFactor hessian1(gfg1), hessian2(gfg2); + EXPECT(assert_equal(hessian1, hessian2, 1e-9)); + } } } - /* ************************************************************************* */ int main() { TestResult tr; From df226fc43633f0a3650239fdd67fe3e79ba048e7 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 14 Jun 2015 12:54:18 -0700 Subject: [PATCH 40/47] No longer store my own matrices but get same performance using block --- gtsam/slam/GeneralSFMFactor.h | 29 +++++++++++++++-------------- 1 file changed, 15 insertions(+), 14 deletions(-) diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index fbf64de6c..c026cd36a 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -135,23 +135,19 @@ public: class LinearizedFactor : public JacobianFactor { // Fixed size matrices - // TODO: implement generic BinaryJacobianFactor next to + // TODO(frank): implement generic BinaryJacobianFactor next to // JacobianFactor - JacobianC AC_; - JacobianL AL_; - Vector2 b_; public: /// Constructor LinearizedFactor(Key i1, const JacobianC& A1, Key i2, const JacobianL& A2, const Vector2& b, const SharedDiagonal& model = SharedDiagonal()) - : JacobianFactor(i1, A1, i2, A2, b, model), AC_(A1), AL_(A2), b_(b) {} + : JacobianFactor(i1, A1, i2, A2, b, model) {} // Fixed-size matrix update void updateHessian(const FastVector& infoKeys, SymmetricBlockMatrix* info) const { gttic(updateHessian_LinearizedFactor); - // Whiten the factor if it has a noise model const SharedDiagonal& model = get_model(); if (model && !model->isUnit()) { @@ -163,17 +159,22 @@ public: whitenedFactor.updateHessian(infoKeys, info); } else { // First build an array of slots - DenseIndex slotC = Slot(infoKeys, keys_.front()); - DenseIndex slotL = Slot(infoKeys, keys_.back()); + DenseIndex slot0 = Slot(infoKeys, keys_.front()); + DenseIndex slot1 = Slot(infoKeys, keys_.back()); DenseIndex slotB = info->nBlocks() - 1; + const Matrix& Ab = Ab_.matrix(); + Eigen::Block A0 = Ab.template block<2,DimC>(0,0); + Eigen::Block A1 = Ab.template block<2,DimL>(0,DimC); + Eigen::Block b = Ab.template block<2,1>(0,DimC+DimL); + // We perform I += A'*A to the upper triangle - (*info)(slotC, slotC).selfadjointView().rankUpdate(AC_.transpose()); - (*info)(slotC, slotL).knownOffDiagonal() += AC_.transpose() * AL_; - (*info)(slotC, slotB).knownOffDiagonal() += AC_.transpose() * b_; - (*info)(slotL, slotL).selfadjointView().rankUpdate(AL_.transpose()); - (*info)(slotL, slotB).knownOffDiagonal() += AL_.transpose() * b_; - (*info)(slotB, slotB).selfadjointView().rankUpdate(b_.transpose()); + (*info)(slot0, slot0).selfadjointView().rankUpdate(A0.transpose()); + (*info)(slot0, slot1).knownOffDiagonal() += A0.transpose() * A1; + (*info)(slot0, slotB).knownOffDiagonal() += A0.transpose() * b; + (*info)(slot1, slot1).selfadjointView().rankUpdate(A1.transpose()); + (*info)(slot1, slotB).knownOffDiagonal() += A1.transpose() * b; + (*info)(slotB, slotB).selfadjointView().rankUpdate(b.transpose()); } } }; From 8c22684bbb6bc1c871bdd73f6ae35a69599bd74d Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 14 Jun 2015 13:02:44 -0700 Subject: [PATCH 41/47] Went back to base 1, and used constructors for blocks (cleaner) --- gtsam/slam/GeneralSFMFactor.h | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index c026cd36a..8097394db 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -159,21 +159,21 @@ public: whitenedFactor.updateHessian(infoKeys, info); } else { // First build an array of slots - DenseIndex slot0 = Slot(infoKeys, keys_.front()); - DenseIndex slot1 = Slot(infoKeys, keys_.back()); + DenseIndex slot1 = Slot(infoKeys, keys_.front()); + DenseIndex slot2 = Slot(infoKeys, keys_.back()); DenseIndex slotB = info->nBlocks() - 1; const Matrix& Ab = Ab_.matrix(); - Eigen::Block A0 = Ab.template block<2,DimC>(0,0); - Eigen::Block A1 = Ab.template block<2,DimL>(0,DimC); - Eigen::Block b = Ab.template block<2,1>(0,DimC+DimL); + Eigen::Block A1(Ab,0,0); + Eigen::Block A2(Ab,0,DimC); + Eigen::Block b(Ab,0,DimC+DimL); // We perform I += A'*A to the upper triangle - (*info)(slot0, slot0).selfadjointView().rankUpdate(A0.transpose()); - (*info)(slot0, slot1).knownOffDiagonal() += A0.transpose() * A1; - (*info)(slot0, slotB).knownOffDiagonal() += A0.transpose() * b; (*info)(slot1, slot1).selfadjointView().rankUpdate(A1.transpose()); + (*info)(slot1, slot2).knownOffDiagonal() += A1.transpose() * A2; (*info)(slot1, slotB).knownOffDiagonal() += A1.transpose() * b; + (*info)(slot2, slot2).selfadjointView().rankUpdate(A2.transpose()); + (*info)(slot2, slotB).knownOffDiagonal() += A2.transpose() * b; (*info)(slotB, slotB).selfadjointView().rankUpdate(b.transpose()); } } From 9fcd498d6a6390c10dfb7ac43eb837ce42ee2797 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 14 Jun 2015 13:37:51 -0700 Subject: [PATCH 42/47] BORG formatting --- gtsam/base/SymmetricBlockMatrix.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/gtsam/base/SymmetricBlockMatrix.cpp b/gtsam/base/SymmetricBlockMatrix.cpp index 546b6a7f1..0fbdfeefc 100644 --- a/gtsam/base/SymmetricBlockMatrix.cpp +++ b/gtsam/base/SymmetricBlockMatrix.cpp @@ -61,13 +61,13 @@ VerticalBlockMatrix SymmetricBlockMatrix::choleskyPartial( // Split conditional - // Create one big conditionals with many frontal variables. - gttic(Construct_conditional); - const size_t varDim = offset(nFrontals); - VerticalBlockMatrix Ab = VerticalBlockMatrix::LikeActiveViewOf(*this, varDim); - Ab.full() = matrix_.topRows(varDim); - Ab.full().triangularView().setZero(); - gttoc(Construct_conditional); + // Create one big conditionals with many frontal variables. + gttic(Construct_conditional); + const size_t varDim = offset(nFrontals); + VerticalBlockMatrix Ab = VerticalBlockMatrix::LikeActiveViewOf(*this, varDim); + Ab.full() = matrix_.topRows(varDim); + Ab.full().triangularView().setZero(); + gttoc(Construct_conditional); gttic(Remaining_factor); // Take lower-right block of Ab_ to get the remaining factor From a94c2e7323b0c4f3911ead650bdd627e68bdf72f Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 14 Jun 2015 15:02:48 -0700 Subject: [PATCH 43/47] Renamed to BinaryJacobianFactor --- gtsam/slam/GeneralSFMFactor.h | 26 +++++++++++++------------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index 8097394db..ba8d478ad 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -133,29 +133,29 @@ public: } } - class LinearizedFactor : public JacobianFactor { + class BinaryJacobianFactor : public JacobianFactor { // Fixed size matrices // TODO(frank): implement generic BinaryJacobianFactor next to // JacobianFactor public: /// Constructor - LinearizedFactor(Key i1, const JacobianC& A1, Key i2, const JacobianL& A2, + BinaryJacobianFactor(Key key1, const JacobianC& A1, Key key2, const JacobianL& A2, const Vector2& b, const SharedDiagonal& model = SharedDiagonal()) - : JacobianFactor(i1, A1, i2, A2, b, model) {} + : JacobianFactor(key1, A1, key2, A2, b, model) {} // Fixed-size matrix update void updateHessian(const FastVector& infoKeys, SymmetricBlockMatrix* info) const { - gttic(updateHessian_LinearizedFactor); + gttic(updateHessian_BinaryJacobianFactor); // Whiten the factor if it has a noise model const SharedDiagonal& model = get_model(); if (model && !model->isUnit()) { if (model->isConstrained()) throw std::invalid_argument( - "JacobianFactor::updateHessian: cannot update information with " + "BinaryJacobianFactor::updateHessian: cannot update information with " "constrained noise model"); - JacobianFactor whitenedFactor = whiten(); + JacobianFactor whitenedFactor = whiten(); // TODO: make BinaryJacobianFactor whitenedFactor.updateHessian(infoKeys, info); } else { // First build an array of slots @@ -164,9 +164,9 @@ public: DenseIndex slotB = info->nBlocks() - 1; const Matrix& Ab = Ab_.matrix(); - Eigen::Block A1(Ab,0,0); - Eigen::Block A2(Ab,0,DimC); - Eigen::Block b(Ab,0,DimC+DimL); + Eigen::Block A1(Ab, 0, 0); + Eigen::Block A2(Ab, 0, DimC); + Eigen::Block b(Ab, 0, DimC + DimL); // We perform I += A'*A to the upper triangle (*info)(slot1, slot1).selfadjointView().rankUpdate(A1.transpose()); @@ -174,7 +174,7 @@ public: (*info)(slot1, slotB).knownOffDiagonal() += A1.transpose() * b; (*info)(slot2, slot2).selfadjointView().rankUpdate(A2.transpose()); (*info)(slot2, slotB).knownOffDiagonal() += A2.transpose() * b; - (*info)(slotB, slotB).selfadjointView().rankUpdate(b.transpose()); + (*info)(slotB, slotB)(0,0) = b.transpose() * b; } } }; @@ -182,7 +182,7 @@ public: /// Linearize using fixed-size matrices boost::shared_ptr linearize(const Values& values) const { // Only linearize if the factor is active - if (!this->active(values)) return boost::shared_ptr(); + if (!this->active(values)) return boost::shared_ptr(); const Key key1 = this->key1(), key2 = this->key2(); JacobianC H1; @@ -212,11 +212,11 @@ public: if (noiseModel && noiseModel->isConstrained()) { using noiseModel::Constrained; - return boost::make_shared( + return boost::make_shared( key1, H1, key2, H2, b, boost::static_pointer_cast(noiseModel)->unit()); } else { - return boost::make_shared(key1, H1, key2, H2, b); + return boost::make_shared(key1, H1, key2, H2, b); } } From 30104a114ea60ae5632406e463eb249289e77dae Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 14 Jun 2015 15:03:44 -0700 Subject: [PATCH 44/47] More tests with failing example --- gtsam/slam/tests/testGeneralSFMFactor.cpp | 129 ++++++++++++++-------- 1 file changed, 82 insertions(+), 47 deletions(-) diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index d14847e52..dd19e0894 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -82,40 +82,6 @@ static double getGaussian() { } static const SharedNoiseModel sigma1(noiseModel::Unit::Create(2)); - -/* ************************************************************************* */ -TEST( GeneralSFMFactor, equals ) { - // Create two identical factors and make sure they're equal - Point2 z(323., 240.); - const Symbol cameraFrameNumber('x', 1), landmarkNumber('l', 1); - const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); - boost::shared_ptr factor1( - new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); - - boost::shared_ptr factor2( - new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); - - EXPECT(assert_equal(*factor1, *factor2)); -} - -/* ************************************************************************* */ -TEST( GeneralSFMFactor, error ) { - Point2 z(3., 0.); - const SharedNoiseModel sigma(noiseModel::Unit::Create(2)); - Projection factor(z, sigma, X(1), L(1)); - // For the following configuration, the factor predicts 320,240 - Values values; - Rot3 R; - Point3 t1(0, 0, -6); - Pose3 x1(R, t1); - values.insert(X(1), GeneralCamera(x1)); - Point3 l1; - values.insert(L(1), l1); - EXPECT( - assert_equal(((Vector ) Vector2(-3., 0.)), - factor.unwhitenedError(values))); -} - static const double baseline = 5.; /* ************************************************************************* */ @@ -162,6 +128,39 @@ static boost::shared_ptr getOrdering( return ordering; } +/* ************************************************************************* */ +TEST( GeneralSFMFactor, equals ) { + // Create two identical factors and make sure they're equal + Point2 z(323., 240.); + const Symbol cameraFrameNumber('x', 1), landmarkNumber('l', 1); + const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); + boost::shared_ptr factor1( + new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); + + boost::shared_ptr factor2( + new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); + + EXPECT(assert_equal(*factor1, *factor2)); +} + +/* ************************************************************************* */ +TEST( GeneralSFMFactor, error ) { + Point2 z(3., 0.); + const SharedNoiseModel sigma(noiseModel::Unit::Create(2)); + Projection factor(z, sigma, X(1), L(1)); + // For the following configuration, the factor predicts 320,240 + Values values; + Rot3 R; + Point3 t1(0, 0, -6); + Pose3 x1(R, t1); + values.insert(X(1), GeneralCamera(x1)); + Point3 l1; + values.insert(L(1), l1); + EXPECT( + assert_equal(((Vector ) Vector2(-3., 0.)), + factor.unwhitenedError(values))); +} + /* ************************************************************************* */ TEST( GeneralSFMFactor, optimize_defaultK ) { @@ -252,10 +251,10 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) { // add measurement with noise const double noise = baseline * 0.1; Graph graph; - for (size_t j = 0; j < cameras.size(); ++j) { - for (size_t i = 0; i < landmarks.size(); ++i) { - Point2 pt = cameras[j].project(landmarks[i]); - graph.addMeasurement(j, i, pt, sigma1); + for (size_t i = 0; i < cameras.size(); ++i) { + for (size_t j = 0; j < landmarks.size(); ++j) { + Point2 z = cameras[i].project(landmarks[j]); + graph.addMeasurement(i, j, z, sigma1); } } @@ -265,12 +264,11 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) { for (size_t i = 0; i < cameras.size(); ++i) values.insert(X(i), cameras[i]); - for (size_t i = 0; i < landmarks.size(); ++i) { - Point3 pt(landmarks[i].x() + noise * getGaussian(), - landmarks[i].y() + noise * getGaussian(), - landmarks[i].z() + noise * getGaussian()); - //Point3 pt(landmarks[i].x(), landmarks[i].y(), landmarks[i].z()); - values.insert(L(i), pt); + for (size_t j = 0; j < landmarks.size(); ++j) { + Point3 pt(landmarks[j].x() + noise * getGaussian(), + landmarks[j].y() + noise * getGaussian(), + landmarks[j].z() + noise * getGaussian()); + values.insert(L(j), pt); } for (size_t i = 0; i < cameras.size(); ++i) @@ -444,8 +442,8 @@ TEST(GeneralSFMFactor, CalibratedCameraPoseRange) { /* ************************************************************************* */ // Frank created these tests after switching to a custom LinearizedFactor -TEST(GeneralSFMFactor, LinearizedFactor) { - Point2 z(3., 0.); +TEST(GeneralSFMFactor, BinaryJacobianFactor) { + Point2 measurement(3., -1.); // Create Values Values values; @@ -468,7 +466,7 @@ TEST(GeneralSFMFactor, LinearizedFactor) { // Now loop over all these noise models BOOST_FOREACH(SharedNoiseModel model, models) { - Projection factor(z, model, X(1), L(1)); + Projection factor(measurement, model, X(1), L(1)); // Test linearize GaussianFactor::shared_ptr expected = // @@ -482,6 +480,14 @@ TEST(GeneralSFMFactor, LinearizedFactor) { HessianFactor expectedHessian(*expected), actualHessian(*actual); EXPECT(assert_equal(expectedHessian, actualHessian, 1e-9)); + // Convert back + JacobianFactor actualJacobian(actualHessian); + // Note we do not expect the actualJacobian to match *expected + // Just that they have the same information on the variable. + EXPECT( + assert_equal(expected->augmentedInformation(), + actualJacobian.augmentedInformation(), 1e-9)); + // Construct from GaussianFactorGraph GaussianFactorGraph gfg1; gfg1 += expected; @@ -492,6 +498,35 @@ TEST(GeneralSFMFactor, LinearizedFactor) { } } } + +/* ************************************************************************* */ +// Do a thorough test of BinaryJacobianFactor +TEST( GeneralSFMFactor, BinaryJacobianFactor2 ) { + + vector landmarks = genPoint3(); + vector cameras = genCameraVariableCalibration(); + + Values values; + for (size_t i = 0; i < cameras.size(); ++i) + values.insert(X(i), cameras[i]); + for (size_t j = 0; j < landmarks.size(); ++j) + values.insert(L(j), landmarks[j]); + + for (size_t i = 0; i < cameras.size(); ++i) { + for (size_t j = 0; j < landmarks.size(); ++j) { + Point2 z = cameras[i].project(landmarks[j]); + Projection::shared_ptr nonlinear = // + boost::make_shared(z, sigma1, X(i), L(j)); + GaussianFactor::shared_ptr factor = nonlinear->linearize(values); + HessianFactor hessian(*factor); + JacobianFactor jacobian(hessian); + EXPECT( + assert_equal(factor->augmentedInformation(), + jacobian.augmentedInformation(), 1e-9)); + } + } +} + /* ************************************************************************* */ int main() { TestResult tr; From 06902209b0a3877ef6204de934767215362837ab Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 14 Jun 2015 15:19:55 -0700 Subject: [PATCH 45/47] Fixed bug in hessian scalar computation --- gtsam/slam/GeneralSFMFactor.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index ba8d478ad..a41047ae4 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -174,7 +174,7 @@ public: (*info)(slot1, slotB).knownOffDiagonal() += A1.transpose() * b; (*info)(slot2, slot2).selfadjointView().rankUpdate(A2.transpose()); (*info)(slot2, slotB).knownOffDiagonal() += A2.transpose() * b; - (*info)(slotB, slotB)(0,0) = b.transpose() * b; + (*info)(slotB, slotB)(0,0) += b.transpose() * b; } } }; From 7698c52ce9ceed0a2ef21acd47dffe5dd7a535e2 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 14 Jun 2015 15:50:15 -0700 Subject: [PATCH 46/47] Created BinaryJacobianFactor template --- gtsam/linear/BinaryJacobianFactor.h | 91 +++++++++++++++++++++++ gtsam/slam/GeneralSFMFactor.h | 61 ++------------- gtsam/slam/tests/testGeneralSFMFactor.cpp | 12 +-- 3 files changed, 104 insertions(+), 60 deletions(-) create mode 100644 gtsam/linear/BinaryJacobianFactor.h diff --git a/gtsam/linear/BinaryJacobianFactor.h b/gtsam/linear/BinaryJacobianFactor.h new file mode 100644 index 000000000..23d11964c --- /dev/null +++ b/gtsam/linear/BinaryJacobianFactor.h @@ -0,0 +1,91 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file BinaryJacobianFactor.h + * + * @brief A binary JacobianFactor specialization that uses fixed matrix math for speed + * + * @date June 2015 + * @author Frank Dellaert + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + +/** + * A binary JacobianFactor specialization that uses fixed matrix math for speed + */ +template +struct BinaryJacobianFactor: JacobianFactor { + + /// Constructor + BinaryJacobianFactor(Key key1, const Eigen::Matrix& A1, + Key key2, const Eigen::Matrix& A2, + const Eigen::Matrix& b, // + const SharedDiagonal& model = SharedDiagonal()) : + JacobianFactor(key1, A1, key2, A2, b, model) { + } + + inline Key key1() const { + return keys_[0]; + } + inline Key key2() const { + return keys_[1]; + } + + // Fixed-size matrix update + void updateHessian(const FastVector& infoKeys, + SymmetricBlockMatrix* info) const { + gttic(updateHessian_BinaryJacobianFactor); + // Whiten the factor if it has a noise model + const SharedDiagonal& model = get_model(); + if (model && !model->isUnit()) { + if (model->isConstrained()) + throw std::invalid_argument( + "BinaryJacobianFactor::updateHessian: cannot update information with " + "constrained noise model"); + BinaryJacobianFactor whitenedFactor(key1(), model->Whiten(getA(begin())), + key2(), model->Whiten(getA(end())), model->whiten(getb())); + whitenedFactor.updateHessian(infoKeys, info); + } else { + // First build an array of slots + DenseIndex slot1 = Slot(infoKeys, key1()); + DenseIndex slot2 = Slot(infoKeys, key2()); + DenseIndex slotB = info->nBlocks() - 1; + + const Matrix& Ab = Ab_.matrix(); + Eigen::Block A1(Ab, 0, 0); + Eigen::Block A2(Ab, 0, N1); + Eigen::Block b(Ab, 0, N1 + N2); + + // We perform I += A'*A to the upper triangle + (*info)(slot1, slot1).selfadjointView().rankUpdate(A1.transpose()); + (*info)(slot1, slot2).knownOffDiagonal() += A1.transpose() * A2; + (*info)(slot1, slotB).knownOffDiagonal() += A1.transpose() * b; + (*info)(slot2, slot2).selfadjointView().rankUpdate(A2.transpose()); + (*info)(slot2, slotB).knownOffDiagonal() += A2.transpose() * b; + (*info)(slotB, slotB)(0, 0) += b.transpose() * b; + } + } +}; + +template +struct traits > : Testable< + BinaryJacobianFactor > { +}; + +} //namespace gtsam diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index a41047ae4..d2b042fed 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -25,7 +25,7 @@ #include #include #include -#include +#include #include #include #include @@ -133,56 +133,10 @@ public: } } - class BinaryJacobianFactor : public JacobianFactor { - // Fixed size matrices - // TODO(frank): implement generic BinaryJacobianFactor next to - // JacobianFactor - - public: - /// Constructor - BinaryJacobianFactor(Key key1, const JacobianC& A1, Key key2, const JacobianL& A2, - const Vector2& b, - const SharedDiagonal& model = SharedDiagonal()) - : JacobianFactor(key1, A1, key2, A2, b, model) {} - - // Fixed-size matrix update - void updateHessian(const FastVector& infoKeys, SymmetricBlockMatrix* info) const { - gttic(updateHessian_BinaryJacobianFactor); - // Whiten the factor if it has a noise model - const SharedDiagonal& model = get_model(); - if (model && !model->isUnit()) { - if (model->isConstrained()) - throw std::invalid_argument( - "BinaryJacobianFactor::updateHessian: cannot update information with " - "constrained noise model"); - JacobianFactor whitenedFactor = whiten(); // TODO: make BinaryJacobianFactor - whitenedFactor.updateHessian(infoKeys, info); - } else { - // First build an array of slots - DenseIndex slot1 = Slot(infoKeys, keys_.front()); - DenseIndex slot2 = Slot(infoKeys, keys_.back()); - DenseIndex slotB = info->nBlocks() - 1; - - const Matrix& Ab = Ab_.matrix(); - Eigen::Block A1(Ab, 0, 0); - Eigen::Block A2(Ab, 0, DimC); - Eigen::Block b(Ab, 0, DimC + DimL); - - // We perform I += A'*A to the upper triangle - (*info)(slot1, slot1).selfadjointView().rankUpdate(A1.transpose()); - (*info)(slot1, slot2).knownOffDiagonal() += A1.transpose() * A2; - (*info)(slot1, slotB).knownOffDiagonal() += A1.transpose() * b; - (*info)(slot2, slot2).selfadjointView().rankUpdate(A2.transpose()); - (*info)(slot2, slotB).knownOffDiagonal() += A2.transpose() * b; - (*info)(slotB, slotB)(0,0) += b.transpose() * b; - } - } - }; - /// Linearize using fixed-size matrices boost::shared_ptr linearize(const Values& values) const { // Only linearize if the factor is active - if (!this->active(values)) return boost::shared_ptr(); + if (!this->active(values)) return boost::shared_ptr(); const Key key1 = this->key1(), key2 = this->key2(); JacobianC H1; @@ -210,14 +164,13 @@ public: b = noiseModel->Whiten(b); } + // Create new (unit) noiseModel, preserving constraints if applicable + SharedDiagonal model; if (noiseModel && noiseModel->isConstrained()) { - using noiseModel::Constrained; - return boost::make_shared( - key1, H1, key2, H2, b, - boost::static_pointer_cast(noiseModel)->unit()); - } else { - return boost::make_shared(key1, H1, key2, H2, b); + model = boost::static_pointer_cast(noiseModel)->unit(); } + + return boost::make_shared >(key1, H1, key2, H2, b, model); } /** return the measured */ diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index dd19e0894..a8f85301e 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -135,11 +135,11 @@ TEST( GeneralSFMFactor, equals ) { const Symbol cameraFrameNumber('x', 1), landmarkNumber('l', 1); const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); boost::shared_ptr factor1( - new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); - + new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); + boost::shared_ptr factor2( - new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); - + new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); + EXPECT(assert_equal(*factor1, *factor2)); } @@ -157,8 +157,8 @@ TEST( GeneralSFMFactor, error ) { Point3 l1; values.insert(L(1), l1); EXPECT( - assert_equal(((Vector ) Vector2(-3., 0.)), - factor.unwhitenedError(values))); + assert_equal(((Vector ) Vector2(-3., 0.)), + factor.unwhitenedError(values))); } /* ************************************************************************* */ From 33e412f2ee1fa3ccb55104b072c199e3590c267a Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 15 Jun 2015 01:05:48 -0700 Subject: [PATCH 47/47] Code review --- gtsam/linear/Scatter.cpp | 5 +++-- gtsam/linear/Scatter.h | 2 +- gtsam/nonlinear/LevenbergMarquardtOptimizer.h | 2 +- 3 files changed, 5 insertions(+), 4 deletions(-) diff --git a/gtsam/linear/Scatter.cpp b/gtsam/linear/Scatter.cpp index 2602e08ba..942b42160 100644 --- a/gtsam/linear/Scatter.cpp +++ b/gtsam/linear/Scatter.cpp @@ -10,9 +10,10 @@ * -------------------------------------------------------------------------- */ /** - * @file HessianFactor.cpp + * @file Scatter.cpp * @author Richard Roberts - * @date Dec 8, 2010 + * @author Frank Dellaert + * @date June 2015 */ #include diff --git a/gtsam/linear/Scatter.h b/gtsam/linear/Scatter.h index e1df2d658..7a37ba1e0 100644 --- a/gtsam/linear/Scatter.h +++ b/gtsam/linear/Scatter.h @@ -14,7 +14,7 @@ * @brief Maps global variable indices to slot indices * @author Richard Roberts * @author Frank Dellaert - * @date Dec 8, 2010 + * @date June 2015 */ #pragma once diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h index 315e95512..e5561af48 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h @@ -46,7 +46,7 @@ public: public: - double lambdaInitial; ///< The initial Levenberg-Marquardt damping term (default: 1e-4) + double lambdaInitial; ///< The initial Levenberg-Marquardt damping term (default: 1e-5) double lambdaFactor; ///< The amount by which to multiply or divide lambda when adjusting lambda (default: 10.0) double lambdaUpperBound; ///< The maximum lambda to try before assuming the optimization has failed (default: 1e5) double lambdaLowerBound; ///< The minimum lambda used in LM (default: 0)