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

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

@ -26,6 +26,7 @@
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp> #include <boost/tuple/tuple.hpp>
#include <boost/bind.hpp>
// template meta-programming headers // template meta-programming headers
#include <boost/mpl/vector.hpp> #include <boost/mpl/vector.hpp>
@ -48,7 +49,8 @@ namespace gtsam {
template<typename T> template<typename T>
class Expression; class Expression;
typedef std::map<Key, Eigen::Block<Matrix> > JacobianMap; typedef std::pair<Key, Eigen::Block<Matrix> > JacobianPair;
typedef std::vector<JacobianPair> JacobianMap;
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
/** /**
@ -78,7 +80,9 @@ struct CallRecord {
template<int ROWS, int COLS> template<int ROWS, int COLS>
void handleLeafCase(const Eigen::Matrix<double, ROWS, COLS>& dTdA, void handleLeafCase(const Eigen::Matrix<double, ROWS, COLS>& dTdA,
JacobianMap& jacobians, Key key) { 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.block < ROWS, COLS > (0, 0) += dTdA; // block makes HUGE difference it->second.block < ROWS, COLS > (0, 0) += dTdA; // block makes HUGE difference
} }
/// Handle Leaf Case for Dynamic Matrix type (slower) /// Handle Leaf Case for Dynamic Matrix type (slower)
@ -86,7 +90,9 @@ template<>
void handleLeafCase( void handleLeafCase(
const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>& dTdA, const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>& dTdA,
JacobianMap& jacobians, Key key) { 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; 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_ // Create and zero out blocks to be passed to expression_
JacobianMap blocks; JacobianMap blocks;
blocks.reserve(size());
for (DenseIndex i = 0; i < size(); i++) { for (DenseIndex i = 0; i < size(); i++) {
Matrix& Hi = H->at(i); Matrix& Hi = H->at(i);
Hi.resize(Dim, dimensions_[i]); Hi.resize(Dim, dimensions_[i]);
Hi.setZero(); // zero out Hi.setZero(); // zero out
Eigen::Block<Matrix> block = Hi.block(0, 0, Dim, dimensions_[i]); 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); T value = expression_.value(x, blocks);
@ -121,8 +122,9 @@ public:
// Create blocks into Ab_ to be passed to expression_ // Create blocks into Ab_ to be passed to expression_
JacobianMap blocks; JacobianMap blocks;
blocks.reserve(size());
for (DenseIndex i = 0; i < size(); i++) 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 // Evaluate error to get Jacobians and RHS vector b
T value = expression_.value(x, blocks); // <<< Reverse AD happens here ! 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 #ifdef GTSAM_USE_QUATERNIONS
EXPECT_LONGS_EQUAL(480,expression.traceSize()); // Todo, should be zero EXPECT_LONGS_EQUAL(480,expression.traceSize()); // Todo, should be zero
#else #else
EXPECT_LONGS_EQUAL(528,expression.traceSize()); // Todo, should be zero EXPECT_LONGS_EQUAL(448,expression.traceSize()); // Todo, should be zero
#endif #endif
set<Key> expected = list_of(1)(2); set<Key> expected = list_of(1)(2);
EXPECT(expected == expression.keys()); EXPECT(expected == expression.keys());

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

@ -63,10 +63,10 @@ TEST(Expression, Leaf) {
JacobianMap expected; JacobianMap expected;
Matrix H = eye(3); 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; 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); Rot3 actual2 = R.reverse(values, actualMap2);
EXPECT(assert_equal(someR, actual2)); EXPECT(assert_equal(someR, actual2));
EXPECT(actualMap2 == expected); EXPECT(actualMap2 == expected);