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 // Explicitly instantiate so we don't have to include everywhere
#include "ISAM2-inl.h" #include "ISAM2-inl.h"
template class ISAM2<GaussianConditional, VectorConfig>; //template class ISAM2<GaussianConditional, VectorConfig>;
namespace gtsam { namespace gtsam {

10
cpp/ISAM2-inl.h
View File

@ -30,10 +30,10 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class Conditional, class Config> 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; config_ = config; // todo
FactorGraph<GaussianFactor> newFactors; //todo = newFactorsXXX.linearize(xxx); FactorGraph<GaussianFactor> newFactors = newFactorsXXX.linearize(config); // todo: just for testing
// Remove the contaminated part of the Bayes tree // Remove the contaminated part of the Bayes tree
FactorGraph<GaussianFactor> factors; FactorGraph<GaussianFactor> factors;
@ -74,9 +74,9 @@ namespace gtsam {
} }
template<class Conditional, class Config> 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; 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> template<class Conditional, class Config>
class ISAM2: public BayesTree<Conditional> { class ISAM2: public BayesTree<Conditional> {
// for keeping all original nonlinear data
Config config_;
NonlinearFactorGraph<Config> nonlinearFactors_; NonlinearFactorGraph<Config> nonlinearFactors_;
public: public:
@ -47,8 +49,8 @@ namespace gtsam {
/** /**
* ISAM2. (update_internal provides access to list of orphans for drawing purposes) * ISAM2. (update_internal provides access to list of orphans for drawing purposes)
*/ */
void update_internal(const NonlinearFactorGraph<Config>& newFactors, Cliques& orphans); void update_internal(const NonlinearFactorGraph<Config>& newFactors, const Config& config, Cliques& orphans);
void update(const NonlinearFactorGraph<Config>& newFactors); void update(const NonlinearFactorGraph<Config>& newFactors, const Config& config);
}; // ISAM2 }; // 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 = double sigmax1 = 0.786153, sigmax2 = 0.687131, sigmax3 = 0.671512, sigmax4 =
0.669534, sigmax5 = sigmax3, sigmax6 = sigmax2, sigmax7 = sigmax1; 0.669534, sigmax5 = sigmax3, sigmax6 = sigmax2, sigmax7 = sigmax1;
#if 0
/* ************************************************************************* */ /* ************************************************************************* */
TEST( ISAM2, ISAM2_smoother ) TEST( ISAM2, ISAM2_smoother )
{ {
@ -38,7 +38,7 @@ TEST( ISAM2, ISAM2_smoother )
BOOST_FOREACH(boost::shared_ptr<NonlinearFactor<VectorConfig> > factor, smoother) { BOOST_FOREACH(boost::shared_ptr<NonlinearFactor<VectorConfig> > factor, smoother) {
ExampleNonlinearFactorGraph factorGraph; ExampleNonlinearFactorGraph factorGraph;
factorGraph.push_back(factor); factorGraph.push_back(factor);
actual.update(factorGraph); actual.update(factorGraph, poses);
} }
// Create expected Bayes Tree by solving smoother with "natural" ordering // Create expected Bayes Tree by solving smoother with "natural" ordering
@ -75,7 +75,7 @@ TEST( ISAM2, ISAM2_smoother2 )
// run ISAM2 with remaining factors // run ISAM2 with remaining factors
ExampleNonlinearFactorGraph factors2; ExampleNonlinearFactorGraph factors2;
for (int i=7;i<13;i++) factors2.push_back(smoother[i]); 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 // Create expected Bayes Tree by solving smoother with "natural" ordering
Ordering ordering; Ordering ordering;
@ -302,7 +302,7 @@ TEST( BayesTree, balanced_smoother_joint )
GaussianBayesNet actual4 = bayesTree.jointBayesNet<GaussianFactor>("x4","x1"); GaussianBayesNet actual4 = bayesTree.jointBayesNet<GaussianFactor>("x4","x1");
CHECK(assert_equal(expected4,actual4,1e-4)); CHECK(assert_equal(expected4,actual4,1e-4));
} }
#endif
/* ************************************************************************* */ /* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);} int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */ /* ************************************************************************* */