Working on fixing ISAM
Richard Roberts
parent
89310ceb09
commit
375f7c16e6
|
|
@ -36,9 +36,8 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
template<class CLIQUE>
|
||||
typename BayesTree<CLIQUE>::CliqueData
|
||||
BayesTree<CLIQUE>::getCliqueData() const {
|
||||
CliqueData data;
|
||||
BayesTreeCliqueData BayesTree<CLIQUE>::getCliqueData() const {
|
||||
BayesTreeCliqueData data;
|
||||
BOOST_FOREACH(const sharedClique& root, roots_)
|
||||
getCliqueData(data, root);
|
||||
return data;
|
||||
|
|
@ -46,7 +45,7 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
template<class CLIQUE>
|
||||
void BayesTree<CLIQUE>::getCliqueData(CliqueData& data, sharedClique clique) const {
|
||||
void BayesTree<CLIQUE>::getCliqueData(BayesTreeCliqueData& data, sharedClique clique) const {
|
||||
data.conditionalSizes.push_back(clique->conditional()->nrFrontals());
|
||||
data.separatorSizes.push_back(clique->conditional()->nrParents());
|
||||
BOOST_FOREACH(sharedClique c, clique->children) {
|
||||
|
|
@ -110,45 +109,6 @@ namespace gtsam {
|
|||
}
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CLIQUE>
|
||||
void BayesTree<CLIQUE>::CliqueStats::print(const std::string& s) const {
|
||||
std::cout << s
|
||||
<< "avg Conditional Size: " << avgConditionalSize << std::endl
|
||||
<< "max Conditional Size: " << maxConditionalSize << std::endl
|
||||
<< "avg Separator Size: " << avgSeparatorSize << std::endl
|
||||
<< "max Separator Size: " << maxSeparatorSize << std::endl;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CLIQUE>
|
||||
typename BayesTree<CLIQUE>::CliqueStats
|
||||
BayesTree<CLIQUE>::CliqueData::getStats() const
|
||||
{
|
||||
CliqueStats stats;
|
||||
|
||||
double sum = 0.0;
|
||||
size_t max = 0;
|
||||
BOOST_FOREACH(size_t s, conditionalSizes) {
|
||||
sum += (double)s;
|
||||
if(s > max) max = s;
|
||||
}
|
||||
stats.avgConditionalSize = sum / (double)conditionalSizes.size();
|
||||
stats.maxConditionalSize = max;
|
||||
|
||||
sum = 0.0;
|
||||
max = 1;
|
||||
BOOST_FOREACH(size_t s, separatorSizes) {
|
||||
sum += (double)s;
|
||||
if(s > max) max = s;
|
||||
}
|
||||
stats.avgSeparatorSize = sum / (double)separatorSizes.size();
|
||||
stats.maxSeparatorSize = max;
|
||||
|
||||
return stats;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CLIQUE>
|
||||
size_t BayesTree<CLIQUE>::size() const {
|
||||
|
|
|
|||
|
|
@ -0,0 +1,62 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 BayesTree.cpp
|
||||
* @brief Bayes Tree is a tree of cliques of a Bayes Chain
|
||||
* @author Frank Dellaert
|
||||
* @author Michael Kaess
|
||||
* @author Viorela Ila
|
||||
* @author Richard Roberts
|
||||
*/
|
||||
|
||||
#include <gtsam/inference/BayesTree.h>
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
void BayesTreeCliqueStats::print(const std::string& s) const {
|
||||
std::cout << s
|
||||
<< "avg Conditional Size: " << avgConditionalSize << std::endl
|
||||
<< "max Conditional Size: " << maxConditionalSize << std::endl
|
||||
<< "avg Separator Size: " << avgSeparatorSize << std::endl
|
||||
<< "max Separator Size: " << maxSeparatorSize << std::endl;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
BayesTreeCliqueStats BayesTreeCliqueData::getStats() const
|
||||
{
|
||||
BayesTreeCliqueStats stats;
|
||||
|
||||
double sum = 0.0;
|
||||
size_t max = 0;
|
||||
BOOST_FOREACH(size_t s, conditionalSizes) {
|
||||
sum += (double)s;
|
||||
if(s > max) max = s;
|
||||
}
|
||||
stats.avgConditionalSize = sum / (double)conditionalSizes.size();
|
||||
stats.maxConditionalSize = max;
|
||||
|
||||
sum = 0.0;
|
||||
max = 1;
|
||||
BOOST_FOREACH(size_t s, separatorSizes) {
|
||||
sum += (double)s;
|
||||
if(s > max) max = s;
|
||||
}
|
||||
stats.avgSeparatorSize = sum / (double)separatorSizes.size();
|
||||
stats.maxSeparatorSize = max;
|
||||
|
||||
return stats;
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -31,6 +31,23 @@ namespace gtsam {
|
|||
// Forward declarations
|
||||
template<class FACTOR> class FactorGraph;
|
||||
|
||||
/* ************************************************************************* */
|
||||
/** clique statistics */
|
||||
struct GTSAM_EXPORT BayesTreeCliqueStats {
|
||||
double avgConditionalSize;
|
||||
std::size_t maxConditionalSize;
|
||||
double avgSeparatorSize;
|
||||
std::size_t maxSeparatorSize;
|
||||
void print(const std::string& s = "") const ;
|
||||
};
|
||||
|
||||
/** store all the sizes */
|
||||
struct GTSAM_EXPORT BayesTreeCliqueData {
|
||||
std::vector<std::size_t> conditionalSizes;
|
||||
std::vector<std::size_t> separatorSizes;
|
||||
BayesTreeCliqueStats getStats() const;
|
||||
};
|
||||
|
||||
/* ************************************************************************* */
|
||||
/**
|
||||
* Bayes tree
|
||||
|
|
@ -68,22 +85,6 @@ namespace gtsam {
|
|||
/** A convenience class for a list of shared cliques */
|
||||
typedef FastList<sharedClique> Cliques;
|
||||
|
||||
/** clique statistics */
|
||||
struct CliqueStats {
|
||||
double avgConditionalSize;
|
||||
std::size_t maxConditionalSize;
|
||||
double avgSeparatorSize;
|
||||
std::size_t maxSeparatorSize;
|
||||
void print(const std::string& s = "") const ;
|
||||
};
|
||||
|
||||
/** store all the sizes */
|
||||
struct CliqueData {
|
||||
std::vector<std::size_t> conditionalSizes;
|
||||
std::vector<std::size_t> separatorSizes;
|
||||
CliqueStats getStats() const;
|
||||
};
|
||||
|
||||
/** Map from keys to Clique */
|
||||
typedef FastMap<Key, sharedClique> Nodes;
|
||||
|
||||
|
|
@ -151,7 +152,7 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/** Gather data on all cliques */
|
||||
CliqueData getCliqueData() const;
|
||||
BayesTreeCliqueData getCliqueData() const;
|
||||
|
||||
/** Collect number of cliques with cached separator marginals */
|
||||
size_t numCachedSeparatorMarginals() const;
|
||||
|
|
@ -233,7 +234,7 @@ namespace gtsam {
|
|||
int parentnum = 0) const;
|
||||
|
||||
/** Gather data on a single clique */
|
||||
void getCliqueData(CliqueData& stats, sharedClique clique) const;
|
||||
void getCliqueData(BayesTreeCliqueData& stats, sharedClique clique) const;
|
||||
|
||||
/** remove a clique: warning, can result in a forest */
|
||||
void removeClique(sharedClique clique);
|
||||
|
|
|
|||
|
|
@ -18,6 +18,7 @@
|
|||
#pragma once
|
||||
|
||||
#include <gtsam/inference/ISAM.h>
|
||||
#include <gtsam/inference/VariableIndex.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -31,9 +32,6 @@ namespace gtsam {
|
|||
if (!this->empty()) {
|
||||
const FastSet<Key> newFactorKeys = newFactors.keys();
|
||||
this->removeTop(std::vector<Key>(newFactorKeys.begin(), newFactorKeys.end()), bn, orphans);
|
||||
if (bn.empty())
|
||||
throw std::runtime_error(
|
||||
"ISAM::update_internal(): no variables in common between existing Bayes tree and incoming factors!");
|
||||
}
|
||||
|
||||
// Add the removed top and the new factors
|
||||
|
|
@ -46,7 +44,11 @@ namespace gtsam {
|
|||
factors += boost::make_shared<BayesTreeOrphanWrapper<Clique> >(orphan);
|
||||
|
||||
// eliminate into a Bayes net
|
||||
Base bayesTree = *factors.eliminateMultifrontal(boost::none, function);
|
||||
const VariableIndex varIndex(factors);
|
||||
const FastSet<Key> newFactorKeys = newFactors.keys();
|
||||
const Ordering constrainedOrdering =
|
||||
Ordering::COLAMDConstrainedLast(varIndex, std::vector<Key>(newFactorKeys.begin(), newFactorKeys.end()));
|
||||
Base bayesTree = *factors.eliminateMultifrontal(constrainedOrdering, function, varIndex);
|
||||
this->roots_.insert(this->roots_.end(), bayesTree.roots().begin(), bayesTree.roots().end());
|
||||
this->nodes_.insert(bayesTree.nodes().begin(), bayesTree.nodes().end());
|
||||
}
|
||||
|
|
|
|||
|
|
@ -165,6 +165,13 @@ namespace gtsam {
|
|||
return exp(logDeterminant());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix GaussianBayesTree::marginalCovariance(Key key) const
|
||||
{
|
||||
return marginalFactor(key)->information().inverse();
|
||||
}
|
||||
|
||||
|
||||
} // \namespace gtsam
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -131,6 +131,10 @@ namespace gtsam {
|
|||
* multiplying we add the logarithms of the diagonal elements and take the exponent at the end
|
||||
* because this is more numerically stable. */
|
||||
double logDeterminant() const;
|
||||
|
||||
/** Return the marginal on the requested variable as a covariance matrix. See also
|
||||
* marginalFactor(). */
|
||||
Matrix marginalCovariance(Key key) const;
|
||||
};
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -402,6 +402,7 @@ void HessianFactor::updateATA(const HessianFactor& update, const Scatter& scatte
|
|||
/* ************************************************************************* */
|
||||
void HessianFactor::updateATA(const JacobianFactor& update, const Scatter& scatter)
|
||||
{
|
||||
if(update.rows() > 0) // Zero-row Jacobians are treated specially
|
||||
updateATA(HessianFactor(update), scatter);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -158,6 +158,7 @@ namespace gtsam {
|
|||
BOOST_FOREACH(const size_t sourceVarpos, slots->second) {
|
||||
if(sourceVarpos < numeric_limits<size_t>::max()) {
|
||||
const JacobianFactor& sourceFactor = *factors[sourceFactorI];
|
||||
if(sourceFactor.rows() > 0) {
|
||||
DenseIndex vardim = sourceFactor.getDim(sourceFactor.begin() + sourceVarpos);
|
||||
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
|
||||
if(varDims[jointVarpos] == numeric_limits<size_t>::max()) {
|
||||
|
|
@ -171,6 +172,7 @@ namespace gtsam {
|
|||
break;
|
||||
#endif
|
||||
}
|
||||
}
|
||||
++ sourceFactorI;
|
||||
}
|
||||
++ jointVarpos;
|
||||
|
|
@ -280,6 +282,7 @@ namespace gtsam {
|
|||
// Slot in source factor
|
||||
const size_t sourceSlot = varslot->second[factorI];
|
||||
const DenseIndex sourceRows = jacobians[factorI]->rows();
|
||||
if(sourceRows > 0) {
|
||||
JacobianFactor::ABlock::RowsBlockXpr destBlock(destSlot.middleRows(nextRow, sourceRows));
|
||||
// Copy if exists in source factor, otherwise set zero
|
||||
if(sourceSlot != numeric_limits<size_t>::max())
|
||||
|
|
@ -288,6 +291,7 @@ namespace gtsam {
|
|||
destBlock.setZero();
|
||||
nextRow += sourceRows;
|
||||
}
|
||||
}
|
||||
++combinedSlot;
|
||||
}
|
||||
gttoc(copy_blocks);
|
||||
|
|
|
|||
|
|
@ -15,13 +15,11 @@
|
|||
* @author Viorela Ila and Richard Roberts
|
||||
*/
|
||||
|
||||
#if 0
|
||||
|
||||
#include <gtsam/nonlinear/NonlinearISAM.h>
|
||||
|
||||
#include <gtsam/linear/GaussianFactorGraph.h>
|
||||
#include <gtsam/inference/ISAM-inl.h>
|
||||
#include <gtsam/nonlinear/Ordering.h>
|
||||
#include <gtsam/inference/ISAM-inst.h>
|
||||
#include <gtsam/inference/Ordering.h>
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
|
||||
|
|
@ -34,12 +32,11 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
void NonlinearISAM::saveGraph(const string& s, const KeyFormatter& keyFormatter) const {
|
||||
isam_.saveGraph(s, OrderingIndexFormatter(ordering_, keyFormatter));
|
||||
isam_.saveGraph(s, keyFormatter);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void NonlinearISAM::update(const NonlinearFactorGraph& newFactors,
|
||||
const Values& initialValues) {
|
||||
void NonlinearISAM::update(const NonlinearFactorGraph& newFactors, const Values& initialValues) {
|
||||
|
||||
if(newFactors.size() > 0) {
|
||||
|
||||
|
|
@ -55,15 +52,10 @@ void NonlinearISAM::update(const NonlinearFactorGraph& newFactors,
|
|||
// TODO: optimize for whole config?
|
||||
linPoint_.insert(initialValues);
|
||||
|
||||
// Augment ordering
|
||||
// TODO: allow for ordering constraints within the new variables
|
||||
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, initialValues)
|
||||
ordering_.insert(key_value.key, ordering_.size());
|
||||
|
||||
boost::shared_ptr<GaussianFactorGraph> linearizedNewFactors = newFactors.linearize(linPoint_, ordering_);
|
||||
boost::shared_ptr<GaussianFactorGraph> linearizedNewFactors = newFactors.linearize(linPoint_);
|
||||
|
||||
// Update ISAM
|
||||
isam_.update(*linearizedNewFactors);
|
||||
isam_.update(*linearizedNewFactors, eliminationFunction_);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -78,16 +70,10 @@ void NonlinearISAM::reorder_relinearize() {
|
|||
|
||||
isam_.clear();
|
||||
|
||||
// Compute an ordering
|
||||
// TODO: allow for constrained ordering here
|
||||
ordering_ = *factors_.orderingCOLAMD(newLinPoint);
|
||||
|
||||
// Create a linear factor graph at the new linearization point
|
||||
// TODO: decouple relinearization and reordering to avoid
|
||||
boost::shared_ptr<GaussianFactorGraph> gfg = factors_.linearize(newLinPoint, ordering_);
|
||||
|
||||
// Just recreate the whole BayesTree
|
||||
isam_.update(*gfg);
|
||||
// TODO: allow for constrained ordering here
|
||||
// TODO: decouple relinearization and reordering to avoid
|
||||
isam_.update(*factors_.linearize(newLinPoint), eliminationFunction_);
|
||||
|
||||
// Update linearization point
|
||||
linPoint_ = newLinPoint;
|
||||
|
|
@ -97,14 +83,14 @@ void NonlinearISAM::reorder_relinearize() {
|
|||
/* ************************************************************************* */
|
||||
Values NonlinearISAM::estimate() const {
|
||||
if(isam_.size() > 0)
|
||||
return linPoint_.retract(optimize(isam_), ordering_);
|
||||
return linPoint_.retract(isam_.optimize());
|
||||
else
|
||||
return linPoint_;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix NonlinearISAM::marginalCovariance(Key key) const {
|
||||
return isam_.marginalCovariance(ordering_[key]);
|
||||
return isam_.marginalCovariance(key);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -112,7 +98,6 @@ void NonlinearISAM::print(const string& s, const KeyFormatter& keyFormatter) con
|
|||
cout << s << "ReorderInterval: " << reorderInterval_ << " Current Count: " << reorderCounter_ << endl;
|
||||
isam_.print("GaussianISAM:\n");
|
||||
linPoint_.print("Linearization Point:\n", keyFormatter);
|
||||
ordering_.print("System Ordering:\n", keyFormatter);
|
||||
factors_.print("Nonlinear Graph:\n", keyFormatter);
|
||||
}
|
||||
|
||||
|
|
@ -124,5 +109,3 @@ void NonlinearISAM::printStats() const {
|
|||
/* ************************************************************************* */
|
||||
|
||||
}///\ namespace gtsam
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -15,8 +15,6 @@
|
|||
* @author Viorela Ila and Richard Roberts
|
||||
*/
|
||||
|
||||
#if 0
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
|
|
@ -35,9 +33,6 @@ protected:
|
|||
/** The current linearization point */
|
||||
Values linPoint_;
|
||||
|
||||
/** The ordering */
|
||||
gtsam::Ordering ordering_;
|
||||
|
||||
/** The original factors, used when relinearizing */
|
||||
NonlinearFactorGraph factors_;
|
||||
|
||||
|
|
@ -45,6 +40,9 @@ protected:
|
|||
int reorderInterval_;
|
||||
int reorderCounter_;
|
||||
|
||||
/** The elimination function */
|
||||
GaussianFactorGraph::Eliminate eliminationFunction_;
|
||||
|
||||
public:
|
||||
|
||||
/// @name Standard Constructors
|
||||
|
|
@ -57,7 +55,9 @@ public:
|
|||
* 1 (default) reorders every time, in worse case is batch every update
|
||||
* typical values are 50 or 100
|
||||
*/
|
||||
NonlinearISAM(int reorderInterval = 1) : reorderInterval_(reorderInterval), reorderCounter_(0) {}
|
||||
NonlinearISAM(int reorderInterval = 1,
|
||||
const GaussianFactorGraph::Eliminate& eliminationFunction = GaussianFactorGraph::EliminationTraitsType::DefaultEliminate) :
|
||||
reorderInterval_(reorderInterval), reorderCounter_(0), eliminationFunction_(eliminationFunction) {}
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
|
|
@ -77,9 +77,6 @@ public:
|
|||
/** Return the current linearization point */
|
||||
const Values& getLinearizationPoint() const { return linPoint_; }
|
||||
|
||||
/** Get the ordering */
|
||||
const gtsam::Ordering& getOrdering() const { return ordering_; }
|
||||
|
||||
/** get underlying nonlinear graph */
|
||||
const NonlinearFactorGraph& getFactorsUnsafe() const { return factors_; }
|
||||
|
||||
|
|
@ -106,16 +103,8 @@ public:
|
|||
/** Relinearization and reordering of variables */
|
||||
void reorder_relinearize();
|
||||
|
||||
/** manually add a key to the end of the ordering */
|
||||
void addKey(Key key) { ordering_.push_back(key); }
|
||||
|
||||
/** replace the current ordering */
|
||||
void setOrdering(const Ordering& new_ordering) { ordering_ = new_ordering; }
|
||||
|
||||
/// @}
|
||||
|
||||
};
|
||||
|
||||
} // \namespace gtsam
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -17,8 +17,6 @@
|
|||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#if 0
|
||||
|
||||
#include <tests/smallExample.h>
|
||||
#include <gtsam/nonlinear/Symbol.h>
|
||||
#include <gtsam/linear/GaussianISAM.h>
|
||||
|
|
@ -27,6 +25,8 @@
|
|||
#include <boost/foreach.hpp>
|
||||
#include <boost/assign/std/list.hpp> // for operator +=
|
||||
using namespace boost::assign;
|
||||
#include <boost/range/adaptor/map.hpp>
|
||||
namespace br { using namespace boost::adaptors; using namespace boost::range; }
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
|
@ -35,11 +35,6 @@ using namespace example;
|
|||
using symbol_shorthand::X;
|
||||
using symbol_shorthand::L;
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Some numbers that should be consistent among all smoother tests
|
||||
|
||||
static const double tol = 1e-4;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( ISAM, iSAM_smoother )
|
||||
{
|
||||
|
|
@ -47,7 +42,7 @@ TEST( ISAM, iSAM_smoother )
|
|||
for (int t = 1; t <= 7; t++) ordering += X(t);
|
||||
|
||||
// Create smoother with 7 nodes
|
||||
GaussianFactorGraph smoother = createSmoother(7, ordering).first;
|
||||
GaussianFactorGraph smoother = createSmoother(7);
|
||||
|
||||
// run iSAM for every factor
|
||||
GaussianISAM actual;
|
||||
|
|
@ -58,27 +53,26 @@ TEST( ISAM, iSAM_smoother )
|
|||
}
|
||||
|
||||
// Create expected Bayes Tree by solving smoother with "natural" ordering
|
||||
BayesTree<GaussianConditional>::shared_ptr bayesTree = GaussianMultifrontalSolver(smoother).eliminate();
|
||||
GaussianISAM expected(*bayesTree);
|
||||
GaussianBayesTree bayesTree = *smoother.eliminateMultifrontal(ordering);
|
||||
GaussianISAM expected;
|
||||
expected.insertRoot(bayesTree.roots().front());
|
||||
|
||||
// Verify sigmas in the bayes tree
|
||||
BOOST_FOREACH(const GaussianBayesTree::sharedClique& clique, bayesTree->nodes()) {
|
||||
BOOST_FOREACH(const GaussianBayesTree::sharedClique& clique, bayesTree.nodes() | br::map_values) {
|
||||
GaussianConditional::shared_ptr conditional = clique->conditional();
|
||||
size_t dim = conditional->dim();
|
||||
EXPECT(assert_equal(gtsam::ones(dim), conditional->get_sigmas(), tol));
|
||||
EXPECT(!conditional->get_model());
|
||||
}
|
||||
|
||||
// Check whether BayesTree is correct
|
||||
EXPECT(assert_equal(expected, actual));
|
||||
|
||||
// obtain solution
|
||||
VectorValues e(VectorValues::Zero(7,2)); // expected solution
|
||||
VectorValues optimized = optimize(actual); // actual solution
|
||||
VectorValues e; // expected solution
|
||||
for (int t = 1; t <= 7; t++) e.insert(X(t), Vector::Zero(2));
|
||||
VectorValues optimized = actual.optimize(); // actual solution
|
||||
EXPECT(assert_equal(e, optimized));
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -5,8 +5,6 @@
|
|||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#if 0
|
||||
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam/nonlinear/NonlinearEquality.h>
|
||||
#include <gtsam/nonlinear/NonlinearISAM.h>
|
||||
|
|
@ -19,14 +17,13 @@
|
|||
|
||||
using namespace gtsam;
|
||||
|
||||
typedef NonlinearISAM PlanarISAM;
|
||||
|
||||
const double tol=1e-5;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(testNonlinearISAM, markov_chain ) {
|
||||
int reorder_interval = 2;
|
||||
PlanarISAM isam(reorder_interval); // create an ISAM object
|
||||
NonlinearISAM isamChol(reorder_interval, EliminatePreferCholesky); // create an ISAM object
|
||||
NonlinearISAM isamQR(reorder_interval, EliminateQR); // create an ISAM object
|
||||
|
||||
SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector_(3, 3.0, 3.0, 0.5));
|
||||
Sampler sampler(model, 42u);
|
||||
|
|
@ -34,52 +31,41 @@ TEST(testNonlinearISAM, markov_chain ) {
|
|||
// create initial graph
|
||||
Pose2 cur_pose; // start at origin
|
||||
NonlinearFactorGraph start_factors;
|
||||
start_factors.add(NonlinearEquality<Pose2>(0, cur_pose));
|
||||
start_factors += NonlinearEquality<Pose2>(0, cur_pose);
|
||||
|
||||
Values init;
|
||||
Values expected;
|
||||
init.insert(0, cur_pose);
|
||||
expected.insert(0, cur_pose);
|
||||
isam.update(start_factors, init);
|
||||
isamChol.update(start_factors, init);
|
||||
isamQR.update(start_factors, init);
|
||||
|
||||
// loop for a period of time to verify memory usage
|
||||
size_t nrPoses = 21;
|
||||
Pose2 z(1.0, 2.0, 0.1);
|
||||
for (size_t i=1; i<=nrPoses; ++i) {
|
||||
NonlinearFactorGraph new_factors;
|
||||
new_factors.add(BetweenFactor<Pose2>(i-1, i, z, model));
|
||||
new_factors += BetweenFactor<Pose2>(i-1, i, z, model);
|
||||
Values new_init;
|
||||
|
||||
// perform a check on changing orderings
|
||||
if (i == 5) {
|
||||
Ordering ordering = isam.getOrdering();
|
||||
|
||||
// swap last two elements
|
||||
Key last = ordering.pop_back().first;
|
||||
Key secondLast = ordering.pop_back().first;
|
||||
ordering.push_back(last);
|
||||
ordering.push_back(secondLast);
|
||||
isam.setOrdering(ordering);
|
||||
|
||||
Ordering expected; expected += (0), (1), (2), (4), (3);
|
||||
EXPECT(assert_equal(expected, isam.getOrdering()));
|
||||
}
|
||||
|
||||
cur_pose = cur_pose.compose(z);
|
||||
new_init.insert(i, cur_pose.retract(sampler.sample()));
|
||||
expected.insert(i, cur_pose);
|
||||
isam.update(new_factors, new_init);
|
||||
isamChol.update(new_factors, new_init);
|
||||
isamQR.update(new_factors, new_init);
|
||||
}
|
||||
|
||||
// verify values - all but the last one should be very close
|
||||
Values actual = isam.estimate();
|
||||
Values actualChol = isamChol.estimate();
|
||||
for (size_t i=0; i<nrPoses; ++i) {
|
||||
EXPECT(assert_equal(expected.at<Pose2>(i), actual.at<Pose2>(i), tol));
|
||||
EXPECT(assert_equal(expected.at<Pose2>(i), actualChol.at<Pose2>(i), tol));
|
||||
}
|
||||
Values actualQR = isamQR.estimate();
|
||||
for (size_t i=0; i<nrPoses; ++i) {
|
||||
EXPECT(assert_equal(expected.at<Pose2>(i), actualQR.at<Pose2>(i), tol));
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue