12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Added and incremental fixed-lag smoother using new iSAM2 marginalization functionality, and created a common base class for all fixed-lag smoother implementations.

release/4.3a0
Stephen Williams 2013-02-27 20:23:47 +00:00
parent 5270cd1d9c
commit 60d3ba2d0e
8 changed files with 803 additions and 340 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

275
gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp
View File

@ -11,126 +11,38 @@
/** /**
* @file BatchFixedLagSmoother.cpp * @file BatchFixedLagSmoother.cpp
* @brief An LM-based fixed-lag smoother. To the extent possible, this class mimics the iSAM2 * @brief An LM-based fixed-lag smoother.
* interface. However, additional parameters, such as the smoother lag and the timestamp associated
* with each variable are needed.
* *
* @author Michael Kaess, Stephen Williams * @author Michael Kaess, Stephen Williams
* @date Oct 14, 2012 * @date Oct 14, 2012
*/ */
#include <gtsam_unstable/nonlinear/BatchFixedLagSmoother.h> #include <gtsam_unstable/nonlinear/BatchFixedLagSmoother.h>
#include <gtsam/linear/GaussianSequentialSolver.h> #include <gtsam_unstable/nonlinear/LinearizedFactor.h>
#include <gtsam/linear/GaussianFactorGraph.h> #include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/GaussianFactor.h> #include <gtsam/linear/GaussianFactor.h>
#include <gtsam/inference/inference.h> #include <gtsam/inference/inference.h>
#include <gtsam/inference/VariableIndex.h>
#include <gtsam/inference/Permutation.h>
#include <gtsam/base/debug.h> #include <gtsam/base/debug.h>
namespace gtsam { namespace gtsam {
/* ************************************************************************* */
void BatchFixedLagSmoother::PrintKeySet(const std::set<Key>& 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 { 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 { bool BatchFixedLagSmoother::equals(const FixedLagSmoother& rhs, double tol) const {
return factors_.equals(rhs.factors_, tol) const BatchFixedLagSmoother* e = dynamic_cast<const BatchFixedLagSmoother*> (&rhs);
&& theta_.equals(rhs.theta_, tol) return e != NULL
&& std::fabs(smootherLag_ - rhs.smootherLag_) < tol && FixedLagSmoother::equals(*e, tol)
&& std::equal(timestampKeyMap_.begin(), timestampKeyMap_.end(), rhs.timestampKeyMap_.begin()); && factors_.equals(e->factors_, tol)
&& theta_.equals(e->theta_, tol);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Matrix BatchFixedLagSmoother::marginalR(Key key) const { FixedLagSmoother::Result BatchFixedLagSmoother::update(const NonlinearFactorGraph& newFactors, const Values& newTheta, const KeyTimestampMap& timestamps) {
// 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<JacobianFactor>(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) {
const bool debug = ISDEBUG("BatchFixedLagSmoother update"); const bool debug = ISDEBUG("BatchFixedLagSmoother update");
if(debug) { if(debug) {
@ -249,118 +161,23 @@ void BatchFixedLagSmoother::removeFactors(const std::set<size_t>& 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<TimestampKeyMap::iterator,TimestampKeyMap::iterator> 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<double>::max();
}
}
/* ************************************************************************* */
std::set<Key> BatchFixedLagSmoother::findKeysBefore(double timestamp) const {
std::set<Key> 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<Key>& keys) { void BatchFixedLagSmoother::eraseKeys(const std::set<Key>& 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) { 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 // Erase the key from the values
theta_.erase(key); theta_.erase(key);
// if(debug) std::cout << "(Hopefully) Erased 1 entries from theta_" << std::endl;
// Erase the key from the factor index // Erase the key from the factor index
ret = factorIndex_.erase(key); factorIndex_.erase(key);
// if(debug) std::cout << "Erased " << ret << " entries from factorIndex_" << std::endl;
// Erase the key from the set of linearized keys // Erase the key from the set of linearized keys
if(linearizedKeys_.exists(key)) { if(linearizedKeys_.exists(key)) {
linearizedKeys_.erase(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<Key>& marginalizableK
// These factors are all generated from BayesNet conditionals. They should all be Jacobians. // These factors are all generated from BayesNet conditionals. They should all be Jacobians.
JacobianFactor::shared_ptr jacobianFactor = boost::dynamic_pointer_cast<JacobianFactor>(gaussianFactor); JacobianFactor::shared_ptr jacobianFactor = boost::dynamic_pointer_cast<JacobianFactor>(gaussianFactor);
assert(jacobianFactor); 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 // add it to the new factor set
newFactors.push_back(factor); newFactors.push_back(factor);
} }
if(debug) std::cout << "1" << std::endl;
// (4) remove the marginalized factors from the graph // (4) remove the marginalized factors from the graph
removeFactors(factorTree.factors); removeFactors(factorTree.factors);
if(debug) std::cout << "2" << std::endl;
// (5) add the factors in this tree to the main set of factors // (5) add the factors in this tree to the main set of factors
updateFactors(newFactors); updateFactors(newFactors);
if(debug) std::cout << "3" << std::endl;
// (6) add the keys involved in the linear(ized) factors to the linearizedKey list // (6) add the keys involved in the linear(ized) factors to the linearizedKey list
FastSet<Key> linearizedKeys = newFactors.keys(); FastSet<Key> linearizedKeys = newFactors.keys();
BOOST_FOREACH(Key key, linearizedKeys) { BOOST_FOREACH(Key key, linearizedKeys) {
@ -539,25 +350,61 @@ void BatchFixedLagSmoother::marginalizeKeys(const std::set<Key>& marginalizableK
linearizedKeys_.insert(key, theta_.at(key)); 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 // Remove the marginalized keys from the smoother data structures
eraseKeys(marginalizableKeys); eraseKeys(marginalizableKeys);
if(debug) std::cout << "6" << std::endl;
} }
if(debug) std::cout << "BatchFixedLagSmoother::marginalizeKeys() FINISH" << std::endl; if(debug) std::cout << "BatchFixedLagSmoother::marginalizeKeys() FINISH" << std::endl;
} }
/* ************************************************************************* */
void BatchFixedLagSmoother::PrintKeySet(const std::set<Key>& 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 } /// namespace gtsam

155
gtsam_unstable/nonlinear/BatchFixedLagSmoother.h
View File

@ -11,9 +11,7 @@
/** /**
* @file BatchFixedLagSmoother.h * @file BatchFixedLagSmoother.h
* @brief An LM-based fixed-lag smoother. To the extent possible, this class mimics the iSAM2 * @brief An LM-based fixed-lag smoother.
* interface. However, additional parameters, such as the smoother lag and the timestamp associated
* with each variable are needed.
* *
* @author Michael Kaess, Stephen Williams * @author Michael Kaess, Stephen Williams
* @date Oct 14, 2012 * @date Oct 14, 2012
@ -22,72 +20,41 @@
// \callgraph // \callgraph
#pragma once #pragma once
#include <gtsam_unstable/nonlinear/LinearizedFactor.h> #include <gtsam_unstable/nonlinear/FixedLagSmoother.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h> #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <queue> #include <queue>
namespace gtsam { namespace gtsam {
class BatchFixedLagSmoother { class BatchFixedLagSmoother : public FixedLagSmoother {
public: public:
/// Typedef for a shared pointer to an Incremental Fixed-Lag Smoother /// Typedef for a shared pointer to an Incremental Fixed-Lag Smoother
typedef boost::shared_ptr<BatchFixedLagSmoother> shared_ptr; typedef boost::shared_ptr<BatchFixedLagSmoother> shared_ptr;
/// Typedef for a Key-Timestamp map/database
typedef std::map<Key, double> KeyTimestampMap;
typedef std::multimap<double, Key> 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 */ /** 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 */ /** destructor */
virtual ~BatchFixedLagSmoother() { }; virtual ~BatchFixedLagSmoother() { };
// TODO: Create a better 'print'
/** Print the factor for debugging and testing (implementing Testable) */ /** Print the factor for debugging and testing (implementing Testable) */
virtual void print(const std::string& s = "BatchFixedLagSmoother:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; virtual void print(const std::string& s = "BatchFixedLagSmoother:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
/** Check if two IncrementalFixedLagSmoother Objects are equal */ /** 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. */
* Add new factors, updating the solution and relinearizing as needed. Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(),
*/ const KeyTimestampMap& timestamps = KeyTimestampMap());
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_;
}
/** Compute an estimate from the incomplete linear delta computed during the last update. /** 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 * 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&). * a single variable is needed, it is faster to call calculateEstimate(const KEY&).
*/ */
Values calculateEstimate() const { Values calculateEstimate() const {
//return theta_.retract(delta_, ordering_);
return theta_; return theta_;
} }
@ -99,94 +66,48 @@ public:
*/ */
template<class VALUE> template<class VALUE>
VALUE calculateEstimate(Key key) const { VALUE calculateEstimate(Key key) const {
//const Index index = ordering_.at(key);
//const SubVector delta = delta_.at(index);
//return theta_.at<VALUE>(key).retract(delta);
return theta_.at<VALUE>(key); return theta_.at<VALUE>(key);
} }
/** Compute an estimate using a complete delta computed by a full back-substitution. /** read the current set of optimizer parameters */
*/
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 */
const LevenbergMarquardtParams& params() const { const LevenbergMarquardtParams& params() const {
return parameters_; return parameters_;
} }
/** return the current smoother lag */ /** update the current set of optimizer parameters */
double smootherLag() const { LevenbergMarquardtParams& params() {
return smootherLag_; 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: protected:
/** A typedef defining an Key-Factor mapping **/
typedef std::map<Key, std::set<Index> > 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 **/ /** The nonlinear factors **/
NonlinearFactorGraph factors_; NonlinearFactorGraph factors_;
/** The current linearization point **/ /** The current linearization point **/
Values theta_; 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<Key, std::set<Index> > 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. **/ /** The set of keys involved in current linearized factors. These keys should not be relinearized. **/
//std::set<Key> linearizedKeys_;
Values linearizedKeys_; Values linearizedKeys_;
/** The set of available factor graph slots. These occur because we are constantly deleting factors, leaving holes. **/ /** The set of available factor graph slots. These occur because we are constantly deleting factors, leaving holes. **/
std::queue<size_t> availableSlots_; std::queue<size_t> 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 */ /** Augment the list of factors with a set of new factors */
void updateFactors(const NonlinearFactorGraph& newFactors); void updateFactors(const NonlinearFactorGraph& newFactors);
@ -194,15 +115,6 @@ protected:
/** Remove factors from the list of factors by slot index */ /** Remove factors from the list of factors by slot index */
void removeFactors(const std::set<size_t>& deleteFactors); void removeFactors(const std::set<size_t>& 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<Key> findKeysBefore(double timestamp) const;
/** Erase any keys associated with timestamps before the provided time */ /** Erase any keys associated with timestamps before the provided time */
void eraseKeys(const std::set<Key>& keys); void eraseKeys(const std::set<Key>& keys);
@ -211,17 +123,12 @@ protected:
private: private:
/** Private methods for printing debug information */
static void PrintKeySet(const std::set<Key>& keys, const std::string& label); static void PrintKeySet(const std::set<Key>& 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 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 NonlinearFactorGraph& graph, const std::string& label);
static void PrintSymbolicGraph(const GaussianFactorGraph& graph, const Ordering& ordering, const std::string& label);
}; // BatchFixedLagSmoother }; // BatchFixedLagSmoother
} /// namespace gtsam } /// namespace gtsam

115
gtsam_unstable/nonlinear/FixedLagSmoother.cpp Normal file
View File

@ -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 <gtsam_unstable/nonlinear/FixedLagSmoother.h>
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<TimestampKeyMap::iterator,TimestampKeyMap::iterator> 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<Key>& 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<double>::max();
}
}
/* ************************************************************************* */
std::set<Key> FixedLagSmoother::findKeysBefore(double timestamp) const {
std::set<Key> 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<Key> FixedLagSmoother::findKeysAfter(double timestamp) const {
std::set<Key> 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

118
gtsam_unstable/nonlinear/FixedLagSmoother.h Normal file
View File

@ -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 <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/Key.h>
#include <map>
namespace gtsam {
class FixedLagSmoother {
public:
/// Typedef for a shared pointer to an Incremental Fixed-Lag Smoother
typedef boost::shared_ptr<FixedLagSmoother> shared_ptr;
/// Typedef for a Key-Timestamp map/database
typedef std::map<Key, double> KeyTimestampMap;
typedef std::multimap<double, Key> 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<Key>& 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<Key> findKeysBefore(double timestamp) const;
/** Find all of the keys associated with timestamps before the provided time */
std::set<Key> findKeysAfter(double timestamp) const;
}; // FixedLagSmoother
} /// namespace gtsam

188
gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp Normal file
View File

@ -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 <gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h>
#include <gtsam/base/debug.h>
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<const IncrementalFixedLagSmoother*> (&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<size_t> removedFactors;
FastMap<Key,int> 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<Key> 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<Key,int>::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<size_t>(), constrainedKeys);
if(debug) {
PrintSymbolicTree(isam_, "Bayes Tree After Update, Before Marginalization:");
}
// Marginalize out any needed variables
FastList<Key> 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<Key>& marginalizableKeys, FastMap<Key,int>& 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<Key>& 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

103
gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h Normal file
View File

@ -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 <gtsam_unstable/nonlinear/FixedLagSmoother.h>
#include <gtsam/nonlinear/ISAM2.h>
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<IncrementalFixedLagSmoother> 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<class VALUE>
VALUE calculateEstimate(Key key) const {
return isam_.calculateEstimate<VALUE>(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<Key>& marginalizableKeys, FastMap<Key,int>& constrainedKeys) const;
private:
/** Private methods for printing debug information */
static void PrintKeySet(const std::set<Key>& 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

4
gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp
View File

@ -16,8 +16,8 @@
* @date May 23, 2012 * @date May 23, 2012
*/ */
#include <gtsam_unstable/nonlinear/BatchFixedLagSmoother.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <gtsam_unstable/nonlinear/BatchFixedLagSmoother.h>
#include <gtsam/slam/PriorFactor.h> #include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h> #include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/Ordering.h> #include <gtsam/nonlinear/Ordering.h>
@ -62,7 +62,7 @@ TEST_UNSAFE( BatchFixedLagSmoother, Example )
// Create a Fixed-Lag Smoother // Create a Fixed-Lag Smoother
typedef BatchFixedLagSmoother::KeyTimestampMap Timestamps; typedef BatchFixedLagSmoother::KeyTimestampMap Timestamps;
BatchFixedLagSmoother smoother(LevenbergMarquardtParams(), 7.0); BatchFixedLagSmoother smoother(7.0, LevenbergMarquardtParams());
// Create containers to keep the full graph // Create containers to keep the full graph
Values fullinit; Values fullinit;

185
gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp Normal file
View File

@ -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 <CppUnitLite/TestHarness.h>
#include <gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/Ordering.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/nonlinear/Key.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/base/debug.h>
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<Point2>(key);
Point2 actual = smoother.calculateEstimate<Point2>(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<Point2>(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<Point2>(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<Point2>(key1, key2, Point2(1.0, 0.0), odometerNoise));
newFactors.add(BetweenFactor<Point2>(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<Point2>(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);}
/* ************************************************************************* */