Added and incremental fixed-lag smoother using new iSAM2 marginalization functionality, and created a common base class for all fixed-lag smoother implementations.
Stephen Williams
parent
5270cd1d9c
commit
60d3ba2d0e
|
|
@ -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 <gtsam_unstable/nonlinear/BatchFixedLagSmoother.h>
|
||||
#include <gtsam/linear/GaussianSequentialSolver.h>
|
||||
#include <gtsam_unstable/nonlinear/LinearizedFactor.h>
|
||||
#include <gtsam/linear/GaussianFactorGraph.h>
|
||||
#include <gtsam/linear/GaussianFactor.h>
|
||||
#include <gtsam/inference/inference.h>
|
||||
#include <gtsam/inference/VariableIndex.h>
|
||||
#include <gtsam/inference/Permutation.h>
|
||||
#include <gtsam/base/debug.h>
|
||||
|
||||
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 {
|
||||
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<const BatchFixedLagSmoother*> (&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<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) {
|
||||
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<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) {
|
||||
|
||||
// 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<Key>& marginalizableK
|
|||
// These factors are all generated from BayesNet conditionals. They should all be Jacobians.
|
||||
JacobianFactor::shared_ptr jacobianFactor = boost::dynamic_pointer_cast<JacobianFactor>(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<Key> linearizedKeys = newFactors.keys();
|
||||
BOOST_FOREACH(Key key, linearizedKeys) {
|
||||
|
|
@ -539,25 +350,61 @@ void BatchFixedLagSmoother::marginalizeKeys(const std::set<Key>& 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<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
|
||||
|
|
|
|||
|
|
@ -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 <gtsam_unstable/nonlinear/LinearizedFactor.h>
|
||||
#include <gtsam_unstable/nonlinear/FixedLagSmoother.h>
|
||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
|
||||
#include <queue>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
class BatchFixedLagSmoother {
|
||||
class BatchFixedLagSmoother : public FixedLagSmoother {
|
||||
|
||||
public:
|
||||
|
||||
/// Typedef for a shared pointer to an Incremental Fixed-Lag Smoother
|
||||
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 */
|
||||
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<class VALUE>
|
||||
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);
|
||||
}
|
||||
|
||||
/** 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<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 **/
|
||||
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<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. **/
|
||||
//std::set<Key> linearizedKeys_;
|
||||
Values linearizedKeys_;
|
||||
|
||||
/** The set of available factor graph slots. These occur because we are constantly deleting factors, leaving holes. **/
|
||||
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 */
|
||||
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<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 */
|
||||
void eraseKeys(const std::set<Key>& keys);
|
||||
|
||||
|
|
@ -211,17 +123,12 @@ protected:
|
|||
|
||||
|
||||
private:
|
||||
|
||||
/** Private methods for printing debug information */
|
||||
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 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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -16,8 +16,8 @@
|
|||
* @date May 23, 2012
|
||||
*/
|
||||
|
||||
#include <gtsam_unstable/nonlinear/BatchFixedLagSmoother.h>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam_unstable/nonlinear/BatchFixedLagSmoother.h>
|
||||
#include <gtsam/slam/PriorFactor.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam/nonlinear/Ordering.h>
|
||||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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);}
|
||||
/* ************************************************************************* */
|
||||
Loading…
Reference in New Issue