From 60d3ba2d0e6f56ef8da602ad281fc5eb73c0dbbc Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Wed, 27 Feb 2013 20:23:47 +0000 Subject: [PATCH] Added and incremental fixed-lag smoother using new iSAM2 marginalization functionality, and created a common base class for all fixed-lag smoother implementations. --- .../nonlinear/BatchFixedLagSmoother.cpp | 275 ++++-------------- .../nonlinear/BatchFixedLagSmoother.h | 155 ++-------- gtsam_unstable/nonlinear/FixedLagSmoother.cpp | 115 ++++++++ gtsam_unstable/nonlinear/FixedLagSmoother.h | 118 ++++++++ .../nonlinear/IncrementalFixedLagSmoother.cpp | 188 ++++++++++++ .../nonlinear/IncrementalFixedLagSmoother.h | 103 +++++++ .../tests/testBatchFixedLagSmoother.cpp | 4 +- .../tests/testIncrementalFixedLagSmoother.cpp | 185 ++++++++++++ 8 files changed, 803 insertions(+), 340 deletions(-) create mode 100644 gtsam_unstable/nonlinear/FixedLagSmoother.cpp create mode 100644 gtsam_unstable/nonlinear/FixedLagSmoother.h create mode 100644 gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp create mode 100644 gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h create mode 100644 gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp index a3e063da1..cbf884ff6 100644 --- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp @@ -11,126 +11,38 @@ /** * @file BatchFixedLagSmoother.cpp - * @brief An LM-based fixed-lag smoother. To the extent possible, this class mimics the iSAM2 - * interface. However, additional parameters, such as the smoother lag and the timestamp associated - * with each variable are needed. + * @brief An LM-based fixed-lag smoother. * * @author Michael Kaess, Stephen Williams * @date Oct 14, 2012 */ #include -#include +#include #include #include #include -#include -#include #include namespace gtsam { -/* ************************************************************************* */ -void BatchFixedLagSmoother::PrintKeySet(const std::set& keys, const std::string& label) { - std::cout << label; - BOOST_FOREACH(gtsam::Key key, keys) { - std::cout << " " << gtsam::DefaultKeyFormatter(key); - } - std::cout << std::endl; -} - -/* ************************************************************************* */ -void BatchFixedLagSmoother::PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor, const Ordering& ordering) { - std::cout << "f("; - BOOST_FOREACH(Index index, factor->keys()) { - std::cout << " " << index << "[" << gtsam::DefaultKeyFormatter(ordering.key(index)) << "]"; - } - std::cout << " )" << std::endl; -} - -/* ************************************************************************* */ -void BatchFixedLagSmoother::PrintSymbolicGraph(const GaussianFactorGraph& graph, const Ordering& ordering, const std::string& label) { - std::cout << label << std::endl; - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, graph) { - PrintSymbolicFactor(factor, ordering); - } -} - -/* ************************************************************************* */ -void BatchFixedLagSmoother::PrintSymbolicFactor(const NonlinearFactor::shared_ptr& factor) { - std::cout << "f("; - if(factor) { - BOOST_FOREACH(Key key, factor->keys()) { - std::cout << " " << gtsam::DefaultKeyFormatter(key); - } - } else { - std::cout << " NULL"; - } - std::cout << " )" << std::endl; -} - -/* ************************************************************************* */ -void BatchFixedLagSmoother::PrintSymbolicGraph(const NonlinearFactorGraph& graph, const std::string& label) { - std::cout << label << std::endl; - BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, graph) { - PrintSymbolicFactor(factor); - } -} - - - - -/* ************************************************************************* */ -BatchFixedLagSmoother::BatchFixedLagSmoother(const LevenbergMarquardtParams& params, double smootherLag, bool enforceConsistency) : - parameters_(params), smootherLag_(smootherLag), enforceConsistency_(enforceConsistency) { -} - /* ************************************************************************* */ void BatchFixedLagSmoother::print(const std::string& s, const KeyFormatter& keyFormatter) const { - std::cout << s; + FixedLagSmoother::print(s, keyFormatter); + // TODO: What else to print? } /* ************************************************************************* */ -bool BatchFixedLagSmoother::equals(const BatchFixedLagSmoother& rhs, double tol) const { - return factors_.equals(rhs.factors_, tol) - && theta_.equals(rhs.theta_, tol) - && std::fabs(smootherLag_ - rhs.smootherLag_) < tol - && std::equal(timestampKeyMap_.begin(), timestampKeyMap_.end(), rhs.timestampKeyMap_.begin()); +bool BatchFixedLagSmoother::equals(const FixedLagSmoother& rhs, double tol) const { + const BatchFixedLagSmoother* e = dynamic_cast (&rhs); + return e != NULL + && FixedLagSmoother::equals(*e, tol) + && factors_.equals(e->factors_, tol) + && theta_.equals(e->theta_, tol); } /* ************************************************************************* */ -Matrix BatchFixedLagSmoother::marginalR(Key key) const { -// TODO: Fix This -// // Use iSAM2 object to get the marginal factor -// GaussianFactor::shared_ptr marginalGaussian; -// if(params().getFactorization() == "QR") -// marginalGaussian = isam_.marginalFactor(getOrdering().at(key), EliminateQR); -// else if(params().getFactorization() == "CHOLESKY") -// marginalGaussian = isam_.marginalFactor(getOrdering().at(key), EliminateCholesky); -// else -// throw std::invalid_argument(boost::str(boost::format("Encountered unknown factorization type of '%s'. Known types are 'QR' and 'CHOLESKY'.") % params().getFactorization())); -// -// // Extract the information matrix -// JacobianFactor::shared_ptr marginalJacobian = boost::dynamic_pointer_cast(marginalGaussian); -// assert(marginalJacobian != 0); -// return marginalJacobian->getA(marginalJacobian->begin()); - return Matrix(); -} - -/* ************************************************************************* */ -Matrix BatchFixedLagSmoother::marginalInformation(Key key) const { - Matrix R(marginalR(key)); - return R.transpose() * R; -} - -/* ************************************************************************* */ -Matrix BatchFixedLagSmoother::marginalCovariance(Key key) const { - Matrix Rinv(inverse(marginalR(key))); - return Rinv * Rinv.transpose(); -} - -/* ************************************************************************* */ -BatchFixedLagSmoother::Result BatchFixedLagSmoother::update(const NonlinearFactorGraph& newFactors, const Values& newTheta, const KeyTimestampMap& timestamps) { +FixedLagSmoother::Result BatchFixedLagSmoother::update(const NonlinearFactorGraph& newFactors, const Values& newTheta, const KeyTimestampMap& timestamps) { const bool debug = ISDEBUG("BatchFixedLagSmoother update"); if(debug) { @@ -249,118 +161,23 @@ void BatchFixedLagSmoother::removeFactors(const std::set& deleteFactors) } } -/* ************************************************************************* */ -void BatchFixedLagSmoother::updateKeyTimestampMap(const KeyTimestampMap& timestamps) { - // Loop through each key and add/update it in the map - BOOST_FOREACH(const KeyTimestampMap::value_type& key_timestamp, timestamps) { - // Check to see if this key already exists inthe database - KeyTimestampMap::iterator keyIter = keyTimestampMap_.find(key_timestamp.first); - - // If the key already exists - if(keyIter != keyTimestampMap_.end()) { - // Find the entry in the Timestamp-Key database - std::pair range = timestampKeyMap_.equal_range(keyIter->second); - TimestampKeyMap::iterator timeIter = range.first; - while(timeIter->second != key_timestamp.first) { - ++timeIter; - } - // remove the entry in the Timestamp-Key database - timestampKeyMap_.erase(timeIter); - // insert an entry at the new time - timestampKeyMap_.insert(TimestampKeyMap::value_type(key_timestamp.second, key_timestamp.first)); - // update the Key-Timestamp database - keyIter->second = key_timestamp.second; - } else { - // Add the Key-Timestamp database - keyTimestampMap_.insert(key_timestamp); - // Add the key to the Timestamp-Key database - timestampKeyMap_.insert(TimestampKeyMap::value_type(key_timestamp.second, key_timestamp.first)); - } - } -} - -/* ************************************************************************* */ -double BatchFixedLagSmoother::getCurrentTimestamp() const { - if(timestampKeyMap_.size() > 0) { - return timestampKeyMap_.rbegin()->first; - } else { - return -std::numeric_limits::max(); - } -} - -/* ************************************************************************* */ -std::set BatchFixedLagSmoother::findKeysBefore(double timestamp) const { - std::set keys; - TimestampKeyMap::const_iterator end = timestampKeyMap_.lower_bound(timestamp); - for(TimestampKeyMap::const_iterator iter = timestampKeyMap_.begin(); iter != end; ++iter) { - keys.insert(iter->second); - } - return keys; -} - /* ************************************************************************* */ void BatchFixedLagSmoother::eraseKeys(const std::set& keys) { -// bool debug = true; - -// if(debug) std::cout << "BatchFixedLagSmoother::eraseKeys() START" << std::endl; - -// if(debug) PrintKeySet(keys, "Keys To Erase: "); - BOOST_FOREACH(Key key, keys) { -// if(debug) std::cout << "Attempting to erase key " << DefaultKeyFormatter(key) << std::endl; - - // Erase the key from the Timestamp->Key map - double timestamp = keyTimestampMap_.at(key); - -// if(debug) std::cout << "Timestamp associated with key: " << timestamp << std::endl; - - TimestampKeyMap::iterator iter = timestampKeyMap_.lower_bound(timestamp); - while(iter != timestampKeyMap_.end() && iter->first == timestamp) { - if(iter->second == key) { - -// if(debug) { -// std::cout << "Contents of TimestampKeyMap before Erase:" << std::endl; -// BOOST_FOREACH(const TimestampKeyMap::value_type& timestamp_key, timestampKeyMap_) { -// std::cout << " " << timestamp_key.first << " -> " << DefaultKeyFormatter(timestamp_key.second) << std::endl; -// } -// } - - timestampKeyMap_.erase(iter++); - -// if(debug) { -// std::cout << "Contents of TimestampKeyMap before Erase:" << std::endl; -// BOOST_FOREACH(const TimestampKeyMap::value_type& timestamp_key, timestampKeyMap_) { -// std::cout << " " << timestamp_key.first << " -> " << DefaultKeyFormatter(timestamp_key.second) << std::endl; -// } -// } - -// if(debug) std::cout << "Erased 1 entry from timestampKeyMap_" << std::endl; - } else { - ++iter; - } - } - // Erase the key from the Key->Timestamp map - size_t ret; - ret = keyTimestampMap_.erase(key); -// if(debug) std::cout << "Erased " << ret << " entries from keyTimestampMap_" << std::endl; - // Erase the key from the values theta_.erase(key); -// if(debug) std::cout << "(Hopefully) Erased 1 entries from theta_" << std::endl; // Erase the key from the factor index - ret = factorIndex_.erase(key); -// if(debug) std::cout << "Erased " << ret << " entries from factorIndex_" << std::endl; + factorIndex_.erase(key); // Erase the key from the set of linearized keys if(linearizedKeys_.exists(key)) { linearizedKeys_.erase(key); -// if(debug) std::cout << "Erased 1 entry from linearizedKeys_" << std::endl; } } -// if(debug) std::cout << "BatchFixedLagSmoother::eraseKeys() FINISH" << std::endl; + eraseKeyTimestampMap(keys); } /* ************************************************************************* */ @@ -515,23 +332,17 @@ void BatchFixedLagSmoother::marginalizeKeys(const std::set& marginalizableK // These factors are all generated from BayesNet conditionals. They should all be Jacobians. JacobianFactor::shared_ptr jacobianFactor = boost::dynamic_pointer_cast(gaussianFactor); assert(jacobianFactor); - LinearizedGaussianFactor::shared_ptr factor = LinearizedJacobianFactor::shared_ptr(new LinearizedJacobianFactor(jacobianFactor, ordering, getLinearizationPoint())); + LinearizedGaussianFactor::shared_ptr factor = LinearizedJacobianFactor::shared_ptr(new LinearizedJacobianFactor(jacobianFactor, ordering, theta_)); // add it to the new factor set newFactors.push_back(factor); } - if(debug) std::cout << "1" << std::endl; - // (4) remove the marginalized factors from the graph removeFactors(factorTree.factors); - if(debug) std::cout << "2" << std::endl; - // (5) add the factors in this tree to the main set of factors updateFactors(newFactors); - if(debug) std::cout << "3" << std::endl; - // (6) add the keys involved in the linear(ized) factors to the linearizedKey list FastSet linearizedKeys = newFactors.keys(); BOOST_FOREACH(Key key, linearizedKeys) { @@ -539,25 +350,61 @@ void BatchFixedLagSmoother::marginalizeKeys(const std::set& marginalizableK linearizedKeys_.insert(key, theta_.at(key)); } } - - if(debug) std::cout << "4" << std::endl; - } - - if(debug) std::cout << "5" << std::endl; - - // Store the final estimate of each marginalized key - BOOST_FOREACH(Key key, marginalizableKeys) { - thetaFinal_.insert(key, theta_.at(key)); } // Remove the marginalized keys from the smoother data structures eraseKeys(marginalizableKeys); - - if(debug) std::cout << "6" << std::endl; } if(debug) std::cout << "BatchFixedLagSmoother::marginalizeKeys() FINISH" << std::endl; } +/* ************************************************************************* */ +void BatchFixedLagSmoother::PrintKeySet(const std::set& keys, const std::string& label) { + std::cout << label; + BOOST_FOREACH(gtsam::Key key, keys) { + std::cout << " " << gtsam::DefaultKeyFormatter(key); + } + std::cout << std::endl; +} + +/* ************************************************************************* */ +void BatchFixedLagSmoother::PrintSymbolicFactor(const NonlinearFactor::shared_ptr& factor) { + std::cout << "f("; + if(factor) { + BOOST_FOREACH(Key key, factor->keys()) { + std::cout << " " << gtsam::DefaultKeyFormatter(key); + } + } else { + std::cout << " NULL"; + } + std::cout << " )" << std::endl; +} + +/* ************************************************************************* */ +void BatchFixedLagSmoother::PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor, const Ordering& ordering) { + std::cout << "f("; + BOOST_FOREACH(Index index, factor->keys()) { + std::cout << " " << index << "[" << gtsam::DefaultKeyFormatter(ordering.key(index)) << "]"; + } + std::cout << " )" << std::endl; +} + +/* ************************************************************************* */ +void BatchFixedLagSmoother::PrintSymbolicGraph(const NonlinearFactorGraph& graph, const std::string& label) { + std::cout << label << std::endl; + BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, graph) { + PrintSymbolicFactor(factor); + } +} + +/* ************************************************************************* */ +void BatchFixedLagSmoother::PrintSymbolicGraph(const GaussianFactorGraph& graph, const Ordering& ordering, const std::string& label) { + std::cout << label << std::endl; + BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, graph) { + PrintSymbolicFactor(factor, ordering); + } +} + /* ************************************************************************* */ } /// namespace gtsam diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h index ec7dc9cb6..12c9aba1d 100644 --- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h +++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h @@ -11,9 +11,7 @@ /** * @file BatchFixedLagSmoother.h - * @brief An LM-based fixed-lag smoother. To the extent possible, this class mimics the iSAM2 - * interface. However, additional parameters, such as the smoother lag and the timestamp associated - * with each variable are needed. + * @brief An LM-based fixed-lag smoother. * * @author Michael Kaess, Stephen Williams * @date Oct 14, 2012 @@ -22,72 +20,41 @@ // \callgraph #pragma once -#include +#include #include -#include - #include namespace gtsam { -class BatchFixedLagSmoother { +class BatchFixedLagSmoother : public FixedLagSmoother { public: /// Typedef for a shared pointer to an Incremental Fixed-Lag Smoother typedef boost::shared_ptr shared_ptr; - /// Typedef for a Key-Timestamp map/database - typedef std::map KeyTimestampMap; - typedef std::multimap TimestampKeyMap; - - /** - * Meta information returned about the update - */ - // TODO: Think of some more things to put here - struct Result { - size_t iterations; ///< The number of optimizer iterations performed - size_t nonlinearVariables; ///< The number of variables that can be relinearized - size_t linearVariables; ///< The number of variables that must keep a constant linearization point - double error; ///< The final factor graph error - Result() : iterations(0), nonlinearVariables(0), linearVariables(0), error(0) {}; - }; - - /** default constructor */ - BatchFixedLagSmoother(const LevenbergMarquardtParams& parameters = LevenbergMarquardtParams(), double smootherLag = 0.0, bool enforceConsistency = false); + BatchFixedLagSmoother(double smootherLag = 0.0, const LevenbergMarquardtParams& parameters = LevenbergMarquardtParams(), bool enforceConsistency = false) : + FixedLagSmoother(smootherLag), parameters_(parameters), enforceConsistency_(enforceConsistency) { }; /** destructor */ virtual ~BatchFixedLagSmoother() { }; - // TODO: Create a better 'print' /** Print the factor for debugging and testing (implementing Testable) */ virtual void print(const std::string& s = "BatchFixedLagSmoother:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; /** Check if two IncrementalFixedLagSmoother Objects are equal */ - virtual bool equals(const BatchFixedLagSmoother& rhs, double tol = 1e-9) const; + virtual bool equals(const FixedLagSmoother& rhs, double tol = 1e-9) const; - /** - * Add new factors, updating the solution and relinearizing as needed. - */ - Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(), const KeyTimestampMap& timestamps = KeyTimestampMap()); - - /** Access the current set of timestamps associated with each variable */ - const KeyTimestampMap& getTimestamps() const { - return keyTimestampMap_; - } - - /** Access the current linearization point */ - const Values& getLinearizationPoint() const { - return theta_; - } + /** Add new factors, updating the solution and relinearizing as needed. */ + Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(), + const KeyTimestampMap& timestamps = KeyTimestampMap()); /** Compute an estimate from the incomplete linear delta computed during the last update. * This delta is incomplete because it was not updated below wildfire_threshold. If only * a single variable is needed, it is faster to call calculateEstimate(const KEY&). */ Values calculateEstimate() const { - //return theta_.retract(delta_, ordering_); return theta_; } @@ -99,94 +66,48 @@ public: */ template VALUE calculateEstimate(Key key) const { - //const Index index = ordering_.at(key); - //const SubVector delta = delta_.at(index); - //return theta_.at(key).retract(delta); return theta_.at(key); } - /** Compute an estimate using a complete delta computed by a full back-substitution. - */ - Values calculateBestEstimate() const { - return calculateEstimate(); - } - - /** Return the final/best estimate of each variable encountered, including those that have been marginalized out previously. - */ - Values calculateFinalEstimate() const { - Values final(thetaFinal_); - final.insert(theta_); - return final; - } - - - /** Access the set of nonlinear factors */ - const NonlinearFactorGraph& getFactorsUnsafe() const { - return factors_; - } - - /** return the current set of iSAM2 parameters */ + /** read the current set of optimizer parameters */ const LevenbergMarquardtParams& params() const { return parameters_; } - /** return the current smoother lag */ - double smootherLag() const { - return smootherLag_; + /** update the current set of optimizer parameters */ + LevenbergMarquardtParams& params() { + return parameters_; } - - - /** return the marginal square root information matrix associated with a specific variable */ - Matrix marginalR(Key key) const; - - /** return the marginal information matrix associated with a specific variable */ - Matrix marginalInformation(Key key) const; - - /** return the marginal covariance associated with a specific variable */ - Matrix marginalCovariance(Key key) const; - - - protected: + /** A typedef defining an Key-Factor mapping **/ + typedef std::map > FactorIndex; + + /** The L-M optimization parameters **/ + LevenbergMarquardtParams parameters_; + + /** A flag indicating if the optimizer should enforce probabilistic consistency by maintaining the + * linearization point of all variables involved in linearized/marginal factors at the edge of the + * smoothing window. This idea is from ??? TODO: Look up paper reference **/ + bool enforceConsistency_; + /** The nonlinear factors **/ NonlinearFactorGraph factors_; /** The current linearization point **/ Values theta_; - /** The final estimate of each variable **/ - Values thetaFinal_; - - /** The L-M optimization parameters **/ - LevenbergMarquardtParams parameters_; - - /** The length of the smoother lag. Any variable older than this amount will be marginalized out. */ - double smootherLag_; - - /** A flag indicating if the optimizer should enforce probabilistic consistency by maintaining the - * linearization point of all variables involved in linearized/marginal factors at the edge of the - * smoothing window. This idea is from ??? TODO: Look up paper reference **/ - bool enforceConsistency_; - - /** The current timestamp associated with each tracked key */ - TimestampKeyMap timestampKeyMap_; - KeyTimestampMap keyTimestampMap_; - - /** A typedef defining an Key-Factor mapping **/ - typedef std::map > FactorIndex; - - /** A cross-reference structure to allow efficient factor lookups by key **/ - FactorIndex factorIndex_; - /** The set of keys involved in current linearized factors. These keys should not be relinearized. **/ - //std::set linearizedKeys_; Values linearizedKeys_; /** The set of available factor graph slots. These occur because we are constantly deleting factors, leaving holes. **/ std::queue availableSlots_; + /** A cross-reference structure to allow efficient factor lookups by key **/ + FactorIndex factorIndex_; + + /** Augment the list of factors with a set of new factors */ void updateFactors(const NonlinearFactorGraph& newFactors); @@ -194,15 +115,6 @@ protected: /** Remove factors from the list of factors by slot index */ void removeFactors(const std::set& deleteFactors); - /** Update the Timestamps associated with the keys */ - void updateKeyTimestampMap(const KeyTimestampMap& newTimestamps); - - /** Find the most recent timestamp of the system */ - double getCurrentTimestamp() const; - - /** Find all of the keys associated with timestamps before the provided time */ - std::set findKeysBefore(double timestamp) const; - /** Erase any keys associated with timestamps before the provided time */ void eraseKeys(const std::set& keys); @@ -211,17 +123,12 @@ protected: private: - + /** Private methods for printing debug information */ static void PrintKeySet(const std::set& keys, const std::string& label); - - static void PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor, const Ordering& ordering); - - static void PrintSymbolicGraph(const GaussianFactorGraph& graph, const Ordering& ordering, const std::string& label); - static void PrintSymbolicFactor(const NonlinearFactor::shared_ptr& factor); - + static void PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor, const Ordering& ordering); static void PrintSymbolicGraph(const NonlinearFactorGraph& graph, const std::string& label); - + static void PrintSymbolicGraph(const GaussianFactorGraph& graph, const Ordering& ordering, const std::string& label); }; // BatchFixedLagSmoother } /// namespace gtsam diff --git a/gtsam_unstable/nonlinear/FixedLagSmoother.cpp b/gtsam_unstable/nonlinear/FixedLagSmoother.cpp new file mode 100644 index 000000000..66d367148 --- /dev/null +++ b/gtsam_unstable/nonlinear/FixedLagSmoother.cpp @@ -0,0 +1,115 @@ +/* ---------------------------------------------------------------------------- + + * 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 FixedLagSmoother.cpp + * @brief The base class for different fixed-lag smoother implementations. + * + * @author Stephen Williams + * @date Feb 27, 2013 + */ + +#include + +namespace gtsam { + +/* ************************************************************************* */ +void FixedLagSmoother::print(const std::string& s, const KeyFormatter& keyFormatter) const { + std::cout << s; + std::cout << " smoother lag: " << smootherLag_ << std::endl; +} + +/* ************************************************************************* */ +bool FixedLagSmoother::equals(const FixedLagSmoother& rhs, double tol) const { + return std::fabs(smootherLag_ - rhs.smootherLag_) < tol + && std::equal(timestampKeyMap_.begin(), timestampKeyMap_.end(), rhs.timestampKeyMap_.begin()); +} + +/* ************************************************************************* */ +void FixedLagSmoother::updateKeyTimestampMap(const KeyTimestampMap& timestamps) { + // Loop through each key and add/update it in the map + BOOST_FOREACH(const KeyTimestampMap::value_type& key_timestamp, timestamps) { + // Check to see if this key already exists inthe database + KeyTimestampMap::iterator keyIter = keyTimestampMap_.find(key_timestamp.first); + + // If the key already exists + if(keyIter != keyTimestampMap_.end()) { + // Find the entry in the Timestamp-Key database + std::pair range = timestampKeyMap_.equal_range(keyIter->second); + TimestampKeyMap::iterator timeIter = range.first; + while(timeIter->second != key_timestamp.first) { + ++timeIter; + } + // remove the entry in the Timestamp-Key database + timestampKeyMap_.erase(timeIter); + // insert an entry at the new time + timestampKeyMap_.insert(TimestampKeyMap::value_type(key_timestamp.second, key_timestamp.first)); + // update the Key-Timestamp database + keyIter->second = key_timestamp.second; + } else { + // Add the Key-Timestamp database + keyTimestampMap_.insert(key_timestamp); + // Add the key to the Timestamp-Key database + timestampKeyMap_.insert(TimestampKeyMap::value_type(key_timestamp.second, key_timestamp.first)); + } + } +} + +/* ************************************************************************* */ +void FixedLagSmoother::eraseKeyTimestampMap(const std::set& keys) { + BOOST_FOREACH(Key key, keys) { + // Erase the key from the Timestamp->Key map + double timestamp = keyTimestampMap_.at(key); + + TimestampKeyMap::iterator iter = timestampKeyMap_.lower_bound(timestamp); + while(iter != timestampKeyMap_.end() && iter->first == timestamp) { + if(iter->second == key) { + timestampKeyMap_.erase(iter++); + } else { + ++iter; + } + } + // Erase the key from the Key->Timestamp map + keyTimestampMap_.erase(key); + } +} + +/* ************************************************************************* */ +double FixedLagSmoother::getCurrentTimestamp() const { + if(timestampKeyMap_.size() > 0) { + return timestampKeyMap_.rbegin()->first; + } else { + return -std::numeric_limits::max(); + } +} + +/* ************************************************************************* */ +std::set FixedLagSmoother::findKeysBefore(double timestamp) const { + std::set keys; + TimestampKeyMap::const_iterator end = timestampKeyMap_.lower_bound(timestamp); + for(TimestampKeyMap::const_iterator iter = timestampKeyMap_.begin(); iter != end; ++iter) { + keys.insert(iter->second); + } + return keys; +} + +/* ************************************************************************* */ +std::set FixedLagSmoother::findKeysAfter(double timestamp) const { + std::set keys; + TimestampKeyMap::const_iterator begin = timestampKeyMap_.upper_bound(timestamp); + for(TimestampKeyMap::const_iterator iter = begin; iter != timestampKeyMap_.end(); ++iter) { + keys.insert(iter->second); + } + return keys; +} + +/* ************************************************************************* */ +} /// namespace gtsam diff --git a/gtsam_unstable/nonlinear/FixedLagSmoother.h b/gtsam_unstable/nonlinear/FixedLagSmoother.h new file mode 100644 index 000000000..dcdd27771 --- /dev/null +++ b/gtsam_unstable/nonlinear/FixedLagSmoother.h @@ -0,0 +1,118 @@ +/* ---------------------------------------------------------------------------- + + * 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 FixedLagSmoother.h + * @brief Base class for a fixed-lag smoother. This mimics the basic interface to iSAM2. + * + * @author Stephen Williams + * @date Feb 27, 2013 + */ + +// \callgraph +#pragma once + +#include +#include +#include +#include + +namespace gtsam { + +class FixedLagSmoother { + +public: + + /// Typedef for a shared pointer to an Incremental Fixed-Lag Smoother + typedef boost::shared_ptr shared_ptr; + + /// Typedef for a Key-Timestamp map/database + typedef std::map KeyTimestampMap; + typedef std::multimap TimestampKeyMap; + + /** + * Meta information returned about the update + */ + // TODO: Think of some more things to put here + struct Result { + size_t iterations; ///< The number of optimizer iterations performed + size_t nonlinearVariables; ///< The number of variables that can be relinearized + size_t linearVariables; ///< The number of variables that must keep a constant linearization point + double error; ///< The final factor graph error + Result() : iterations(0), nonlinearVariables(0), linearVariables(0), error(0) {}; + }; + + + /** default constructor */ + FixedLagSmoother(double smootherLag = 0.0) : smootherLag_(smootherLag) { }; + + /** destructor */ + virtual ~FixedLagSmoother() { }; + + /** Print the factor for debugging and testing (implementing Testable) */ + virtual void print(const std::string& s = "FixedLagSmoother:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + + /** Check if two IncrementalFixedLagSmoother Objects are equal */ + virtual bool equals(const FixedLagSmoother& rhs, double tol = 1e-9) const; + + /** Add new factors, updating the solution and relinearizing as needed. */ + virtual Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(), + const KeyTimestampMap& timestamps = KeyTimestampMap()) = 0; + + /** Access the current set of timestamps associated with each variable */ + const KeyTimestampMap& getTimestamps() const { + return keyTimestampMap_; + } + + /** Compute an estimate from the incomplete linear delta computed during the last update. + * This delta is incomplete because it was not updated below wildfire_threshold. If only + * a single variable is needed, it is faster to call calculateEstimate(const KEY&). + */ + virtual Values calculateEstimate() const = 0; + + /** read the current smoother lag */ + double smootherLag() const { + return smootherLag_; + } + + /** write to the current smoother lag */ + double& smootherLag() { + return smootherLag_; + } + + +protected: + + /** The length of the smoother lag. Any variable older than this amount will be marginalized out. */ + double smootherLag_; + + /** The current timestamp associated with each tracked key */ + TimestampKeyMap timestampKeyMap_; + KeyTimestampMap keyTimestampMap_; + + /** Update the Timestamps associated with the keys */ + void updateKeyTimestampMap(const KeyTimestampMap& newTimestamps); + + /** Erase keys from the Key-Timestamps database */ + void eraseKeyTimestampMap(const std::set& keys); + + /** Find the most recent timestamp of the system */ + double getCurrentTimestamp() const; + + /** Find all of the keys associated with timestamps before the provided time */ + std::set findKeysBefore(double timestamp) const; + + /** Find all of the keys associated with timestamps before the provided time */ + std::set findKeysAfter(double timestamp) const; + +}; // FixedLagSmoother + +} /// namespace gtsam diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp new file mode 100644 index 000000000..708c34bef --- /dev/null +++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp @@ -0,0 +1,188 @@ +/* ---------------------------------------------------------------------------- + + * 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 IncrementalFixedLagSmoother.cpp + * @brief An iSAM2-based fixed-lag smoother. To the extent possible, this class mimics the iSAM2 + * interface. However, additional parameters, such as the smoother lag and the timestamp associated + * with each variable are needed. + * + * @author Michael Kaess, Stephen Williams + * @date Oct 14, 2012 + */ + +#include +#include + +namespace gtsam { + +/* ************************************************************************* */ +void IncrementalFixedLagSmoother::print(const std::string& s, const KeyFormatter& keyFormatter) const { + FixedLagSmoother::print(s, keyFormatter); + // TODO: What else to print? +} + +/* ************************************************************************* */ +bool IncrementalFixedLagSmoother::equals(const FixedLagSmoother& rhs, double tol) const { + const IncrementalFixedLagSmoother* e = dynamic_cast (&rhs); + return e != NULL + && FixedLagSmoother::equals(*e, tol) + && isam_.equals(e->isam_, tol); +} + +/* ************************************************************************* */ +FixedLagSmoother::Result IncrementalFixedLagSmoother::update(const NonlinearFactorGraph& newFactors, const Values& newTheta, const KeyTimestampMap& timestamps) { + + const bool debug = ISDEBUG("IncrementalFixedLagSmoother update"); + if(debug) { + std::cout << "IncrementalFixedLagSmoother::update()" << std::endl; + PrintSymbolicTree(isam_, "Bayes Tree Before Update:"); + } + + FastVector removedFactors; + FastMap constrainedKeys; + + // Update the Timestamps associated with the factor keys + updateKeyTimestampMap(timestamps); + + // Get current timestamp + double current_timestamp = getCurrentTimestamp(); + if(debug) std::cout << "Current Timestamp: " << current_timestamp << std::endl; + + // Find the set of variables to be marginalized out + std::set marginalizableKeys = findKeysBefore(current_timestamp - smootherLag_); + if(debug) { + std::cout << "Marginalizable Keys: "; + BOOST_FOREACH(Key key, marginalizableKeys) { + std::cout << DefaultKeyFormatter(key) << " "; + } + std::cout << std::endl; + } + + // Force iSAM2 to put the marginalizable variables at the beginning + createOrderingConstraints(marginalizableKeys, constrainedKeys); + if(debug) { + std::cout << "Constrained Keys: "; + for(FastMap::const_iterator iter = constrainedKeys.begin(); iter != constrainedKeys.end(); ++iter) { + std::cout << DefaultKeyFormatter(iter->first) << "(" << iter->second << ") "; + } + std::cout << std::endl; + } + + // Update iSAM2 + ISAM2Result isamResult = isam_.update(newFactors, newTheta, FastVector(), constrainedKeys); + + if(debug) { + PrintSymbolicTree(isam_, "Bayes Tree After Update, Before Marginalization:"); + } + + // Marginalize out any needed variables + FastList leafKeys(marginalizableKeys.begin(), marginalizableKeys.end()); + isam_.experimentalMarginalizeLeaves(leafKeys); + // Remove marginalized keys from the KeyTimestampMap + eraseKeyTimestampMap(marginalizableKeys); + + if(debug) { + PrintSymbolicTree(isam_, "Bayes Tree After Marginalization:"); + } + + // TODO: Fill in result structure + Result result; + result.iterations = 1; + result.linearVariables = 0; + result.nonlinearVariables = 0; + result.error = 0; + + return result; +} + +/* ************************************************************************* */ +void IncrementalFixedLagSmoother::eraseKeysBefore(double timestamp) { + TimestampKeyMap::iterator end = timestampKeyMap_.lower_bound(timestamp); + TimestampKeyMap::iterator iter = timestampKeyMap_.begin(); + while(iter != end) { + keyTimestampMap_.erase(iter->second); + timestampKeyMap_.erase(iter++); + } +} + +/* ************************************************************************* */ +void IncrementalFixedLagSmoother::createOrderingConstraints(const std::set& marginalizableKeys, FastMap& constrainedKeys) const { + if(marginalizableKeys.size() > 0) { + // Generate ordering constraints so that the marginalizable variables will be eliminated first + // Set all variables to Group1 + BOOST_FOREACH(const TimestampKeyMap::value_type& timestamp_key, timestampKeyMap_) { + constrainedKeys[timestamp_key.second] = 1; + } + // Set marginalizable variables to Group0 + BOOST_FOREACH(Key key, marginalizableKeys){ + constrainedKeys[key] = 0; + } + } +} + +/* ************************************************************************* */ +void IncrementalFixedLagSmoother::PrintKeySet(const std::set& keys, const std::string& label) { + std::cout << label; + BOOST_FOREACH(gtsam::Key key, keys) { + std::cout << " " << gtsam::DefaultKeyFormatter(key); + } + std::cout << std::endl; +} + +/* ************************************************************************* */ +void IncrementalFixedLagSmoother::PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor, const gtsam::Ordering& ordering) { + std::cout << "f("; + BOOST_FOREACH(Index index, factor->keys()) { + std::cout << " " << index << "[" << gtsam::DefaultKeyFormatter(ordering.key(index)) << "]"; + } + std::cout << " )" << std::endl; +} + +/* ************************************************************************* */ +void IncrementalFixedLagSmoother::PrintSymbolicGraph(const GaussianFactorGraph& graph, const gtsam::Ordering& ordering, const std::string& label) { + std::cout << label << std::endl; + BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, graph) { + PrintSymbolicFactor(factor, ordering); + } +} + +/* ************************************************************************* */ +void IncrementalFixedLagSmoother::PrintSymbolicTree(const gtsam::ISAM2& isam, const std::string& label) { + std::cout << label << std::endl; + if(isam.root()) + PrintSymbolicTreeHelper(isam.root(), isam.getOrdering()); + else + std::cout << "{Empty Tree}" << std::endl; +} + +/* ************************************************************************* */ +void IncrementalFixedLagSmoother::PrintSymbolicTreeHelper(const gtsam::ISAM2Clique::shared_ptr& clique, const gtsam::Ordering& ordering, const std::string indent) { + + // Print the current clique + std::cout << indent << "P( "; + BOOST_FOREACH(gtsam::Index index, clique->conditional()->frontals()) { + std::cout << gtsam::DefaultKeyFormatter(ordering.key(index)) << " "; + } + if(clique->conditional()->nrParents() > 0) std::cout << "| "; + BOOST_FOREACH(gtsam::Index index, clique->conditional()->parents()) { + std::cout << gtsam::DefaultKeyFormatter(ordering.key(index)) << " "; + } + std::cout << ")" << std::endl; + + // Recursively print all of the children + BOOST_FOREACH(const gtsam::ISAM2Clique::shared_ptr& child, clique->children()) { + PrintSymbolicTreeHelper(child, ordering, indent+" "); + } +} + +/* ************************************************************************* */ +} /// namespace gtsam diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h new file mode 100644 index 000000000..6a5d7e98d --- /dev/null +++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h @@ -0,0 +1,103 @@ +/* ---------------------------------------------------------------------------- + + * 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 IncrementalFixedLagSmoother.h + * @brief An iSAM2-based fixed-lag smoother. + * + * @author Michael Kaess, Stephen Williams + * @date Oct 14, 2012 + */ + +// \callgraph +#pragma once + +#include +#include + + +namespace gtsam { + +/** + * This is a base class for the various HMF2 implementations. The HMF2 eliminates the factor graph + * such that the active states are placed in/near the root. This base class implements a function + * to calculate the ordering, and an update function to incorporate new factors into the HMF. + */ +class IncrementalFixedLagSmoother : public FixedLagSmoother { + +public: + + /// Typedef for a shared pointer to an Incremental Fixed-Lag Smoother + typedef boost::shared_ptr shared_ptr; + + /** default constructor */ + IncrementalFixedLagSmoother(double smootherLag = 0.0, const ISAM2Params& parameters = ISAM2Params()) : + FixedLagSmoother(smootherLag), isam_(parameters) { }; + + /** destructor */ + virtual ~IncrementalFixedLagSmoother() { }; + + /** Print the factor for debugging and testing (implementing Testable) */ + virtual void print(const std::string& s = "IncrementalFixedLagSmoother:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + + /** Check if two IncrementalFixedLagSmoother Objects are equal */ + virtual bool equals(const FixedLagSmoother& rhs, double tol = 1e-9) const; + + /** Add new factors, updating the solution and relinearizing as needed. */ + Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(), + const KeyTimestampMap& timestamps = KeyTimestampMap()); + + /** Compute an estimate from the incomplete linear delta computed during the last update. + * This delta is incomplete because it was not updated below wildfire_threshold. If only + * a single variable is needed, it is faster to call calculateEstimate(const KEY&). + */ + Values calculateEstimate() const { + return isam_.calculateEstimate(); + } + + /** Compute an estimate for a single variable using its incomplete linear delta computed + * during the last update. This is faster than calling the no-argument version of + * calculateEstimate, which operates on all variables. + * @param key + * @return + */ + template + VALUE calculateEstimate(Key key) const { + return isam_.calculateEstimate(key); + } + + /** return the current set of iSAM2 parameters */ + const ISAM2Params& params() const { + return isam_.params(); + } + +protected: + /** An iSAM2 object used to perform inference. The smoother lag is controlled + * by what factors are removed each iteration */ + ISAM2 isam_; + + /** Erase any keys associated with timestamps before the provided time */ + void eraseKeysBefore(double timestamp); + + /** Fill in an iSAM2 ConstrainedKeys structure such that the provided keys are eliminated before all others */ + void createOrderingConstraints(const std::set& marginalizableKeys, FastMap& constrainedKeys) const; + +private: + /** Private methods for printing debug information */ + static void PrintKeySet(const std::set& keys, const std::string& label = "Keys:"); + static void PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor, const gtsam::Ordering& ordering); + static void PrintSymbolicGraph(const GaussianFactorGraph& graph, const gtsam::Ordering& ordering, const std::string& label = "Factor Graph:"); + static void PrintSymbolicTree(const gtsam::ISAM2& isam, const std::string& label = "Bayes Tree:"); + static void PrintSymbolicTreeHelper(const gtsam::ISAM2Clique::shared_ptr& clique, const gtsam::Ordering& ordering, const std::string indent = ""); + +}; // IncrementalFixedLagSmoother + +} /// namespace gtsam diff --git a/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp index c3480d2db..09fe3c381 100644 --- a/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp @@ -16,8 +16,8 @@ * @date May 23, 2012 */ -#include #include +#include #include #include #include @@ -62,7 +62,7 @@ TEST_UNSAFE( BatchFixedLagSmoother, Example ) // Create a Fixed-Lag Smoother typedef BatchFixedLagSmoother::KeyTimestampMap Timestamps; - BatchFixedLagSmoother smoother(LevenbergMarquardtParams(), 7.0); + BatchFixedLagSmoother smoother(7.0, LevenbergMarquardtParams()); // Create containers to keep the full graph Values fullinit; diff --git a/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp new file mode 100644 index 000000000..73bb8c0a3 --- /dev/null +++ b/gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp @@ -0,0 +1,185 @@ +/* ---------------------------------------------------------------------------- + + * 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 testIncrementalFixedLagSmoother.cpp + * @brief Unit tests for the Incremental Fixed-Lag Smoother + * @author Stephen Williams (swilliams8@gatech.edu) + * @date May 23, 2012 + */ + + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +Key MakeKey(size_t index) { return Symbol('x', index); } + +/* ************************************************************************* */ +bool check_smoother(const NonlinearFactorGraph& fullgraph, const Values& fullinit, const IncrementalFixedLagSmoother& smoother, const Key& key) { + + Ordering ordering = *fullgraph.orderingCOLAMD(fullinit); + GaussianFactorGraph linearized = *fullgraph.linearize(fullinit, ordering); + GaussianBayesNet gbn = *GaussianSequentialSolver(linearized).eliminate(); + VectorValues delta = optimize(gbn); + Values fullfinal = fullinit.retract(delta, ordering); + + Point2 expected = fullfinal.at(key); + Point2 actual = smoother.calculateEstimate(key); + + return assert_equal(expected, actual); +} + +/* ************************************************************************* */ +TEST_UNSAFE( IncrementalFixedLagSmoother, Example ) +{ + // Test the IncrementalFixedLagSmoother in a pure linear environment. Thus, full optimization and + // the IncrementalFixedLagSmoother should be identical (even with the linearized approximations at + // the end of the smoothing lag) + + SETDEBUG("IncrementalFixedLagSmoother update", true); + + // Set up parameters + SharedDiagonal odometerNoise = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); + SharedDiagonal loopNoise = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); + + // Create a Fixed-Lag Smoother + typedef IncrementalFixedLagSmoother::KeyTimestampMap Timestamps; + IncrementalFixedLagSmoother smoother(7.0, ISAM2Params()); + + // Create containers to keep the full graph + Values fullinit; + NonlinearFactorGraph fullgraph; + + + + // i keeps track of the time step + size_t i = 0; + + // Add a prior at time 0 and update the HMF + { + Key key0 = MakeKey(0); + + NonlinearFactorGraph newFactors; + Values newValues; + Timestamps newTimestamps; + + newFactors.add(PriorFactor(key0, Point2(0.0, 0.0), odometerNoise)); + newValues.insert(key0, Point2(0.01, 0.01)); + newTimestamps[key0] = 0.0; + + fullgraph.push_back(newFactors); + fullinit.insert(newValues); + + // Update the smoother + smoother.update(newFactors, newValues, newTimestamps); + + // Check + CHECK(check_smoother(fullgraph, fullinit, smoother, key0)); + + ++i; + } + + // Add odometry from time 0 to time 5 + while(i <= 5) { + Key key1 = MakeKey(i-1); + Key key2 = MakeKey(i); + + NonlinearFactorGraph newFactors; + Values newValues; + Timestamps newTimestamps; + + newFactors.add(BetweenFactor(key1, key2, Point2(1.0, 0.0), odometerNoise)); + newValues.insert(key2, Point2(double(i)+0.1, -0.1)); + newTimestamps[key2] = double(i); + + fullgraph.push_back(newFactors); + fullinit.insert(newValues); + + // Update the smoother + smoother.update(newFactors, newValues, newTimestamps); + + // Check + CHECK(check_smoother(fullgraph, fullinit, smoother, key2)); + + ++i; + } + + // Add odometry from time 5 to 6 to the HMF and a loop closure at time 5 to the TSM + { + // Add the odometry factor to the HMF + Key key1 = MakeKey(i-1); + Key key2 = MakeKey(i); + + NonlinearFactorGraph newFactors; + Values newValues; + Timestamps newTimestamps; + + newFactors.add(BetweenFactor(key1, key2, Point2(1.0, 0.0), odometerNoise)); + newFactors.add(BetweenFactor(MakeKey(2), MakeKey(5), Point2(3.5, 0.0), loopNoise)); + newValues.insert(key2, Point2(double(i)+0.1, -0.1)); + newTimestamps[key2] = double(i); + + fullgraph.push_back(newFactors); + fullinit.insert(newValues); + + // Update the smoother + smoother.update(newFactors, newValues, newTimestamps); + + // Check + CHECK(check_smoother(fullgraph, fullinit, smoother, key2)); + + ++i; + } + + // Add odometry from time 6 to time 15 + while(i <= 15) { + Key key1 = MakeKey(i-1); + Key key2 = MakeKey(i); + + NonlinearFactorGraph newFactors; + Values newValues; + Timestamps newTimestamps; + + newFactors.add(BetweenFactor(key1, key2, Point2(1.0, 0.0), odometerNoise)); + newValues.insert(key2, Point2(double(i)+0.1, -0.1)); + newTimestamps[key2] = double(i); + + fullgraph.push_back(newFactors); + fullinit.insert(newValues); + + // Update the smoother + smoother.update(newFactors, newValues, newTimestamps); + + // Check + CHECK(check_smoother(fullgraph, fullinit, smoother, key2)); + + ++i; + } + +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */