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

unit test enabled again and working

release/4.3a0
Michael Kaess 2009-12-29 14:54:45 +00:00
parent c43cd425ab
commit 737a2f1e05
4 changed files with 14 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cpp/GaussianISAM2.cpp
View File

@ -11,7 +11,7 @@ using namespace gtsam;
// Explicitly instantiate so we don't have to include everywhere
#include "ISAM2-inl.h"
template class ISAM2<GaussianConditional, VectorConfig>;
//template class ISAM2<GaussianConditional, VectorConfig>;
namespace gtsam {

10
cpp/ISAM2-inl.h
View File

@ -30,10 +30,10 @@ namespace gtsam {
/* ************************************************************************* */
template<class Conditional, class Config>
void ISAM2<Conditional, Config>::update_internal(const NonlinearFactorGraph<Config>& newFactorsXXX, Cliques& orphans) {
void ISAM2<Conditional, Config>::update_internal(const NonlinearFactorGraph<Config>& newFactorsXXX, const Config& config, Cliques& orphans) {
Config xxx;
FactorGraph<GaussianFactor> newFactors; //todo = newFactorsXXX.linearize(xxx);
config_ = config; // todo
FactorGraph<GaussianFactor> newFactors = newFactorsXXX.linearize(config); // todo: just for testing
// Remove the contaminated part of the Bayes tree
FactorGraph<GaussianFactor> factors;
@ -74,9 +74,9 @@ namespace gtsam {
}
template<class Conditional, class Config>
void ISAM2<Conditional, Config>::update(const NonlinearFactorGraph<Config>& newFactors) {
void ISAM2<Conditional, Config>::update(const NonlinearFactorGraph<Config>& newFactors, const Config& config) {
Cliques orphans;
this->update_internal(newFactors, orphans);
this->update_internal(newFactors, config, orphans);
}
/* ************************************************************************* */

6
cpp/ISAM2.h
View File

@ -26,6 +26,8 @@ namespace gtsam {
template<class Conditional, class Config>
class ISAM2: public BayesTree<Conditional> {
// for keeping all original nonlinear data
Config config_;
NonlinearFactorGraph<Config> nonlinearFactors_;
public:
@ -47,8 +49,8 @@ namespace gtsam {
/**
* ISAM2. (update_internal provides access to list of orphans for drawing purposes)
*/
void update_internal(const NonlinearFactorGraph<Config>& newFactors, Cliques& orphans);
void update(const NonlinearFactorGraph<Config>& newFactors);
void update_internal(const NonlinearFactorGraph<Config>& newFactors, const Config& config, Cliques& orphans);
void update(const NonlinearFactorGraph<Config>& newFactors, const Config& config);
}; // ISAM2

8
cpp/testGaussianISAM2.cpp
View File

@ -24,7 +24,7 @@ using namespace gtsam;
double sigmax1 = 0.786153, sigmax2 = 0.687131, sigmax3 = 0.671512, sigmax4 =
0.669534, sigmax5 = sigmax3, sigmax6 = sigmax2, sigmax7 = sigmax1;
#if 0
/* ************************************************************************* */
TEST( ISAM2, ISAM2_smoother )
{
@ -38,7 +38,7 @@ TEST( ISAM2, ISAM2_smoother )
BOOST_FOREACH(boost::shared_ptr<NonlinearFactor<VectorConfig> > factor, smoother) {
ExampleNonlinearFactorGraph factorGraph;
factorGraph.push_back(factor);
actual.update(factorGraph);
actual.update(factorGraph, poses);
}
// Create expected Bayes Tree by solving smoother with "natural" ordering
@ -75,7 +75,7 @@ TEST( ISAM2, ISAM2_smoother2 )
// run ISAM2 with remaining factors
ExampleNonlinearFactorGraph factors2;
for (int i=7;i<13;i++) factors2.push_back(smoother[i]);
actual.update(factors2);
actual.update(factors2, poses);
// Create expected Bayes Tree by solving smoother with "natural" ordering
Ordering ordering;
@ -302,7 +302,7 @@ TEST( BayesTree, balanced_smoother_joint )
GaussianBayesNet actual4 = bayesTree.jointBayesNet<GaussianFactor>("x4","x1");
CHECK(assert_equal(expected4,actual4,1e-4));
}
#endif
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */