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

relinearization working, but currently applied to all variables

release/4.3a0
Michael Kaess 2010-01-18 22:37:44 +00:00
parent 5e15564525
commit 0e6607d160
6 changed files with 24 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
.cproject
View File

@ -753,10 +753,18 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testKey.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="testISAM.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testKey.run</buildTarget>
<buildTarget>testISAM.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianISAM.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testGaussianISAM.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>

19
cpp/BayesTree-inl.h
View File

@ -402,11 +402,11 @@ namespace gtsam {
/* ************************************************************************* */
template<class Conditional>
template<class Factor>
void BayesTree<Conditional>::removeTop(const boost::shared_ptr<Factor>& newFactor,
void BayesTree<Conditional>::removeTop(const list<Symbol>& keys,
FactorGraph<Factor> &factors, typename BayesTree<Conditional>::Cliques& orphans) {
// process each key of the new factor
BOOST_FOREACH(const Symbol& key, newFactor->keys())
BOOST_FOREACH(const Symbol& key, keys)
try {
// get the clique and remove it from orphans (if it exists)
sharedClique clique = (*this)[key];
@ -424,21 +424,6 @@ namespace gtsam {
}
}
/* ************************************************************************* */
template<class Conditional>
template<class Factor>
pair<FactorGraph<Factor>, typename BayesTree<Conditional>::Cliques>
BayesTree<Conditional>::removeTop(const FactorGraph<Factor>& newFactors) {
// Remove the contaminated part of the Bayes tree
FactorGraph<Factor> factors;
Cliques orphans;
BOOST_FOREACH(boost::shared_ptr<Factor> factor, newFactors)
this->removeTop<Factor>(factor, factors, orphans);
return make_pair(factors,orphans);
}
/* ************************************************************************* */
}

12
cpp/BayesTree.h
View File

@ -174,21 +174,13 @@ namespace gtsam {
std::pair<FactorGraph<Factor>, Cliques> removePath(sharedClique clique);
/**
* Given a factor, turn "contaminated" part of the tree back into a factor graph.
* Given a list of keys, turn "contaminated" part of the tree back into a factor graph.
* Factors and orphans are added to the in/out arguments.
*/
template<class Factor>
void removeTop(const boost::shared_ptr<Factor>& newFactor,
void removeTop(const std::list<Symbol>& keys,
FactorGraph<Factor> &factors, Cliques& orphans);
/**
* Given a set of factors, turn "contaminated" part of the tree back into a
* factor graph and return it along with a list of orphaned subtree roots.
* Used for incrementally updating a BayesTree given new measurements (factors).
*/
template<class Factor>
std::pair<FactorGraph<Factor>, Cliques> removeTop(const FactorGraph<Factor>& newFactors);
}; // BayesTree
} /// namespace gtsam

2
cpp/ISAM-inl.h
View File

@ -31,7 +31,7 @@ namespace gtsam {
// Remove the contaminated part of the Bayes tree
FactorGraph<Factor> factors;
boost::tie(factors, orphans) = removeTop(newFactors);
removeTop(newFactors.keys(), factors, orphans);
// add the factors themselves
factors.push_back(newFactors);

8
cpp/ISAM2-inl.h
View File

@ -119,6 +119,9 @@ namespace gtsam {
void ISAM2<Conditional, Config>::update_internal(const NonlinearFactorGraph<Config>& newFactors,
const Config& config, Cliques& orphans) {
// todo: updates all variables... needs to be synced with removeTop!!!
linPoint_ = expmap(linPoint_, delta_);
// add new variables
linPoint_.insert(config);
@ -126,7 +129,9 @@ namespace gtsam {
// Remove the contaminated part of the Bayes tree
FactorGraph<GaussianFactor> affectedFactors;
boost::tie(affectedFactors, orphans) = this->removeTop(newFactorsLinearized);
// list<Symbol> keysToBeRemoved = newFactorsLinearized.keys(); // todo
list<Symbol> keysToBeRemoved = nonlinearFactors_.keys();
this->removeTop(keysToBeRemoved, affectedFactors, orphans);
// relinearize the affected factors ...
list<Symbol> affectedKeys = affectedFactors.keys();
@ -172,7 +177,6 @@ namespace gtsam {
// update solution - todo: potentially expensive
delta_ = optimize2(*this);
}
template<class Conditional, class Config>

6
cpp/testBayesTree.cpp
View File

@ -214,7 +214,7 @@ TEST( BayesTree, removeTop )
// Remove the contaminated part of the Bayes tree
FactorGraph<SymbolicFactor> factors;
SymbolicBayesTree::Cliques orphans;
bayesTree.removeTop<SymbolicFactor>(newFactor, factors, orphans);
bayesTree.removeTop<SymbolicFactor>(newFactor->keys(), factors, orphans);
// Check expected outcome
SymbolicFactorGraph expected;
@ -231,7 +231,7 @@ TEST( BayesTree, removeTop )
boost::shared_ptr<SymbolicFactor> newFactor2(new SymbolicFactor("B"));
FactorGraph<SymbolicFactor> factors2;
SymbolicBayesTree::Cliques orphans2;
bayesTree.removeTop<SymbolicFactor>(newFactor2, factors2, orphans2);
bayesTree.removeTop<SymbolicFactor>(newFactor2->keys(), factors2, orphans2);
SymbolicFactorGraph expected2;
CHECK(assert_equal((FactorGraph<SymbolicFactor>)expected2, factors2));
SymbolicBayesTree::Cliques expectedOrphans2;
@ -252,7 +252,7 @@ TEST( BayesTree, removeTop2 )
// Remove the contaminated part of the Bayes tree
FactorGraph<SymbolicFactor> factors;
SymbolicBayesTree::Cliques orphans;
boost::tie(factors,orphans) = bayesTree.removeTop<SymbolicFactor>(newFactors);
bayesTree.removeTop<SymbolicFactor>(newFactors.keys(), factors, orphans);
// Check expected outcome
SymbolicFactorGraph expected;