template nightmare resolved, all tests working
Michael Kaess
parent
266fc56dea
commit
5dfd1921e1
57
.cproject
57
.cproject
|
|
@ -300,7 +300,6 @@
|
|||
<buildTargets>
|
||||
<target name="install" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>install</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -308,7 +307,6 @@
|
|||
</target>
|
||||
<target name="check" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>check</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -324,7 +322,6 @@
|
|||
</target>
|
||||
<target name="testSimpleCamera.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSimpleCamera.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -340,6 +337,7 @@
|
|||
</target>
|
||||
<target name="testVSLAMFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testVSLAMFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -347,7 +345,6 @@
|
|||
</target>
|
||||
<target name="testCalibratedCamera.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testCalibratedCamera.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -355,6 +352,7 @@
|
|||
</target>
|
||||
<target name="testGaussianConditional.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testGaussianConditional.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -362,7 +360,6 @@
|
|||
</target>
|
||||
<target name="testPose2.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testPose2.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -370,7 +367,6 @@
|
|||
</target>
|
||||
<target name="testRot3.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testRot3.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -378,6 +374,7 @@
|
|||
</target>
|
||||
<target name="testNonlinearOptimizer.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testNonlinearOptimizer.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -385,7 +382,6 @@
|
|||
</target>
|
||||
<target name="testGaussianFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testGaussianFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -393,7 +389,6 @@
|
|||
</target>
|
||||
<target name="testGaussianFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testGaussianFactorGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -401,6 +396,7 @@
|
|||
</target>
|
||||
<target name="testNonlinearFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testNonlinearFactorGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -408,7 +404,6 @@
|
|||
</target>
|
||||
<target name="testPose3.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testPose3.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -416,6 +411,7 @@
|
|||
</target>
|
||||
<target name="testVectorConfig.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testVectorConfig.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -423,6 +419,7 @@
|
|||
</target>
|
||||
<target name="testPoint2.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testPoint2.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -430,7 +427,6 @@
|
|||
</target>
|
||||
<target name="testNonlinearFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testNonlinearFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -438,7 +434,6 @@
|
|||
</target>
|
||||
<target name="timeGaussianFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>timeGaussianFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -446,7 +441,6 @@
|
|||
</target>
|
||||
<target name="timeGaussianFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>timeGaussianFactorGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -454,7 +448,6 @@
|
|||
</target>
|
||||
<target name="testGaussianBayesNet.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testGaussianBayesNet.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -462,6 +455,7 @@
|
|||
</target>
|
||||
<target name="testBayesTree.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testBayesTree.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -469,7 +463,6 @@
|
|||
</target>
|
||||
<target name="testSymbolicBayesNet.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSymbolicBayesNet.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -477,6 +470,7 @@
|
|||
</target>
|
||||
<target name="testSymbolicFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSymbolicFactorGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -484,7 +478,6 @@
|
|||
</target>
|
||||
<target name="testVector.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testVector.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -492,7 +485,6 @@
|
|||
</target>
|
||||
<target name="testMatrix.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testMatrix.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -500,6 +492,7 @@
|
|||
</target>
|
||||
<target name="testVSLAMGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testVSLAMGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -507,6 +500,7 @@
|
|||
</target>
|
||||
<target name="testNonlinearEquality.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testNonlinearEquality.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -514,7 +508,6 @@
|
|||
</target>
|
||||
<target name="testSQP.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSQP.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -522,6 +515,7 @@
|
|||
</target>
|
||||
<target name="testNonlinearConstraint.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testNonlinearConstraint.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -529,7 +523,6 @@
|
|||
</target>
|
||||
<target name="testSQPOptimizer.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSQPOptimizer.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -537,7 +530,6 @@
|
|||
</target>
|
||||
<target name="testVSLAMConfig.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testVSLAMConfig.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -545,7 +537,6 @@
|
|||
</target>
|
||||
<target name="testControlConfig.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testControlConfig.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -553,7 +544,6 @@
|
|||
</target>
|
||||
<target name="testControlPoint.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testControlPoint.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -561,6 +551,7 @@
|
|||
</target>
|
||||
<target name="testControlGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testControlGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -568,7 +559,6 @@
|
|||
</target>
|
||||
<target name="testOrdering.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testOrdering.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -576,6 +566,7 @@
|
|||
</target>
|
||||
<target name="testPose2Constraint.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testPose2Constraint.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -583,22 +574,21 @@
|
|||
</target>
|
||||
<target name="testRot2.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testRot2.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testGaussianBayesTree.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="testGaussianISAM.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testGaussianBayesTree.run</buildTarget>
|
||||
<buildTarget>testGaussianISAM.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testPose3Factor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testPose3Factor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -606,6 +596,7 @@
|
|||
</target>
|
||||
<target name="testUrbanGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testUrbanGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -613,6 +604,7 @@
|
|||
</target>
|
||||
<target name="testUrbanConfig.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testUrbanConfig.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -620,6 +612,7 @@
|
|||
</target>
|
||||
<target name="testUrbanMeasurement.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testUrbanMeasurement.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -627,14 +620,22 @@
|
|||
</target>
|
||||
<target name="testUrbanOdometry.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testUrbanOdometry.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testISAM.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testISAM.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>install</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -642,6 +643,7 @@
|
|||
</target>
|
||||
<target name="clean" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>clean</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -649,6 +651,7 @@
|
|||
</target>
|
||||
<target name="check" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>check</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
|
|||
|
|
@ -371,7 +371,7 @@ namespace gtsam {
|
|||
template<class Conditional>
|
||||
template<class Factor>
|
||||
pair<FactorGraph<Factor>, typename BayesTree<Conditional>::Cliques>
|
||||
ISAM<Conditional>::removePath(sharedClique clique) {
|
||||
BayesTree<Conditional>::removePath(sharedClique clique) {
|
||||
|
||||
FactorGraph<Factor> factors;
|
||||
Cliques orphans;
|
||||
|
|
@ -403,7 +403,7 @@ namespace gtsam {
|
|||
// TODO: add to factors and orphans
|
||||
template<class Conditional>
|
||||
template<class Factor>
|
||||
void ISAM<Conditional>::removeTop(const boost::shared_ptr<Factor>& newFactor,
|
||||
void BayesTree<Conditional>::removeTop(const boost::shared_ptr<Factor>& newFactor,
|
||||
FactorGraph<Factor> &factors, typename BayesTree<Conditional>::Cliques& orphans) {
|
||||
|
||||
// process each key of the new factor
|
||||
|
|
@ -429,7 +429,7 @@ namespace gtsam {
|
|||
template<class Conditional>
|
||||
template<class Factor>
|
||||
pair<FactorGraph<Factor>, typename BayesTree<Conditional>::Cliques>
|
||||
ISAM<Conditional>::removeTop(const FactorGraph<Factor>& newFactors) {
|
||||
BayesTree<Conditional>::removeTop(const FactorGraph<Factor>& newFactors) {
|
||||
// Remove the contaminated part of the Bayes tree
|
||||
FactorGraph<Factor> factors;
|
||||
Cliques orphans;
|
||||
|
|
@ -440,56 +440,6 @@ namespace gtsam {
|
|||
return make_pair(factors,orphans);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class Conditional>
|
||||
template<class Factor>
|
||||
void ISAM<Conditional>::update_internal(const FactorGraph<Factor>& newFactors, Cliques& orphans) {
|
||||
|
||||
// Remove the contaminated part of the Bayes tree
|
||||
FactorGraph<Factor> factors;
|
||||
boost::tie(factors, orphans) = this->removeTop<Factor>(newFactors);
|
||||
|
||||
// add the factors themselves
|
||||
factors.push_back(newFactors);
|
||||
|
||||
// create an ordering for the new and contaminated factors
|
||||
Ordering ordering;
|
||||
if (true) {
|
||||
ordering = factors.getOrdering();
|
||||
} else {
|
||||
list<string> keys = factors.keys();
|
||||
keys.sort(); // todo: correct sorting order?
|
||||
ordering = keys;
|
||||
}
|
||||
|
||||
// eliminate into a Bayes net
|
||||
BayesNet<Conditional> bayesNet = eliminate<Factor, Conditional>(factors,ordering);
|
||||
|
||||
// insert conditionals back in, straight into the topless bayesTree
|
||||
typename BayesNet<Conditional>::const_reverse_iterator rit;
|
||||
for ( rit=bayesNet.rbegin(); rit != bayesNet.rend(); ++rit )
|
||||
this->insert(*rit);
|
||||
|
||||
int count = 0;
|
||||
// add orphans to the bottom of the new tree
|
||||
BOOST_FOREACH(sharedClique orphan, orphans) {
|
||||
|
||||
string key = orphan->separator_.front();
|
||||
sharedClique parent = (*this)[key];
|
||||
|
||||
parent->children_ += orphan;
|
||||
orphan->parent_ = parent; // set new parent!
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
template<class Conditional>
|
||||
template<class Factor>
|
||||
void ISAM<Conditional>::update(const FactorGraph<Factor>& newFactors) {
|
||||
Cliques orphans;
|
||||
this->update_internal<Factor>(newFactors, orphans);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -165,27 +165,6 @@ namespace gtsam {
|
|||
template<class Factor>
|
||||
BayesNet<Conditional> jointBayesNet(const std::string& key1, const std::string& key2) const;
|
||||
|
||||
}; // BayesTree
|
||||
|
||||
template<class Conditional>
|
||||
class ISAM: public BayesTree<Conditional> {
|
||||
|
||||
public:
|
||||
|
||||
/** Create an empty Bayes Tree */
|
||||
ISAM() : BayesTree<Conditional>() {}
|
||||
|
||||
/** Create a Bayes Tree from a Bayes Net */
|
||||
ISAM(const BayesNet<Conditional>& bayesNet) : BayesTree<Conditional>(bayesNet) {}
|
||||
|
||||
/** Destructor */
|
||||
virtual ~ISAM() {
|
||||
}
|
||||
|
||||
typedef typename BayesTree<Conditional>::sharedClique sharedClique;
|
||||
|
||||
typedef typename BayesTree<Conditional>::Cliques Cliques;
|
||||
|
||||
/**
|
||||
* Remove path from clique to root and return that path as factors
|
||||
* plus a list of orphaned subtree roots. Used in removeTop below.
|
||||
|
|
@ -210,14 +189,6 @@ namespace gtsam {
|
|||
std::pair<FactorGraph<Factor>, Cliques>
|
||||
removeTop(const FactorGraph<Factor>& newFactors);
|
||||
|
||||
/**
|
||||
* iSAM. (_internal provides access to list of orphans for drawing purposes)
|
||||
*/
|
||||
template<class Factor>
|
||||
void update_internal(const FactorGraph<Factor>& newFactors, Cliques& orphans);
|
||||
template<class Factor>
|
||||
void update(const FactorGraph<Factor>& newFactors);
|
||||
|
||||
}; // ISAM
|
||||
}; // BayesTree
|
||||
|
||||
} /// namespace gtsam
|
||||
|
|
|
|||
|
|
@ -1,33 +0,0 @@
|
|||
/**
|
||||
* @file GaussianBayesTree
|
||||
* @brief Bayes Tree is a tree of cliques of a Bayes Chain
|
||||
* @author Michael Kaess
|
||||
*/
|
||||
|
||||
// \callgraph
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <map>
|
||||
#include <list>
|
||||
#include <vector>
|
||||
#include <boost/serialization/map.hpp>
|
||||
#include <boost/serialization/list.hpp>
|
||||
#include <stdexcept>
|
||||
|
||||
#include "Testable.h"
|
||||
#include "BayesTree.h"
|
||||
#include "VectorConfig.h"
|
||||
#include "GaussianConditional.h"
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
typedef BayesTree<GaussianConditional> GaussianBayesTree;
|
||||
|
||||
// recursively optimize this conditional and all subtrees
|
||||
void optimize(const GaussianBayesTree::sharedClique& clique, VectorConfig& result);
|
||||
|
||||
// optimize the BayesTree, starting from the root
|
||||
VectorConfig optimize(const GaussianBayesTree& bayesTree);
|
||||
|
||||
}/// namespace gtsam
|
||||
|
|
@ -1,41 +1,38 @@
|
|||
/**
|
||||
* @file GaussianBayesTree
|
||||
* @brief Bayes Tree is a tree of cliques of a Bayes Chain
|
||||
* @file GaussianISAM
|
||||
* @brief
|
||||
* @author Michael Kaess
|
||||
*/
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
|
||||
#include "GaussianBayesTree.h"
|
||||
#include "VectorConfig.h"
|
||||
#include "GaussianISAM.h"
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
// Explicitly instantiate so we don't have to include everywhere
|
||||
#include "BayesTree-inl.h"
|
||||
template class BayesTree<GaussianConditional>;
|
||||
#include "ISAM-inl.h"
|
||||
template class ISAM<GaussianConditional>;
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
void optimize(const GaussianBayesTree::sharedClique& clique, VectorConfig& result) {
|
||||
void optimize(const GaussianISAM::sharedClique& clique, VectorConfig& result) {
|
||||
// parents are assumed to already be solved and available in result
|
||||
GaussianBayesTree::Clique::const_reverse_iterator it;
|
||||
GaussianISAM::Clique::const_reverse_iterator it;
|
||||
for (it = clique->rbegin(); it!=clique->rend(); it++) {
|
||||
GaussianConditional::shared_ptr cg = *it;
|
||||
Vector x = cg->solve(result); // Solve for that variable
|
||||
result.insert(cg->key(), x); // store result in partial solution
|
||||
}
|
||||
BOOST_FOREACH(GaussianBayesTree::sharedClique child, clique->children_) {
|
||||
// list<GaussianBayesTree::Clique::shared_ptr>::const_iterator child;
|
||||
BOOST_FOREACH(GaussianISAM::sharedClique child, clique->children_) {
|
||||
// list<GaussianISAM::Clique::shared_ptr>::const_iterator child;
|
||||
// for (child = clique->children_.begin(); child != clique->children_.end(); child++) {
|
||||
optimize(child, result);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
VectorConfig optimize(const GaussianBayesTree& bayesTree) {
|
||||
VectorConfig optimize(const GaussianISAM& bayesTree) {
|
||||
VectorConfig result;
|
||||
// starting from the root, call optimize on each conditional
|
||||
optimize(bayesTree.root(), result);
|
||||
|
|
@ -0,0 +1,24 @@
|
|||
/**
|
||||
* @file GaussianISAM
|
||||
* @brief
|
||||
* @author Michael Kaess
|
||||
*/
|
||||
|
||||
// \callgraph
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "ISAM.h"
|
||||
#include "GaussianConditional.h"
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
typedef ISAM<GaussianConditional> GaussianISAM;
|
||||
|
||||
// recursively optimize this conditional and all subtrees
|
||||
void optimize(const GaussianISAM::sharedClique& clique, VectorConfig& result);
|
||||
|
||||
// optimize the BayesTree, starting from the root
|
||||
VectorConfig optimize(const GaussianISAM& bayesTree);
|
||||
|
||||
}/// namespace gtsam
|
||||
|
|
@ -0,0 +1,80 @@
|
|||
/**
|
||||
* @file ISAM-inl.h
|
||||
* @brief Incremental update functionality (iSAM) for BayesTree.
|
||||
* @author Michael Kaess
|
||||
*/
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/assign/std/list.hpp> // for operator +=
|
||||
using namespace boost::assign;
|
||||
|
||||
#include "Conditional.h"
|
||||
#include "BayesTree-inl.h"
|
||||
#include "ISAM.h"
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
using namespace std;
|
||||
|
||||
/** Create an empty Bayes Tree */
|
||||
template<class Conditional>
|
||||
ISAM<Conditional>::ISAM() : BayesTree<Conditional>() {}
|
||||
|
||||
/** Create a Bayes Tree from a Bayes Net */
|
||||
template<class Conditional>
|
||||
ISAM<Conditional>::ISAM(const BayesNet<Conditional>& bayesNet) : BayesTree<Conditional>(bayesNet) {}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class Conditional>
|
||||
template<class Factor>
|
||||
void ISAM<Conditional>::update_internal(const FactorGraph<Factor>& newFactors, Cliques& orphans) {
|
||||
|
||||
// Remove the contaminated part of the Bayes tree
|
||||
FactorGraph<Factor> factors;
|
||||
boost::tie(factors, orphans) = removeTop(newFactors);
|
||||
|
||||
// add the factors themselves
|
||||
factors.push_back(newFactors);
|
||||
|
||||
// create an ordering for the new and contaminated factors
|
||||
Ordering ordering;
|
||||
if (true) {
|
||||
ordering = factors.getOrdering();
|
||||
} else {
|
||||
list<string> keys = factors.keys();
|
||||
keys.sort(); // todo: correct sorting order?
|
||||
ordering = keys;
|
||||
}
|
||||
|
||||
// eliminate into a Bayes net
|
||||
BayesNet<Conditional> bayesNet = eliminate<Factor, Conditional>(factors,ordering);
|
||||
|
||||
// insert conditionals back in, straight into the topless bayesTree
|
||||
typename BayesNet<Conditional>::const_reverse_iterator rit;
|
||||
for ( rit=bayesNet.rbegin(); rit != bayesNet.rend(); ++rit )
|
||||
this->insert(*rit);
|
||||
|
||||
int count = 0;
|
||||
// add orphans to the bottom of the new tree
|
||||
BOOST_FOREACH(sharedClique orphan, orphans) {
|
||||
|
||||
string key = orphan->separator_.front();
|
||||
sharedClique parent = (*this)[key];
|
||||
|
||||
parent->children_ += orphan;
|
||||
orphan->parent_ = parent; // set new parent!
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
template<class Conditional>
|
||||
template<class Factor>
|
||||
void ISAM<Conditional>::update(const FactorGraph<Factor>& newFactors) {
|
||||
Cliques orphans;
|
||||
this->update_internal<Factor>(newFactors, orphans);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
}
|
||||
/// namespace gtsam
|
||||
|
|
@ -0,0 +1,54 @@
|
|||
/**
|
||||
* @file ISAM.h
|
||||
* @brief Incremental update functionality (iSAM) for BayesTree.
|
||||
* @author Michael Kaess
|
||||
*/
|
||||
|
||||
// \callgraph
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <map>
|
||||
#include <list>
|
||||
#include <vector>
|
||||
#include <boost/serialization/map.hpp>
|
||||
#include <boost/serialization/list.hpp>
|
||||
#include <stdexcept>
|
||||
|
||||
#include "Testable.h"
|
||||
#include "FactorGraph.h"
|
||||
#include "BayesNet.h"
|
||||
#include "BayesTree.h"
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
template<class Conditional>
|
||||
class ISAM: public BayesTree<Conditional> {
|
||||
|
||||
public:
|
||||
|
||||
/** Create an empty Bayes Tree */
|
||||
ISAM();
|
||||
|
||||
/** Create a Bayes Tree from a Bayes Net */
|
||||
ISAM(const BayesNet<Conditional>& bayesNet);
|
||||
|
||||
/** Destructor */
|
||||
virtual ~ISAM() {
|
||||
}
|
||||
|
||||
typedef typename BayesTree<Conditional>::sharedClique sharedClique;
|
||||
|
||||
typedef typename BayesTree<Conditional>::Cliques Cliques;
|
||||
|
||||
/**
|
||||
* iSAM. (update_internal provides access to list of orphans for drawing purposes)
|
||||
*/
|
||||
template<class Factor>
|
||||
void update_internal(const FactorGraph<Factor>& newFactors, Cliques& orphans);
|
||||
template<class Factor>
|
||||
void update(const FactorGraph<Factor>& newFactors);
|
||||
|
||||
}; // ISAM
|
||||
|
||||
} /// namespace gtsam
|
||||
|
|
@ -75,19 +75,23 @@ testSymbolicBayesNet_LDADD = libgtsam.la
|
|||
headers += inference.h inference-inl.h
|
||||
headers += FactorGraph.h FactorGraph-inl.h
|
||||
headers += BayesNet.h BayesNet-inl.h
|
||||
headers += BayesTree.h BayesTree-inl.h GaussianBayesTree.h
|
||||
sources += GaussianBayesTree.cpp
|
||||
check_PROGRAMS += testFactorgraph testBayesTree testGaussianBayesTree testInference testOrdering
|
||||
headers += BayesTree.h BayesTree-inl.h
|
||||
headers += ISAM.h ISAM-inl.h GaussianISAM.h
|
||||
sources += GaussianISAM.cpp
|
||||
check_PROGRAMS += testFactorgraph testInference testOrdering
|
||||
check_PROGRAMS += testBayesTree testISAM testGaussianISAM
|
||||
testFactorgraph_SOURCES = testFactorgraph.cpp
|
||||
testBayesTree_SOURCES = $(example) testBayesTree.cpp
|
||||
testGaussianBayesTree_SOURCES = $(example) testGaussianBayesTree.cpp
|
||||
testInference_SOURCES = $(example) testInference.cpp
|
||||
testFactorgraph_LDADD = libgtsam.la
|
||||
testBayesTree_LDADD = libgtsam.la
|
||||
testGaussianBayesTree_LDADD = libgtsam.la
|
||||
testInference_LDADD = libgtsam.la
|
||||
testOrdering_SOURCES = testOrdering.cpp
|
||||
testOrdering_LDADD = libgtsam.la
|
||||
testBayesTree_SOURCES = $(example) testBayesTree.cpp
|
||||
testBayesTree_LDADD = libgtsam.la
|
||||
testGaussianISAM_SOURCES = $(example) testGaussianISAM.cpp
|
||||
testGaussianISAM_LDADD = libgtsam.la
|
||||
testISAM_SOURCES = $(example) testISAM.cpp
|
||||
testISAM_LDADD = libgtsam.la
|
||||
|
||||
# Binary Inference
|
||||
headers += BinaryConditional.h
|
||||
|
|
|
|||
|
|
@ -20,10 +20,6 @@ using namespace boost::assign;
|
|||
using namespace gtsam;
|
||||
|
||||
typedef BayesTree<SymbolicConditional> SymbolicBayesTree;
|
||||
typedef ISAM<SymbolicConditional> SymbolicISAM;
|
||||
|
||||
//template class BayesTree<SymbolicConditional>; // todo: needed?
|
||||
//template class ISAM<SymbolicConditional>;
|
||||
|
||||
/* ************************************************************************* */
|
||||
// SLAM example from RSS sqrtSAM paper
|
||||
|
|
@ -34,9 +30,9 @@ SymbolicConditional::shared_ptr x3(new SymbolicConditional("x3")),
|
|||
l2(new SymbolicConditional("l2","x1","x3"));
|
||||
|
||||
// Bayes Tree for sqrtSAM example
|
||||
SymbolicISAM createSlamSymbolicBayesTree(){
|
||||
SymbolicBayesTree createSlamSymbolicBayesTree(){
|
||||
// Create using insert
|
||||
SymbolicISAM bayesTree_slam;
|
||||
SymbolicBayesTree bayesTree_slam;
|
||||
bayesTree_slam.insert(x3);
|
||||
bayesTree_slam.insert(x2);
|
||||
bayesTree_slam.insert(x1);
|
||||
|
|
@ -58,8 +54,8 @@ SymbolicConditional::shared_ptr
|
|||
X(new SymbolicConditional("X", "E"));
|
||||
|
||||
// Bayes Tree for Asia example
|
||||
SymbolicISAM createAsiaSymbolicBayesTree() {
|
||||
SymbolicISAM bayesTree;
|
||||
SymbolicBayesTree createAsiaSymbolicBayesTree() {
|
||||
SymbolicBayesTree bayesTree;
|
||||
bayesTree.insert(B);
|
||||
bayesTree.insert(L);
|
||||
bayesTree.insert(E);
|
||||
|
|
@ -128,7 +124,7 @@ TEST( BayesTree, removePath )
|
|||
D(new SymbolicConditional("D", "C")),
|
||||
E(new SymbolicConditional("E", "B")),
|
||||
F(new SymbolicConditional("F", "E"));
|
||||
SymbolicISAM bayesTree;
|
||||
SymbolicBayesTree bayesTree;
|
||||
bayesTree.insert(A);
|
||||
bayesTree.insert(B);
|
||||
bayesTree.insert(C);
|
||||
|
|
@ -142,11 +138,11 @@ TEST( BayesTree, removePath )
|
|||
expected.push_factor("A","B");
|
||||
expected.push_factor("A");
|
||||
expected.push_factor("A","C");
|
||||
SymbolicISAM::Cliques expectedOrphans;
|
||||
SymbolicBayesTree::Cliques expectedOrphans;
|
||||
expectedOrphans += bayesTree["D"], bayesTree["E"];
|
||||
|
||||
FactorGraph<SymbolicFactor> factors;
|
||||
SymbolicISAM::Cliques orphans;
|
||||
SymbolicBayesTree::Cliques orphans;
|
||||
boost::tie(factors,orphans) = bayesTree.removePath<SymbolicFactor>(bayesTree["C"]);
|
||||
CHECK(assert_equal((FactorGraph<SymbolicFactor>)expected, factors));
|
||||
CHECK(assert_equal(expectedOrphans, orphans));
|
||||
|
|
@ -154,7 +150,7 @@ TEST( BayesTree, removePath )
|
|||
// remove E: factor graph with EB; E|B removed from second orphan tree
|
||||
SymbolicFactorGraph expected2;
|
||||
expected2.push_factor("B","E");
|
||||
SymbolicISAM::Cliques expectedOrphans2;
|
||||
SymbolicBayesTree::Cliques expectedOrphans2;
|
||||
expectedOrphans2 += bayesTree["F"];
|
||||
|
||||
boost::tie(factors,orphans) = bayesTree.removePath<SymbolicFactor>(bayesTree["E"]);
|
||||
|
|
@ -165,11 +161,11 @@ TEST( BayesTree, removePath )
|
|||
/* ************************************************************************* */
|
||||
TEST( BayesTree, removePath2 )
|
||||
{
|
||||
SymbolicISAM bayesTree = createAsiaSymbolicBayesTree();
|
||||
SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree();
|
||||
|
||||
// Call remove-path with clique B
|
||||
FactorGraph<SymbolicFactor> factors;
|
||||
SymbolicISAM::Cliques orphans;
|
||||
SymbolicBayesTree::Cliques orphans;
|
||||
boost::tie(factors,orphans) = bayesTree.removePath<SymbolicFactor>(bayesTree["B"]);
|
||||
|
||||
// Check expected outcome
|
||||
|
|
@ -178,7 +174,7 @@ TEST( BayesTree, removePath2 )
|
|||
expected.push_factor("B","L");
|
||||
expected.push_factor("B");
|
||||
CHECK(assert_equal((FactorGraph<SymbolicFactor>)expected, factors));
|
||||
SymbolicISAM::Cliques expectedOrphans;
|
||||
SymbolicBayesTree::Cliques expectedOrphans;
|
||||
expectedOrphans += bayesTree["S"], bayesTree["T"], bayesTree["X"];
|
||||
CHECK(assert_equal(expectedOrphans, orphans));
|
||||
}
|
||||
|
|
@ -186,11 +182,11 @@ TEST( BayesTree, removePath2 )
|
|||
/* ************************************************************************* */
|
||||
TEST( BayesTree, removePath3 )
|
||||
{
|
||||
SymbolicISAM bayesTree = createAsiaSymbolicBayesTree();
|
||||
SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree();
|
||||
|
||||
// Call remove-path with clique S
|
||||
FactorGraph<SymbolicFactor> factors;
|
||||
SymbolicISAM::Cliques orphans;
|
||||
SymbolicBayesTree::Cliques orphans;
|
||||
boost::tie(factors,orphans) = bayesTree.removePath<SymbolicFactor>(bayesTree["S"]);
|
||||
|
||||
// Check expected outcome
|
||||
|
|
@ -200,22 +196,22 @@ TEST( BayesTree, removePath3 )
|
|||
expected.push_factor("B");
|
||||
expected.push_factor("L","B","S");
|
||||
CHECK(assert_equal((FactorGraph<SymbolicFactor>)expected, factors));
|
||||
SymbolicISAM::Cliques expectedOrphans;
|
||||
SymbolicBayesTree::Cliques expectedOrphans;
|
||||
expectedOrphans += bayesTree["T"], bayesTree["X"];
|
||||
CHECK(assert_equal(expectedOrphans, orphans));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( ISAM, removeTop )
|
||||
TEST( BayesTree, removeTop )
|
||||
{
|
||||
SymbolicISAM bayesTree = createAsiaSymbolicBayesTree();
|
||||
SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree();
|
||||
|
||||
// create a new factor to be inserted
|
||||
boost::shared_ptr<SymbolicFactor> newFactor(new SymbolicFactor("B","S"));
|
||||
|
||||
// Remove the contaminated part of the Bayes tree
|
||||
FactorGraph<SymbolicFactor> factors;
|
||||
SymbolicISAM::Cliques orphans;
|
||||
SymbolicBayesTree::Cliques orphans;
|
||||
bayesTree.removeTop<SymbolicFactor>(newFactor, factors, orphans);
|
||||
|
||||
// Check expected outcome
|
||||
|
|
@ -225,26 +221,26 @@ TEST( ISAM, removeTop )
|
|||
expected.push_factor("B");
|
||||
expected.push_factor("L","B","S");
|
||||
CHECK(assert_equal((FactorGraph<SymbolicFactor>)expected, factors));
|
||||
SymbolicISAM::Cliques expectedOrphans;
|
||||
SymbolicBayesTree::Cliques expectedOrphans;
|
||||
expectedOrphans += bayesTree["T"], bayesTree["X"];
|
||||
CHECK(assert_equal(expectedOrphans, orphans));
|
||||
|
||||
// Try removeTop again with a factor that should not change a thing
|
||||
boost::shared_ptr<SymbolicFactor> newFactor2(new SymbolicFactor("B"));
|
||||
FactorGraph<SymbolicFactor> factors2;
|
||||
SymbolicISAM::Cliques orphans2;
|
||||
SymbolicBayesTree::Cliques orphans2;
|
||||
bayesTree.removeTop<SymbolicFactor>(newFactor2, factors2, orphans2);
|
||||
SymbolicFactorGraph expected2;
|
||||
CHECK(assert_equal((FactorGraph<SymbolicFactor>)expected2, factors2));
|
||||
SymbolicISAM::Cliques expectedOrphans2;
|
||||
SymbolicBayesTree::Cliques expectedOrphans2;
|
||||
CHECK(assert_equal(expectedOrphans2, orphans2));
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( ISAM, removeTop2 )
|
||||
TEST( BayesTree, removeTop2 )
|
||||
{
|
||||
SymbolicISAM bayesTree = createAsiaSymbolicBayesTree();
|
||||
SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree();
|
||||
|
||||
// create two factors to be inserted
|
||||
SymbolicFactorGraph newFactors;
|
||||
|
|
@ -253,7 +249,7 @@ TEST( ISAM, removeTop2 )
|
|||
|
||||
// Remove the contaminated part of the Bayes tree
|
||||
FactorGraph<SymbolicFactor> factors;
|
||||
SymbolicISAM::Cliques orphans;
|
||||
SymbolicBayesTree::Cliques orphans;
|
||||
boost::tie(factors,orphans) = bayesTree.removeTop<SymbolicFactor>(newFactors);
|
||||
|
||||
// Check expected outcome
|
||||
|
|
@ -263,77 +259,11 @@ TEST( ISAM, removeTop2 )
|
|||
expected.push_factor("B");
|
||||
expected.push_factor("L","B","S");
|
||||
CHECK(assert_equal((FactorGraph<SymbolicFactor>)expected, factors));
|
||||
SymbolicISAM::Cliques expectedOrphans;
|
||||
SymbolicBayesTree::Cliques expectedOrphans;
|
||||
expectedOrphans += bayesTree["T"], bayesTree["X"];
|
||||
CHECK(assert_equal(expectedOrphans, orphans));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( ISAM, iSAM )
|
||||
{
|
||||
SymbolicISAM bayesTree = createAsiaSymbolicBayesTree();
|
||||
|
||||
// Now we modify the Bayes tree by inserting a new factor over B and S
|
||||
|
||||
// New conditionals in modified top of the tree
|
||||
SymbolicConditional::shared_ptr
|
||||
S_(new SymbolicConditional("S")),
|
||||
L_(new SymbolicConditional("L", "S")),
|
||||
E_(new SymbolicConditional("E", "L", "S")),
|
||||
B_(new SymbolicConditional("B", "E", "L", "S"));
|
||||
|
||||
// Create expected Bayes tree
|
||||
SymbolicISAM expected;
|
||||
expected.insert(S_);
|
||||
expected.insert(L_);
|
||||
expected.insert(E_);
|
||||
expected.insert(B_);
|
||||
expected.insert(T);
|
||||
expected.insert(X);
|
||||
|
||||
// create new factors to be inserted
|
||||
SymbolicFactorGraph factorGraph;
|
||||
factorGraph.push_factor("B","S");
|
||||
factorGraph.push_factor("B");
|
||||
|
||||
// do incremental inference
|
||||
bayesTree.update(factorGraph);
|
||||
|
||||
// Check whether the same
|
||||
CHECK(assert_equal(expected,bayesTree));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( ISAM, iSAM_slam )
|
||||
{
|
||||
// Create using insert
|
||||
SymbolicISAM bayesTree_slam = createSlamSymbolicBayesTree();
|
||||
|
||||
//New conditionals for the expected Bayes tree
|
||||
SymbolicConditional::shared_ptr
|
||||
l1_(new SymbolicConditional("l1","x1","x2","x3"));
|
||||
|
||||
// Create expected Bayes tree
|
||||
SymbolicISAM expected_slam;
|
||||
expected_slam.insert(x3);
|
||||
expected_slam.insert(x2);
|
||||
expected_slam.insert(x1);
|
||||
expected_slam.insert(l1_);
|
||||
expected_slam.insert(l2);
|
||||
|
||||
|
||||
// create new factors to be inserted
|
||||
SymbolicFactorGraph factorGraph_slam;
|
||||
factorGraph_slam.push_factor("x3","l1");
|
||||
factorGraph_slam.push_factor("x3");
|
||||
|
||||
// do incremental inference
|
||||
bayesTree_slam.update(factorGraph_slam);
|
||||
|
||||
// Check whether the same
|
||||
CHECK(assert_equal(expected_slam,bayesTree_slam));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/**
|
||||
* @file testGaussianBayesTree.cpp
|
||||
* @brief Unit tests for GaussianBayesTree
|
||||
* @file testGaussianISAM.cpp
|
||||
* @brief Unit tests for GaussianISAM
|
||||
* @author Michael Kaess
|
||||
*/
|
||||
|
||||
|
|
@ -12,8 +12,8 @@ using namespace boost::assign;
|
|||
|
||||
#include "Ordering.h"
|
||||
#include "GaussianBayesNet.h"
|
||||
#include "BayesTree-inl.h"
|
||||
#include "GaussianBayesTree.h"
|
||||
#include "ISAM-inl.h"
|
||||
#include "GaussianISAM.h"
|
||||
#include "smallExample.h"
|
||||
|
||||
using namespace std;
|
||||
|
|
@ -25,6 +25,62 @@ using namespace gtsam;
|
|||
double sigmax1 = 0.786153, sigmax2 = 0.687131, sigmax3 = 0.671512, sigmax4 =
|
||||
0.669534, sigmax5 = sigmax3, sigmax6 = sigmax2, sigmax7 = sigmax1;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( ISAM, iSAM_smoother )
|
||||
{
|
||||
// Create smoother with 7 nodes
|
||||
GaussianFactorGraph smoother = createSmoother(7);
|
||||
|
||||
// run iSAM for every factor
|
||||
GaussianISAM actual;
|
||||
BOOST_FOREACH(boost::shared_ptr<GaussianFactor> factor, smoother) {
|
||||
GaussianFactorGraph factorGraph;
|
||||
factorGraph.push_back(factor);
|
||||
actual.update(factorGraph);
|
||||
}
|
||||
|
||||
// Create expected Bayes Tree by solving smoother with "natural" ordering
|
||||
Ordering ordering;
|
||||
for (int t = 1; t <= 7; t++) ordering += symbol('x', t);
|
||||
GaussianISAM expected(smoother.eliminate(ordering));
|
||||
|
||||
// Check whether BayesTree is correct
|
||||
CHECK(assert_equal(expected, actual));
|
||||
|
||||
// obtain solution
|
||||
VectorConfig e; // expected solution
|
||||
Vector v = Vector_(2, 0., 0.);
|
||||
for (int i=1; i<=7; i++)
|
||||
e.insert(symbol('x', i), v);
|
||||
VectorConfig optimized = optimize(actual); // actual solution
|
||||
CHECK(assert_equal(e, optimized));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( ISAM, iSAM_smoother2 )
|
||||
{
|
||||
// Create smoother with 7 nodes
|
||||
GaussianFactorGraph smoother = createSmoother(7);
|
||||
|
||||
// Create initial tree from first 4 timestamps in reverse order !
|
||||
Ordering ord; ord += "x4","x3","x2","x1";
|
||||
GaussianFactorGraph factors1;
|
||||
for (int i=0;i<7;i++) factors1.push_back(smoother[i]);
|
||||
GaussianISAM actual(factors1.eliminate(ord));
|
||||
|
||||
// run iSAM with remaining factors
|
||||
GaussianFactorGraph factors2;
|
||||
for (int i=7;i<13;i++) factors2.push_back(smoother[i]);
|
||||
actual.update(factors2);
|
||||
|
||||
// Create expected Bayes Tree by solving smoother with "natural" ordering
|
||||
Ordering ordering;
|
||||
for (int t = 1; t <= 7; t++) ordering += symbol('x', t);
|
||||
GaussianISAM expected(smoother.eliminate(ordering));
|
||||
|
||||
CHECK(assert_equal(expected, actual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* *
|
||||
Bayes tree for smoother with "natural" ordering:
|
||||
C1 x6 x7
|
||||
|
|
@ -46,17 +102,17 @@ TEST( BayesTree, linear_smoother_shortcuts )
|
|||
GaussianBayesNet chordalBayesNet = smoother.eliminate(ordering);
|
||||
|
||||
// Create the Bayes tree
|
||||
GaussianBayesTree bayesTree(chordalBayesNet);
|
||||
GaussianISAM bayesTree(chordalBayesNet);
|
||||
LONGS_EQUAL(6,bayesTree.size());
|
||||
|
||||
// Check the conditional P(Root|Root)
|
||||
GaussianBayesNet empty;
|
||||
GaussianBayesTree::sharedClique R = bayesTree.root();
|
||||
GaussianISAM::sharedClique R = bayesTree.root();
|
||||
GaussianBayesNet actual1 = R->shortcut<GaussianFactor>(R);
|
||||
CHECK(assert_equal(empty,actual1,1e-4));
|
||||
|
||||
// Check the conditional P(C2|Root)
|
||||
GaussianBayesTree::sharedClique C2 = bayesTree["x5"];
|
||||
GaussianISAM::sharedClique C2 = bayesTree["x5"];
|
||||
GaussianBayesNet actual2 = C2->shortcut<GaussianFactor>(R);
|
||||
CHECK(assert_equal(empty,actual2,1e-4));
|
||||
|
||||
|
|
@ -65,7 +121,7 @@ TEST( BayesTree, linear_smoother_shortcuts )
|
|||
Matrix A56 = Matrix_(2,2,-0.382022,0.,0.,-0.382022);
|
||||
GaussianBayesNet expected3;
|
||||
push_front(expected3,"x5", zero(2), eye(2), "x6", A56, sigma3);
|
||||
GaussianBayesTree::sharedClique C3 = bayesTree["x4"];
|
||||
GaussianISAM::sharedClique C3 = bayesTree["x4"];
|
||||
GaussianBayesNet actual3 = C3->shortcut<GaussianFactor>(R);
|
||||
CHECK(assert_equal(expected3,actual3,1e-4));
|
||||
|
||||
|
|
@ -74,7 +130,7 @@ TEST( BayesTree, linear_smoother_shortcuts )
|
|||
Matrix A46 = Matrix_(2,2,-0.146067,0.,0.,-0.146067);
|
||||
GaussianBayesNet expected4;
|
||||
push_front(expected4,"x4", zero(2), eye(2), "x6", A46, sigma4);
|
||||
GaussianBayesTree::sharedClique C4 = bayesTree["x3"];
|
||||
GaussianISAM::sharedClique C4 = bayesTree["x3"];
|
||||
GaussianBayesNet actual4 = C4->shortcut<GaussianFactor>(R);
|
||||
CHECK(assert_equal(expected4,actual4,1e-4));
|
||||
}
|
||||
|
|
@ -115,7 +171,7 @@ TEST( BayesTree, balanced_smoother_marginals )
|
|||
CHECK(assert_equal(expectedSolution,actualSolution,1e-4));
|
||||
|
||||
// Create the Bayes tree
|
||||
GaussianBayesTree bayesTree(chordalBayesNet);
|
||||
GaussianISAM bayesTree(chordalBayesNet);
|
||||
LONGS_EQUAL(4,bayesTree.size());
|
||||
|
||||
// Check marginal on x1
|
||||
|
|
@ -154,23 +210,23 @@ TEST( BayesTree, balanced_smoother_shortcuts )
|
|||
|
||||
// Create the Bayes tree
|
||||
GaussianBayesNet chordalBayesNet = smoother.eliminate(ordering);
|
||||
GaussianBayesTree bayesTree(chordalBayesNet);
|
||||
GaussianISAM bayesTree(chordalBayesNet);
|
||||
|
||||
// Check the conditional P(Root|Root)
|
||||
GaussianBayesNet empty;
|
||||
GaussianBayesTree::sharedClique R = bayesTree.root();
|
||||
GaussianISAM::sharedClique R = bayesTree.root();
|
||||
GaussianBayesNet actual1 = R->shortcut<GaussianFactor>(R);
|
||||
CHECK(assert_equal(empty,actual1,1e-4));
|
||||
|
||||
// Check the conditional P(C2|Root)
|
||||
GaussianBayesTree::sharedClique C2 = bayesTree["x3"];
|
||||
GaussianISAM::sharedClique C2 = bayesTree["x3"];
|
||||
GaussianBayesNet actual2 = C2->shortcut<GaussianFactor>(R);
|
||||
CHECK(assert_equal(empty,actual2,1e-4));
|
||||
|
||||
// Check the conditional P(C3|Root), which should be equal to P(x2|x4)
|
||||
GaussianConditional::shared_ptr p_x2_x4 = chordalBayesNet["x2"];
|
||||
GaussianBayesNet expected3; expected3.push_back(p_x2_x4);
|
||||
GaussianBayesTree::sharedClique C3 = bayesTree["x1"];
|
||||
GaussianISAM::sharedClique C3 = bayesTree["x1"];
|
||||
GaussianBayesNet actual3 = C3->shortcut<GaussianFactor>(R);
|
||||
CHECK(assert_equal(expected3,actual3,1e-4));
|
||||
}
|
||||
|
|
@ -185,14 +241,14 @@ TEST( BayesTree, balanced_smoother_clique_marginals )
|
|||
|
||||
// Create the Bayes tree
|
||||
GaussianBayesNet chordalBayesNet = smoother.eliminate(ordering);
|
||||
GaussianBayesTree bayesTree(chordalBayesNet);
|
||||
GaussianISAM bayesTree(chordalBayesNet);
|
||||
|
||||
// Check the clique marginal P(C3)
|
||||
GaussianBayesNet expected = simpleGaussian("x2",zero(2),sigmax2);
|
||||
Vector sigma = repeat(2, 0.707107);
|
||||
Matrix A12 = (-0.5)*eye(2);
|
||||
push_front(expected,"x1", zero(2), eye(2), "x2", A12, sigma);
|
||||
GaussianBayesTree::sharedClique R = bayesTree.root(), C3 = bayesTree["x1"];
|
||||
GaussianISAM::sharedClique R = bayesTree.root(), C3 = bayesTree["x1"];
|
||||
FactorGraph<GaussianFactor> marginal = C3->marginal<GaussianFactor>(R);
|
||||
GaussianBayesNet actual = eliminate<GaussianFactor,GaussianConditional>(marginal,C3->keys());
|
||||
CHECK(assert_equal(expected,actual,1e-4));
|
||||
|
|
@ -208,7 +264,7 @@ TEST( BayesTree, balanced_smoother_joint )
|
|||
|
||||
// Create the Bayes tree
|
||||
GaussianBayesNet chordalBayesNet = smoother.eliminate(ordering);
|
||||
GaussianBayesTree bayesTree(chordalBayesNet);
|
||||
GaussianISAM bayesTree(chordalBayesNet);
|
||||
|
||||
// Conditional density elements reused by both tests
|
||||
Vector sigma = repeat(2, 0.786146);
|
||||
|
|
@ -243,62 +299,6 @@ TEST( BayesTree, balanced_smoother_joint )
|
|||
CHECK(assert_equal(expected4,actual4,1e-4));
|
||||
}
|
||||
|
||||
/* ************************************************************************* *
|
||||
TEST( BayesTree, iSAM_smoother )
|
||||
{
|
||||
// Create smoother with 7 nodes
|
||||
GaussianFactorGraph smoother = createSmoother(7);
|
||||
|
||||
// run iSAM for every factor
|
||||
GaussianBayesTree actual;
|
||||
BOOST_FOREACH(boost::shared_ptr<GaussianFactor> factor, smoother) {
|
||||
GaussianFactorGraph factorGraph;
|
||||
factorGraph.push_back(factor);
|
||||
actual.update(factorGraph);
|
||||
}
|
||||
|
||||
// Create expected Bayes Tree by solving smoother with "natural" ordering
|
||||
Ordering ordering;
|
||||
for (int t = 1; t <= 7; t++) ordering += symbol('x', t);
|
||||
GaussianBayesTree expected(smoother.eliminate(ordering));
|
||||
|
||||
// Check whether BayesTree is correct
|
||||
CHECK(assert_equal(expected, actual));
|
||||
|
||||
// obtain solution
|
||||
VectorConfig e; // expected solution
|
||||
Vector v = Vector_(2, 0., 0.);
|
||||
for (int i=1; i<=7; i++)
|
||||
e.insert(symbol('x', i), v);
|
||||
VectorConfig optimized = optimize(actual); // actual solution
|
||||
CHECK(assert_equal(e, optimized));
|
||||
}
|
||||
|
||||
/* ************************************************************************* *
|
||||
TEST( BayesTree, iSAM_smoother2 )
|
||||
{
|
||||
// Create smoother with 7 nodes
|
||||
GaussianFactorGraph smoother = createSmoother(7);
|
||||
|
||||
// Create initial tree from first 4 timestamps in reverse order !
|
||||
Ordering ord; ord += "x4","x3","x2","x1";
|
||||
GaussianFactorGraph factors1;
|
||||
for (int i=0;i<7;i++) factors1.push_back(smoother[i]);
|
||||
GaussianBayesTree actual(factors1.eliminate(ord));
|
||||
|
||||
// run iSAM with remaining factors
|
||||
GaussianFactorGraph factors2;
|
||||
for (int i=7;i<13;i++) factors2.push_back(smoother[i]);
|
||||
actual.update(factors2);
|
||||
|
||||
// Create expected Bayes Tree by solving smoother with "natural" ordering
|
||||
Ordering ordering;
|
||||
for (int t = 1; t <= 7; t++) ordering += symbol('x', t);
|
||||
GaussianBayesTree expected(smoother.eliminate(ordering));
|
||||
|
||||
CHECK(assert_equal(expected, actual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -0,0 +1,143 @@
|
|||
/**
|
||||
* @file testISAM.cpp
|
||||
* @brief Unit tests for ISAM
|
||||
* @author Michael Kaess
|
||||
*/
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/assign/std/list.hpp> // for operator +=
|
||||
using namespace boost::assign;
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include "SymbolicBayesNet.h"
|
||||
#include "SymbolicFactorGraph.h"
|
||||
#include "Ordering.h"
|
||||
#include "GaussianBayesNet.h"
|
||||
#include "ISAM-inl.h"
|
||||
#include "smallExample.h"
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
typedef ISAM<SymbolicConditional> SymbolicISAM;
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Some numbers that should be consistent among all smoother tests
|
||||
|
||||
double sigmax1 = 0.786153, sigmax2 = 0.687131, sigmax3 = 0.671512, sigmax4 =
|
||||
0.669534, sigmax5 = sigmax3, sigmax6 = sigmax2, sigmax7 = sigmax1;
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
// SLAM example from RSS sqrtSAM paper
|
||||
SymbolicConditional::shared_ptr x3(new SymbolicConditional("x3")),
|
||||
x2(new SymbolicConditional("x2","x3")),
|
||||
x1(new SymbolicConditional("x1","x2","x3")),
|
||||
l1(new SymbolicConditional("l1","x1","x2")),
|
||||
l2(new SymbolicConditional("l2","x1","x3"));
|
||||
|
||||
// ISAM for sqrtSAM example
|
||||
SymbolicISAM createSlamSymbolicISAM(){
|
||||
// Create using insert
|
||||
SymbolicISAM bayesTree_slam;
|
||||
bayesTree_slam.insert(x3);
|
||||
bayesTree_slam.insert(x2);
|
||||
bayesTree_slam.insert(x1);
|
||||
bayesTree_slam.insert(l2);
|
||||
bayesTree_slam.insert(l1);
|
||||
return bayesTree_slam;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
// Conditionals for ASIA example from the tutorial with A and D evidence
|
||||
SymbolicConditional::shared_ptr
|
||||
B(new SymbolicConditional("B")),
|
||||
L(new SymbolicConditional("L", "B")),
|
||||
E(new SymbolicConditional("E", "B", "L")),
|
||||
S(new SymbolicConditional("S", "L", "B")),
|
||||
T(new SymbolicConditional("T", "E", "L")),
|
||||
X(new SymbolicConditional("X", "E"));
|
||||
|
||||
// ISAM for Asia example
|
||||
SymbolicISAM createAsiaSymbolicISAM() {
|
||||
SymbolicISAM bayesTree;
|
||||
bayesTree.insert(B);
|
||||
bayesTree.insert(L);
|
||||
bayesTree.insert(E);
|
||||
bayesTree.insert(S);
|
||||
bayesTree.insert(T);
|
||||
bayesTree.insert(X);
|
||||
return bayesTree;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( ISAM, iSAM )
|
||||
{
|
||||
SymbolicISAM bayesTree = createAsiaSymbolicISAM();
|
||||
|
||||
// Now we modify the Bayes tree by inserting a new factor over B and S
|
||||
|
||||
// New conditionals in modified top of the tree
|
||||
SymbolicConditional::shared_ptr
|
||||
S_(new SymbolicConditional("S")),
|
||||
L_(new SymbolicConditional("L", "S")),
|
||||
E_(new SymbolicConditional("E", "L", "S")),
|
||||
B_(new SymbolicConditional("B", "E", "L", "S"));
|
||||
|
||||
// Create expected Bayes tree
|
||||
SymbolicISAM expected;
|
||||
expected.insert(S_);
|
||||
expected.insert(L_);
|
||||
expected.insert(E_);
|
||||
expected.insert(B_);
|
||||
expected.insert(T);
|
||||
expected.insert(X);
|
||||
|
||||
// create new factors to be inserted
|
||||
SymbolicFactorGraph factorGraph;
|
||||
factorGraph.push_factor("B","S");
|
||||
factorGraph.push_factor("B");
|
||||
|
||||
// do incremental inference
|
||||
bayesTree.update(factorGraph);
|
||||
|
||||
// Check whether the same
|
||||
CHECK(assert_equal(expected,bayesTree));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( ISAM, iSAM_slam )
|
||||
{
|
||||
// Create using insert
|
||||
SymbolicISAM bayesTree_slam = createSlamSymbolicISAM();
|
||||
|
||||
//New conditionals for the expected Bayes tree
|
||||
SymbolicConditional::shared_ptr
|
||||
l1_(new SymbolicConditional("l1","x1","x2","x3"));
|
||||
|
||||
// Create expected Bayes tree
|
||||
SymbolicISAM expected_slam;
|
||||
expected_slam.insert(x3);
|
||||
expected_slam.insert(x2);
|
||||
expected_slam.insert(x1);
|
||||
expected_slam.insert(l1_);
|
||||
expected_slam.insert(l2);
|
||||
|
||||
|
||||
// create new factors to be inserted
|
||||
SymbolicFactorGraph factorGraph_slam;
|
||||
factorGraph_slam.push_factor("x3","l1");
|
||||
factorGraph_slam.push_factor("x3");
|
||||
|
||||
// do incremental inference
|
||||
bayesTree_slam.update(factorGraph_slam);
|
||||
|
||||
// Check whether the same
|
||||
CHECK(assert_equal(expected_slam,bayesTree_slam));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
|
||||
/* ************************************************************************* */
|
||||
Loading…
Reference in New Issue