Removed ISAM2 templating
Richard Roberts
parent
6a957d059b
commit
047dda05d7
|
|
@ -21,7 +21,7 @@
|
|||
<folderInfo id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.2031210194" name="/" resourcePath="">
|
||||
<toolChain id="cdt.managedbuild.toolchain.gnu.macosx.base.677243255" name="cdt.managedbuild.toolchain.gnu.macosx.base" superClass="cdt.managedbuild.toolchain.gnu.macosx.base">
|
||||
<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF;org.eclipse.cdt.core.MachO64" id="cdt.managedbuild.target.gnu.platform.macosx.base.752782918" name="Debug Platform" osList="macosx" superClass="cdt.managedbuild.target.gnu.platform.macosx.base"/>
|
||||
<builder arguments="" buildPath="${ProjDirPath}/build" command="make" id="cdt.managedbuild.target.gnu.builder.macosx.base.319933862" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" parallelBuildOn="false" parallelizationNumber="4" superClass="cdt.managedbuild.target.gnu.builder.macosx.base"/>
|
||||
<builder arguments="" buildPath="${ProjDirPath}/build" command="make" id="cdt.managedbuild.target.gnu.builder.macosx.base.319933862" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="8" superClass="cdt.managedbuild.target.gnu.builder.macosx.base"/>
|
||||
<tool id="cdt.managedbuild.tool.macosx.c.linker.macosx.base.457360678" name="MacOS X C Linker" superClass="cdt.managedbuild.tool.macosx.c.linker.macosx.base"/>
|
||||
<tool id="cdt.managedbuild.tool.macosx.cpp.linker.macosx.base.1011140787" name="MacOS X C++ Linker" superClass="cdt.managedbuild.tool.macosx.cpp.linker.macosx.base">
|
||||
<inputType id="cdt.managedbuild.tool.macosx.cpp.linker.input.1032375444" superClass="cdt.managedbuild.tool.macosx.cpp.linker.input">
|
||||
|
|
|
|||
2
.project
2
.project
|
|
@ -23,7 +23,7 @@
|
|||
</dictionary>
|
||||
<dictionary>
|
||||
<key>org.eclipse.cdt.make.core.buildArguments</key>
|
||||
<value></value>
|
||||
<value>-j8</value>
|
||||
</dictionary>
|
||||
<dictionary>
|
||||
<key>org.eclipse.cdt.make.core.buildCommand</key>
|
||||
|
|
|
|||
|
|
@ -25,4 +25,17 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
namespace internal {
|
||||
template<class BAYESTREE>
|
||||
void optimizeInPlace(const typename BAYESTREE::sharedClique& clique, VectorValues& result) {
|
||||
// parents are assumed to already be solved and available in result
|
||||
clique->conditional()->solveInPlace(result);
|
||||
|
||||
// starting from the root, call optimize on each conditional
|
||||
BOOST_FOREACH(const typename BAYESTREE::sharedClique& child, clique->children_)
|
||||
optimizeInPlace<BAYESTREE>(child, result);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -23,22 +23,15 @@
|
|||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
namespace internal {
|
||||
void optimizeInPlace(const boost::shared_ptr<BayesTreeClique<GaussianConditional> >& clique, VectorValues& result) {
|
||||
// parents are assumed to already be solved and available in result
|
||||
clique->conditional()->solveInPlace(result);
|
||||
|
||||
// starting from the root, call optimize on each conditional
|
||||
BOOST_FOREACH(const boost::shared_ptr<BayesTreeClique<GaussianConditional> >& child, clique->children_)
|
||||
optimizeInPlace(child, result);
|
||||
}
|
||||
VectorValues optimize(const GaussianBayesTree& bayesTree) {
|
||||
VectorValues result = *allocateVectorValues(bayesTree);
|
||||
optimizeInPlace(bayesTree, result);
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
VectorValues optimize(const GaussianBayesTree& bayesTree) {
|
||||
VectorValues result = *allocateVectorValues(bayesTree);
|
||||
internal::optimizeInPlace(bayesTree.root(), result);
|
||||
return result;
|
||||
void optimizeInPlace(const GaussianBayesTree& bayesTree, VectorValues& result) {
|
||||
internal::optimizeInPlace<GaussianBayesTree>(bayesTree.root(), result);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -77,11 +70,6 @@ void optimizeGradientSearchInPlace(const GaussianBayesTree& Rd, VectorValues& gr
|
|||
toc(4, "Compute point");
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void optimizeInPlace(const GaussianBayesTree& bayesTree, VectorValues& result) {
|
||||
internal::optimizeInPlace(bayesTree.root(), result);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
VectorValues gradient(const GaussianBayesTree& bayesTree, const VectorValues& x0) {
|
||||
return gradient(FactorGraph<JacobianFactor>(bayesTree), x0);
|
||||
|
|
|
|||
|
|
@ -34,7 +34,8 @@ VectorValues optimize(const GaussianBayesTree& bayesTree);
|
|||
void optimizeInPlace(const GaussianBayesTree& clique, VectorValues& result);
|
||||
|
||||
namespace internal {
|
||||
void optimizeInPlace(const boost::shared_ptr<BayesTreeClique<GaussianConditional> >& clique, VectorValues& result);
|
||||
template<class BAYESTREE>
|
||||
void optimizeInPlace(const typename BAYESTREE::sharedClique& clique, VectorValues& result);
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -50,7 +50,7 @@ namespace gtsam {
|
|||
|
||||
// back-substitution
|
||||
tic(3, "back-substitute");
|
||||
internal::optimizeInPlace(rootClique, result);
|
||||
internal::optimizeInPlace<GaussianBayesTree>(rootClique, result);
|
||||
toc(3, "back-substitute");
|
||||
return result;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -16,13 +16,14 @@
|
|||
*/
|
||||
|
||||
#include <gtsam/nonlinear/ISAM2-impl.h>
|
||||
#include <gtsam/base/debug.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
void ISAM2::Impl::AddVariables(
|
||||
const Values& newTheta, Values& theta, Permuted<VectorValues>& delta, vector<bool>& replacedKeys,
|
||||
Ordering& ordering,typename Base::Nodes& nodes, const KeyFormatter& keyFormatter) {
|
||||
Ordering& ordering, Base::Nodes& nodes, const KeyFormatter& keyFormatter) {
|
||||
const bool debug = ISDEBUG("ISAM2 AddVariables");
|
||||
|
||||
theta.insert(newTheta);
|
||||
|
|
@ -54,9 +55,9 @@ void ISAM2::Impl::AddVariables(
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
FastSet<Index> ISAM2::Impl::IndicesFromFactors(const Ordering& ordering, const GRAPH& factors) {
|
||||
FastSet<Index> ISAM2::Impl::IndicesFromFactors(const Ordering& ordering, const NonlinearFactorGraph& factors) {
|
||||
FastSet<Index> indices;
|
||||
BOOST_FOREACH(const typename NonlinearFactor::shared_ptr& factor, factors) {
|
||||
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, factors) {
|
||||
BOOST_FOREACH(Key key, factor->keys()) {
|
||||
indices.insert(ordering[key]);
|
||||
}
|
||||
|
|
@ -93,7 +94,7 @@ FastSet<Index> ISAM2::Impl::CheckRelinearization(const Permuted<VectorValues>& d
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void ISAM2::Impl::FindAll(typename ISAM2Clique::shared_ptr clique, FastSet<Index>& keys, const vector<bool>& markedMask) {
|
||||
void ISAM2::Impl::FindAll(ISAM2Clique::shared_ptr clique, FastSet<Index>& keys, const vector<bool>& markedMask) {
|
||||
static const bool debug = false;
|
||||
// does the separator contain any of the variables?
|
||||
bool found = false;
|
||||
|
|
@ -107,7 +108,7 @@ void ISAM2::Impl::FindAll(typename ISAM2Clique::shared_ptr clique, FastSet<Index
|
|||
if(debug) clique->print("Key(s) marked in clique ");
|
||||
if(debug) cout << "so marking key " << (*clique)->keys().front() << endl;
|
||||
}
|
||||
BOOST_FOREACH(const typename ISAM2Clique::shared_ptr& child, clique->children_) {
|
||||
BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children_) {
|
||||
FindAll(child, keys, markedMask);
|
||||
}
|
||||
}
|
||||
|
|
@ -226,7 +227,7 @@ ISAM2::Impl::PartialSolve(GaussianFactorGraph& factors,
|
|||
|
||||
// eliminate into a Bayes net
|
||||
tic(7,"eliminate");
|
||||
JunctionTree<GaussianFactorGraph, typename ISAM2::Clique> jt(factors, affectedFactorsIndex);
|
||||
JunctionTree<GaussianFactorGraph, ISAM2::Clique> jt(factors, affectedFactorsIndex);
|
||||
result.bayesTree = jt.eliminate(EliminatePreferLDL);
|
||||
toc(7,"eliminate");
|
||||
|
||||
|
|
|
|||
|
|
@ -15,7 +15,10 @@
|
|||
* @author Michael Kaess, Richard Roberts
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/linear/GaussianBayesTree.h>
|
||||
#include <gtsam/nonlinear/ISAM2.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -24,7 +27,7 @@ using namespace std;
|
|||
struct ISAM2::Impl {
|
||||
|
||||
struct PartialSolveResult {
|
||||
typename ISAM2::sharedClique bayesTree;
|
||||
ISAM2::sharedClique bayesTree;
|
||||
Permutation fullReordering;
|
||||
Permutation fullReorderingInverse;
|
||||
};
|
||||
|
|
@ -46,7 +49,7 @@ struct ISAM2::Impl {
|
|||
* @param keyFormatter Formatter for printing nonlinear keys during debugging
|
||||
*/
|
||||
static void AddVariables(const Values& newTheta, Values& theta, Permuted<VectorValues>& delta, vector<bool>& replacedKeys,
|
||||
Ordering& ordering, typename Base::Nodes& nodes, const KeyFormatter& keyFormatter = DefaultKeyFormatter);
|
||||
Ordering& ordering, Base::Nodes& nodes, const KeyFormatter& keyFormatter = DefaultKeyFormatter);
|
||||
|
||||
/**
|
||||
* Extract the set of variable indices from a NonlinearFactorGraph. For each Symbol
|
||||
|
|
@ -55,7 +58,7 @@ struct ISAM2::Impl {
|
|||
* @param factors The factors from which to extract the variables
|
||||
* @return The set of variables indices from the factors
|
||||
*/
|
||||
static FastSet<Index> IndicesFromFactors(const Ordering& ordering, const GRAPH& factors);
|
||||
static FastSet<Index> IndicesFromFactors(const Ordering& ordering, const NonlinearFactorGraph& factors);
|
||||
|
||||
/**
|
||||
* Find the set of variables to be relinearized according to relinearizeThreshold.
|
||||
|
|
@ -84,7 +87,7 @@ struct ISAM2::Impl {
|
|||
*
|
||||
* Alternatively could we trace up towards the root for each variable here?
|
||||
*/
|
||||
static void FindAll(typename ISAM2Clique<CONDITIONAL>::shared_ptr clique, FastSet<Index>& keys, const vector<bool>& markedMask);
|
||||
static void FindAll(ISAM2Clique::shared_ptr clique, FastSet<Index>& keys, const vector<bool>& markedMask);
|
||||
|
||||
/**
|
||||
* Apply expmap to the given values, but only for indices appearing in
|
||||
|
|
@ -120,7 +123,7 @@ struct ISAM2::Impl {
|
|||
static PartialSolveResult PartialSolve(GaussianFactorGraph& factors, const FastSet<Index>& keys,
|
||||
const ReorderingMode& reorderingMode);
|
||||
|
||||
static size_t UpdateDelta(const boost::shared_ptr<ISAM2Clique<CONDITIONAL> >& root, std::vector<bool>& replacedKeys, Permuted<VectorValues>& delta, double wildfireThreshold);
|
||||
static size_t UpdateDelta(const boost::shared_ptr<ISAM2Clique>& root, std::vector<bool>& replacedKeys, Permuted<VectorValues>& delta, double wildfireThreshold);
|
||||
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -26,7 +26,6 @@ using namespace boost::assign;
|
|||
#include <gtsam/linear/HessianFactor.h>
|
||||
|
||||
#include <gtsam/nonlinear/ISAM2.h>
|
||||
#include <gtsam/nonlinear/ISAM2-impl-inl.h>
|
||||
#include <gtsam/nonlinear/DoglegOptimizerImpl.h>
|
||||
|
||||
|
||||
|
|
@ -120,7 +119,7 @@ ISAM2::relinearizeAffectedFactors(const FastList<Index>& affectedKeys) const {
|
|||
|
||||
/* ************************************************************************* */
|
||||
// find intermediate (linearized) factors from cache that are passed into the affected area
|
||||
FactorGraph<typename ISAM2::CacheFactor>
|
||||
FactorGraph<ISAM2::CacheFactor>
|
||||
ISAM2::getCachedBoundaryFactors(Cliques& orphans) {
|
||||
|
||||
static const bool debug = false;
|
||||
|
|
@ -252,7 +251,7 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
|
|||
toc(2,"linearize");
|
||||
|
||||
tic(5,"eliminate");
|
||||
JunctionTree<GaussianFactorGraph, typename Base::Clique> jt(factors, variableIndex_);
|
||||
JunctionTree<GaussianFactorGraph, Base::Clique> jt(factors, variableIndex_);
|
||||
sharedClique newRoot = jt.eliminate(EliminatePreferLDL);
|
||||
if(debug) newRoot->print("Eliminated: ");
|
||||
toc(5,"eliminate");
|
||||
|
|
@ -324,7 +323,7 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
|
|||
toc(1,"list to set");
|
||||
|
||||
tic(2,"PartialSolve");
|
||||
typename Impl::ReorderingMode reorderingMode;
|
||||
Impl::ReorderingMode reorderingMode;
|
||||
reorderingMode.nFullSystemVars = ordering_.nVars();
|
||||
reorderingMode.algorithm = Impl::ReorderingMode::COLAMD;
|
||||
reorderingMode.constrain = Impl::ReorderingMode::CONSTRAIN_LAST;
|
||||
|
|
@ -332,7 +331,7 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
|
|||
reorderingMode.constrainedKeys = *constrainKeys;
|
||||
else
|
||||
reorderingMode.constrainedKeys = FastSet<Index>(newKeys.begin(), newKeys.end());
|
||||
typename Impl::PartialSolveResult partialSolveResult =
|
||||
Impl::PartialSolveResult partialSolveResult =
|
||||
Impl::PartialSolve(factors, *affectedKeysSet, reorderingMode);
|
||||
toc(2,"PartialSolve");
|
||||
|
||||
|
|
@ -605,7 +604,7 @@ VALUE ISAM2::calculateEstimate(Key key) const {
|
|||
/* ************************************************************************* */
|
||||
Values ISAM2::calculateBestEstimate() const {
|
||||
VectorValues delta(theta_.dims(ordering_));
|
||||
optimize2(this->root(), delta);
|
||||
internal::optimizeInPlace<Base>(this->root(), delta);
|
||||
return theta_.retract(delta, ordering_);
|
||||
}
|
||||
|
||||
|
|
@ -616,6 +615,13 @@ const Permuted<VectorValues>& ISAM2::getDelta() const {
|
|||
return delta_;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
VectorValues optimize(const ISAM2& isam) {
|
||||
VectorValues delta = *allocateVectorValues(isam);
|
||||
internal::optimizeInPlace<ISAM2::Base>(isam.root(), delta);
|
||||
return delta;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
VectorValues optimizeGradientSearch(const ISAM2& isam) {
|
||||
tic(0, "Allocate VectorValues");
|
||||
|
|
|
|||
|
|
@ -21,7 +21,7 @@
|
|||
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/DoglegOptimizerImpl.h>
|
||||
#include <gtsam/inference/BayesTree.h>
|
||||
#include <gtsam/linear/GaussianBayesTree.h>
|
||||
|
||||
#include <boost/variant.hpp>
|
||||
|
||||
|
|
@ -186,9 +186,9 @@ struct ISAM2Clique : public BayesTreeCliqueBase<ISAM2Clique, GaussianConditional
|
|||
typedef boost::shared_ptr<This> shared_ptr;
|
||||
typedef boost::weak_ptr<This> weak_ptr;
|
||||
typedef GaussianConditional ConditionalType;
|
||||
typedef typename ConditionalType::shared_ptr sharedConditional;
|
||||
typedef ConditionalType::shared_ptr sharedConditional;
|
||||
|
||||
typename Base::FactorType::shared_ptr cachedFactor_;
|
||||
Base::FactorType::shared_ptr cachedFactor_;
|
||||
Vector gradientContribution_;
|
||||
|
||||
/** Construct from a conditional */
|
||||
|
|
@ -196,7 +196,7 @@ struct ISAM2Clique : public BayesTreeCliqueBase<ISAM2Clique, GaussianConditional
|
|||
throw runtime_error("ISAM2Clique should always be constructed with the elimination result constructor"); }
|
||||
|
||||
/** Construct from an elimination result */
|
||||
ISAM2Clique(const std::pair<sharedConditional, boost::shared_ptr<typename ConditionalType::FactorType> >& result) :
|
||||
ISAM2Clique(const std::pair<sharedConditional, boost::shared_ptr<ConditionalType::FactorType> >& result) :
|
||||
Base(result.first), cachedFactor_(result.second), gradientContribution_(result.first->get_R().cols() + result.first->get_S().cols()) {
|
||||
// Compute gradient contribution
|
||||
const ConditionalType& conditional(*result.first);
|
||||
|
|
@ -208,13 +208,13 @@ struct ISAM2Clique : public BayesTreeCliqueBase<ISAM2Clique, GaussianConditional
|
|||
shared_ptr clone() const {
|
||||
shared_ptr copy(new ISAM2Clique(make_pair(
|
||||
sharedConditional(new ConditionalType(*Base::conditional_)),
|
||||
cachedFactor_ ? cachedFactor_->clone() : typename Base::FactorType::shared_ptr())));
|
||||
cachedFactor_ ? cachedFactor_->clone() : Base::FactorType::shared_ptr())));
|
||||
copy->gradientContribution_ = gradientContribution_;
|
||||
return copy;
|
||||
}
|
||||
|
||||
/** Access the cached factor */
|
||||
typename Base::FactorType::shared_ptr& cachedFactor() { return cachedFactor_; }
|
||||
Base::FactorType::shared_ptr& cachedFactor() { return cachedFactor_; }
|
||||
|
||||
/** Access the gradient contribution */
|
||||
const Vector& gradientContribution() const { return gradientContribution_; }
|
||||
|
|
@ -343,9 +343,9 @@ public:
|
|||
/** Create an empty ISAM2 instance using the default set of parameters (see ISAM2Params) */
|
||||
ISAM2();
|
||||
|
||||
typedef typename Base::Clique Clique; ///< A clique
|
||||
typedef typename Base::sharedClique sharedClique; ///< Shared pointer to a clique
|
||||
typedef typename Base::Cliques Cliques; ///< List of Clique typedef from base class
|
||||
typedef Base::Clique Clique; ///< A clique
|
||||
typedef Base::sharedClique sharedClique; ///< Shared pointer to a clique
|
||||
typedef Base::Cliques Cliques; ///< List of Clique typedef from base class
|
||||
|
||||
void cloneTo(boost::shared_ptr<ISAM2>& newISAM2) const {
|
||||
boost::shared_ptr<Base> bayesTree = boost::static_pointer_cast<Base>(newISAM2);
|
||||
|
|
@ -457,11 +457,7 @@ private:
|
|||
}; // ISAM2
|
||||
|
||||
/** Get the linear delta for the ISAM2 object, unpermuted the delta returned by ISAM2::getDelta() */
|
||||
VectorValues optimize(const ISAM2& isam) {
|
||||
VectorValues delta = *allocateVectorValues(isam);
|
||||
internal::optimizeInPlace(isam.root(), delta);
|
||||
return delta;
|
||||
}
|
||||
VectorValues optimize(const ISAM2& isam);
|
||||
|
||||
/// Optimize the BayesTree, starting from the root.
|
||||
/// @param replaced Needs to contain
|
||||
|
|
@ -541,3 +537,4 @@ void gradientAtZero(const BayesTree<GaussianConditional, ISAM2Clique>& bayesTree
|
|||
} /// namespace gtsam
|
||||
|
||||
#include <gtsam/nonlinear/ISAM2-inl.h>
|
||||
#include <gtsam/nonlinear/ISAM2-impl.h>
|
||||
|
|
|
|||
|
|
@ -17,7 +17,7 @@ using namespace boost::assign;
|
|||
#include <gtsam/linear/GaussianBayesNet.h>
|
||||
#include <gtsam/linear/GaussianSequentialSolver.h>
|
||||
#include <gtsam/linear/GaussianBayesTree.h>
|
||||
#include <gtsam/nonlinear/GaussianISAM2.h>
|
||||
#include <gtsam/nonlinear/ISAM2.h>
|
||||
#include <gtsam/slam/smallExample.h>
|
||||
#include <gtsam/slam/planarSLAM.h>
|
||||
|
||||
|
|
@ -52,7 +52,7 @@ TEST(ISAM2, AddVariables) {
|
|||
|
||||
Ordering ordering; ordering += planarSLAM::PointKey(0), planarSLAM::PoseKey(0);
|
||||
|
||||
GaussianISAM2<>::Nodes nodes(2);
|
||||
ISAM2::Nodes nodes(2);
|
||||
|
||||
// Verify initial state
|
||||
LONGS_EQUAL(0, ordering[planarSLAM::PointKey(0)]);
|
||||
|
|
@ -82,11 +82,11 @@ TEST(ISAM2, AddVariables) {
|
|||
|
||||
Ordering orderingExpected; orderingExpected += planarSLAM::PointKey(0), planarSLAM::PoseKey(0), planarSLAM::PoseKey(1);
|
||||
|
||||
GaussianISAM2<>::Nodes nodesExpected(
|
||||
3, GaussianISAM2<>::sharedClique());
|
||||
ISAM2::Nodes nodesExpected(
|
||||
3, ISAM2::sharedClique());
|
||||
|
||||
// Expand initial state
|
||||
GaussianISAM2<>::Impl::AddVariables(newTheta, theta, delta, replacedKeys, ordering, nodes);
|
||||
ISAM2::Impl::AddVariables(newTheta, theta, delta, replacedKeys, ordering, nodes);
|
||||
|
||||
EXPECT(assert_equal(thetaExpected, theta));
|
||||
EXPECT(assert_equal(deltaUnpermutedExpected, deltaUnpermuted));
|
||||
|
|
@ -171,10 +171,10 @@ TEST(ISAM2, optimize2) {
|
|||
conditional->solveInPlace(expected);
|
||||
|
||||
// Clique
|
||||
GaussianISAM2<>::sharedClique clique(
|
||||
GaussianISAM2<>::Clique::Create(make_pair(conditional,GaussianFactor::shared_ptr())));
|
||||
ISAM2::sharedClique clique(
|
||||
ISAM2::Clique::Create(make_pair(conditional,GaussianFactor::shared_ptr())));
|
||||
VectorValues actual(theta.dims(ordering));
|
||||
internal::optimizeInPlace(clique, actual);
|
||||
internal::optimizeInPlace<ISAM2::Base>(clique, actual);
|
||||
|
||||
// expected.print("expected: ");
|
||||
// actual.print("actual: ");
|
||||
|
|
@ -182,7 +182,7 @@ TEST(ISAM2, optimize2) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool isam_check(const planarSLAM::Graph& fullgraph, const Values& fullinit, const GaussianISAM2<>& isam) {
|
||||
bool isam_check(const planarSLAM::Graph& fullgraph, const Values& fullinit, const ISAM2& isam) {
|
||||
Values actual = isam.calculateEstimate();
|
||||
Ordering ordering = isam.getOrdering(); // *fullgraph.orderingCOLAMD(fullinit).first;
|
||||
GaussianFactorGraph linearized = *fullgraph.linearize(fullinit, ordering);
|
||||
|
|
@ -212,7 +212,7 @@ TEST(ISAM2, slamlike_solution_gaussnewton)
|
|||
SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1));
|
||||
|
||||
// These variables will be reused and accumulate factors and values
|
||||
GaussianISAM2<> isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false));
|
||||
ISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false));
|
||||
Values fullinit;
|
||||
planarSLAM::Graph fullgraph;
|
||||
|
||||
|
|
@ -300,7 +300,7 @@ TEST(ISAM2, slamlike_solution_gaussnewton)
|
|||
CHECK(isam_check(fullgraph, fullinit, isam));
|
||||
|
||||
// Check gradient at each node
|
||||
typedef GaussianISAM2<>::sharedClique sharedClique;
|
||||
typedef ISAM2::sharedClique sharedClique;
|
||||
BOOST_FOREACH(const sharedClique& clique, isam.nodes()) {
|
||||
// Compute expected gradient
|
||||
FactorGraph<JacobianFactor> jfg;
|
||||
|
|
@ -345,7 +345,7 @@ TEST(ISAM2, slamlike_solution_dogleg)
|
|||
SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1));
|
||||
|
||||
// These variables will be reused and accumulate factors and values
|
||||
GaussianISAM2<> isam(ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false));
|
||||
ISAM2 isam(ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false));
|
||||
Values fullinit;
|
||||
planarSLAM::Graph fullgraph;
|
||||
|
||||
|
|
@ -433,7 +433,7 @@ TEST(ISAM2, slamlike_solution_dogleg)
|
|||
CHECK(isam_check(fullgraph, fullinit, isam));
|
||||
|
||||
// Check gradient at each node
|
||||
typedef GaussianISAM2<>::sharedClique sharedClique;
|
||||
typedef ISAM2::sharedClique sharedClique;
|
||||
BOOST_FOREACH(const sharedClique& clique, isam.nodes()) {
|
||||
// Compute expected gradient
|
||||
FactorGraph<JacobianFactor> jfg;
|
||||
|
|
@ -473,7 +473,7 @@ TEST(ISAM2, clone) {
|
|||
SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1));
|
||||
|
||||
// These variables will be reused and accumulate factors and values
|
||||
GaussianISAM2<> isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, true));
|
||||
ISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, true));
|
||||
Values fullinit;
|
||||
planarSLAM::Graph fullgraph;
|
||||
|
||||
|
|
@ -558,8 +558,8 @@ TEST(ISAM2, clone) {
|
|||
}
|
||||
|
||||
// CLONING...
|
||||
boost::shared_ptr<GaussianISAM2<> > isam2
|
||||
= boost::shared_ptr<GaussianISAM2<> >(new GaussianISAM2<>());
|
||||
boost::shared_ptr<ISAM2 > isam2
|
||||
= boost::shared_ptr<ISAM2 >(new ISAM2());
|
||||
isam.cloneTo(isam2);
|
||||
|
||||
CHECK(assert_equal(isam, *isam2));
|
||||
|
|
@ -567,24 +567,23 @@ TEST(ISAM2, clone) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
TEST(ISAM2, permute_cached) {
|
||||
typedef ISAM2Clique<GaussianConditional> Clique;
|
||||
typedef boost::shared_ptr<ISAM2Clique<GaussianConditional> > sharedClique;
|
||||
typedef boost::shared_ptr<ISAM2Clique> sharedISAM2Clique;
|
||||
|
||||
// Construct expected permuted BayesTree (variable 2 has been changed to 1)
|
||||
BayesTree<GaussianConditional, Clique> expected;
|
||||
expected.insert(sharedClique(new Clique(make_pair(
|
||||
BayesTree<GaussianConditional, ISAM2Clique> expected;
|
||||
expected.insert(sharedISAM2Clique(new ISAM2Clique(make_pair(
|
||||
boost::make_shared<GaussianConditional>(pair_list_of
|
||||
(3, Matrix_(1,1,1.0))
|
||||
(4, Matrix_(1,1,2.0)),
|
||||
2, Vector_(1,1.0), Vector_(1,1.0)), // p(3,4)
|
||||
HessianFactor::shared_ptr())))); // Cached: empty
|
||||
expected.insert(sharedClique(new Clique(make_pair(
|
||||
expected.insert(sharedISAM2Clique(new ISAM2Clique(make_pair(
|
||||
boost::make_shared<GaussianConditional>(pair_list_of
|
||||
(2, Matrix_(1,1,1.0))
|
||||
(3, Matrix_(1,1,2.0)),
|
||||
1, Vector_(1,1.0), Vector_(1,1.0)), // p(2|3)
|
||||
boost::make_shared<HessianFactor>(3, Matrix_(1,1,1.0), Vector_(1,1.0), 0.0))))); // Cached: p(3)
|
||||
expected.insert(sharedClique(new Clique(make_pair(
|
||||
expected.insert(sharedISAM2Clique(new ISAM2Clique(make_pair(
|
||||
boost::make_shared<GaussianConditional>(pair_list_of
|
||||
(0, Matrix_(1,1,1.0))
|
||||
(2, Matrix_(1,1,2.0)),
|
||||
|
|
@ -595,20 +594,20 @@ TEST(ISAM2, permute_cached) {
|
|||
expected.root()->children().front()->children().front()->conditional()->keys()[1] = 1;
|
||||
|
||||
// Construct unpermuted BayesTree
|
||||
BayesTree<GaussianConditional, Clique> actual;
|
||||
actual.insert(sharedClique(new Clique(make_pair(
|
||||
BayesTree<GaussianConditional, ISAM2Clique> actual;
|
||||
actual.insert(sharedISAM2Clique(new ISAM2Clique(make_pair(
|
||||
boost::make_shared<GaussianConditional>(pair_list_of
|
||||
(3, Matrix_(1,1,1.0))
|
||||
(4, Matrix_(1,1,2.0)),
|
||||
2, Vector_(1,1.0), Vector_(1,1.0)), // p(3,4)
|
||||
HessianFactor::shared_ptr())))); // Cached: empty
|
||||
actual.insert(sharedClique(new Clique(make_pair(
|
||||
actual.insert(sharedISAM2Clique(new ISAM2Clique(make_pair(
|
||||
boost::make_shared<GaussianConditional>(pair_list_of
|
||||
(2, Matrix_(1,1,1.0))
|
||||
(3, Matrix_(1,1,2.0)),
|
||||
1, Vector_(1,1.0), Vector_(1,1.0)), // p(2|3)
|
||||
boost::make_shared<HessianFactor>(3, Matrix_(1,1,1.0), Vector_(1,1.0), 0.0))))); // Cached: p(3)
|
||||
actual.insert(sharedClique(new Clique(make_pair(
|
||||
actual.insert(sharedISAM2Clique(new ISAM2Clique(make_pair(
|
||||
boost::make_shared<GaussianConditional>(pair_list_of
|
||||
(0, Matrix_(1,1,1.0))
|
||||
(2, Matrix_(1,1,2.0)),
|
||||
|
|
@ -646,7 +645,7 @@ TEST(ISAM2, removeFactors)
|
|||
SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1));
|
||||
|
||||
// These variables will be reused and accumulate factors and values
|
||||
GaussianISAM2<> isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false));
|
||||
ISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false));
|
||||
Values fullinit;
|
||||
planarSLAM::Graph fullgraph;
|
||||
|
||||
|
|
@ -740,7 +739,7 @@ TEST(ISAM2, removeFactors)
|
|||
CHECK(isam_check(fullgraph, fullinit, isam));
|
||||
|
||||
// Check gradient at each node
|
||||
typedef GaussianISAM2<>::sharedClique sharedClique;
|
||||
typedef ISAM2::sharedClique sharedClique;
|
||||
BOOST_FOREACH(const sharedClique& clique, isam.nodes()) {
|
||||
// Compute expected gradient
|
||||
FactorGraph<JacobianFactor> jfg;
|
||||
|
|
@ -785,7 +784,7 @@ TEST(ISAM2, constrained_ordering)
|
|||
SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1));
|
||||
|
||||
// These variables will be reused and accumulate factors and values
|
||||
GaussianISAM2<> isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false));
|
||||
ISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false));
|
||||
Values fullinit;
|
||||
planarSLAM::Graph fullgraph;
|
||||
|
||||
|
|
@ -883,7 +882,7 @@ TEST(ISAM2, constrained_ordering)
|
|||
(isam.getOrdering()[planarSLAM::PoseKey(3)] == 13 && isam.getOrdering()[planarSLAM::PoseKey(4)] == 12));
|
||||
|
||||
// Check gradient at each node
|
||||
typedef GaussianISAM2<>::sharedClique sharedClique;
|
||||
typedef ISAM2::sharedClique sharedClique;
|
||||
BOOST_FOREACH(const sharedClique& clique, isam.nodes()) {
|
||||
// Compute expected gradient
|
||||
FactorGraph<JacobianFactor> jfg;
|
||||
|
|
|
|||
Loading…
Reference in New Issue