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

Merged in feature/BAD_vector (pull request #28) 

Changed the type of JacobianMap as std::vector
release/4.3a0
Frank Dellaert 2014-10-31 16:29:35 +01:00
parent f5c6ccca17 593edd1e5c
commit 8e7864dc96
4 changed files with 17 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
gtsam_unstable/nonlinear/Expression-inl.h
View File

@ -26,6 +26,7 @@
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/bind.hpp>
// template meta-programming headers
#include <boost/mpl/vector.hpp>
@ -48,7 +49,8 @@ namespace gtsam {
template<typename T>
class Expression;
typedef std::map<Key, Eigen::Block<Matrix> > JacobianMap;
typedef std::pair<Key, Eigen::Block<Matrix> > JacobianPair;
typedef std::vector<JacobianPair> JacobianMap;
//-----------------------------------------------------------------------------
/**
@ -78,15 +80,19 @@ struct CallRecord {
template<int ROWS, int COLS>
void handleLeafCase(const Eigen::Matrix<double, ROWS, COLS>& dTdA,
JacobianMap& jacobians, Key key) {
JacobianMap::iterator it = jacobians.find(key);
it->second.block<ROWS, COLS>(0, 0) += dTdA; // block makes HUGE difference
JacobianMap::iterator it = std::find_if(jacobians.begin(), jacobians.end(),
boost::bind(&JacobianPair::first, _1) == key);
assert(it!=jacobians.end());
it->second.block < ROWS, COLS > (0, 0) += dTdA; // block makes HUGE difference
}
/// Handle Leaf Case for Dynamic Matrix type (slower)
template<>
void handleLeafCase(
const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>& dTdA,
JacobianMap& jacobians, Key key) {
JacobianMap::iterator it = jacobians.find(key);
JacobianMap::iterator it = std::find_if(jacobians.begin(), jacobians.end(),
boost::bind(&JacobianPair::first, _1) == key);
assert(it!=jacobians.end());
it->second += dTdA;
}

6
gtsam_unstable/nonlinear/ExpressionFactor.h
View File

@ -87,12 +87,13 @@ public:
// Create and zero out blocks to be passed to expression_
JacobianMap blocks;
blocks.reserve(size());
for (DenseIndex i = 0; i < size(); i++) {
Matrix& Hi = H->at(i);
Hi.resize(Dim, dimensions_[i]);
Hi.setZero(); // zero out
Eigen::Block<Matrix> block = Hi.block(0, 0, Dim, dimensions_[i]);
blocks.insert(std::make_pair(keys_[i], block));
blocks.push_back(std::make_pair(keys_[i], block));
}
T value = expression_.value(x, blocks);
@ -121,8 +122,9 @@ public:
// Create blocks into Ab_ to be passed to expression_
JacobianMap blocks;
blocks.reserve(size());
for (DenseIndex i = 0; i < size(); i++)
blocks.insert(std::make_pair(keys_[i], Ab(i)));
blocks.push_back(std::make_pair(keys_[i], Ab(i)));
// Evaluate error to get Jacobians and RHS vector b
T value = expression_.value(x, blocks); // <<< Reverse AD happens here !

2
gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp
View File

@ -202,7 +202,7 @@ TEST(Expression, Snavely) {
#ifdef GTSAM_USE_QUATERNIONS
EXPECT_LONGS_EQUAL(480,expression.traceSize()); // Todo, should be zero
#else
EXPECT_LONGS_EQUAL(528,expression.traceSize()); // Todo, should be zero
EXPECT_LONGS_EQUAL(448,expression.traceSize()); // Todo, should be zero
#endif
set<Key> expected = list_of(1)(2);
EXPECT(expected == expression.keys());

4
gtsam_unstable/nonlinear/tests/testExpression.cpp
View File

@ -63,10 +63,10 @@ TEST(Expression, Leaf) {
JacobianMap expected;
Matrix H = eye(3);
expected.insert(make_pair(100, H.block(0, 0, 3, 3)));
expected.push_back(make_pair(100, H.block(0, 0, 3, 3)));
JacobianMap actualMap2;
actualMap2.insert(make_pair(100, H.block(0, 0, 3, 3)));
actualMap2.push_back(make_pair(100, H.block(0, 0, 3, 3)));
Rot3 actual2 = R.reverse(values, actualMap2);
EXPECT(assert_equal(someR, actual2));
EXPECT(actualMap2 == expected);