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

Unit test succeeds !!!

release/4.3a0
dellaert 2014-09-21 18:54:01 +02:00
parent 4bc82da85c
commit b47837462e
1 changed files with 20 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
gtsam_unstable/base/tests/testBAD.cpp
View File

@ -24,7 +24,7 @@
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <boost/make_shared.hpp> #include <boost/make_shared.hpp>
#include <boost/lambda/lambda.hpp> #include <boost/foreach.hpp>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
@ -119,6 +119,12 @@ public:
/// Get the Jacobians from the subtree and apply the chain rule /// Get the Jacobians from the subtree and apply the chain rule
std::map<Key, Matrix> jacobians(const Values& values) const { std::map<Key, Matrix> jacobians(const Values& values) const {
std::map<Key, Matrix> terms = expression_.jacobians(values); std::map<Key, Matrix> terms = expression_.jacobians(values);
Matrix H;
// TODO, wasted value calculation, create a combined call
f_(expression_.value(values), H);
typedef std::pair<Key, Matrix> Pair;
BOOST_FOREACH(const Pair& term, terms)
terms[term.first] = H * term.second;
return terms; return terms;
} }
}; };
@ -158,7 +164,17 @@ public:
std::map<Key, Matrix> jacobians(const Values& values) const { std::map<Key, Matrix> jacobians(const Values& values) const {
std::map<Key, Matrix> terms1 = expression1_.jacobians(values); std::map<Key, Matrix> terms1 = expression1_.jacobians(values);
std::map<Key, Matrix> terms2 = expression2_.jacobians(values); std::map<Key, Matrix> terms2 = expression2_.jacobians(values);
return terms2; Matrix H1, H2;
// TODO, wasted value calculation, create a combined call
f_(expression1_.value(values), expression2_.value(values), H1, H2);
std::map<Key, Matrix> terms;
// TODO if Key already exists, add !
typedef std::pair<Key, Matrix> Pair;
BOOST_FOREACH(const Pair& term, terms1)
terms[term.first] = H1 * term.second;
BOOST_FOREACH(const Pair& term, terms2)
terms[term.first] = H2 * term.second;
return terms;
} }
}; };
@ -167,6 +183,7 @@ public:
void printPair(std::pair<Key, Matrix> pair) { void printPair(std::pair<Key, Matrix> pair) {
std::cout << pair.first << ": " << pair.second << std::endl; std::cout << pair.first << ": " << pair.second << std::endl;
} }
// usage: std::for_each(terms.begin(), terms.end(), printPair);
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
/// AD Factor /// AD Factor
@ -179,7 +196,7 @@ class BADFactor: NonlinearFactor {
/// get value from expression and calculate error with respect to measurement /// get value from expression and calculate error with respect to measurement
Vector unwhitenedError(const Values& values) const { Vector unwhitenedError(const Values& values) const {
const T& value = expression_.value(values); const T& value = expression_.value(values);
return measurement_.localCoordinates(value); return value.localCoordinates(measurement_);
} }
public: public:
@ -215,7 +232,6 @@ public:
// value type is std::pair<Key, Matrix>, specifying the // value type is std::pair<Key, Matrix>, specifying the
// collection of keys and matrices making up the factor. // collection of keys and matrices making up the factor.
std::map<Key, Matrix> terms = expression_.jacobians(values); std::map<Key, Matrix> terms = expression_.jacobians(values);
std::for_each(terms.begin(), terms.end(), printPair);
Vector b = unwhitenedError(values); Vector b = unwhitenedError(values);
SharedDiagonal model = SharedDiagonal(); SharedDiagonal model = SharedDiagonal();
return boost::shared_ptr<JacobianFactor>( return boost::shared_ptr<JacobianFactor>(