Merge remote-tracking branch 'origin/feature/BAD_VerticalBlockMatrix' into feature/BAD_WriteableJacobianFactor

Conflicts:
	gtsam_unstable/nonlinear/ExpressionFactor.h
release/4.3a0
dellaert 2014-11-01 17:45:23 +01:00
commit 7aaf4dae8c
8 changed files with 42 additions and 47 deletions

Binary file not shown.

Binary file not shown.

Binary file not shown.

View File

@ -23,6 +23,7 @@
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/Manifold.h>
#include <gtsam/base/VerticalBlockMatrix.h>
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
@ -49,8 +50,25 @@ namespace gtsam {
template<typename T>
class Expression;
typedef std::pair<Key, Eigen::Block<Matrix> > JacobianPair;
typedef std::vector<JacobianPair> JacobianMap;
/**
* Expressions are designed to write their derivatives into an already allocated
* Jacobian of the correct size, of type VerticalBlockMatrix.
* The JacobianMap provides a mapping from keys to the underlying blocks.
*/
class JacobianMap {
const FastVector<Key>& keys_;
VerticalBlockMatrix& Ab_;
public:
JacobianMap(const FastVector<Key>& keys, VerticalBlockMatrix& Ab) :
keys_(keys), Ab_(Ab) {
}
/// Access via key
VerticalBlockMatrix::Block operator()(Key key) {
FastVector<Key>::const_iterator it = std::find(keys_.begin(),keys_.end(),key);
DenseIndex block = it - keys_.begin();
return Ab_(block);
}
};
//-----------------------------------------------------------------------------
/**
@ -80,20 +98,14 @@ struct CallRecord {
template<int ROWS, int COLS>
void handleLeafCase(const Eigen::Matrix<double, ROWS, COLS>& dTdA,
JacobianMap& jacobians, Key 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
jacobians(key).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 = std::find_if(jacobians.begin(), jacobians.end(),
boost::bind(&JacobianPair::first, _1) == key);
assert(it!=jacobians.end());
it->second += dTdA;
jacobians(key) += dTdA;
}
//-----------------------------------------------------------------------------

View File

@ -123,7 +123,7 @@ public:
}
/// Return value and derivatives, reverse AD version
T reverse(const Values& values, JacobianMap& jacobians) const {
T value(const Values& values, JacobianMap& jacobians) const {
// The following piece of code is absolutely crucial for performance.
// We allocate a block of memory on the stack, which can be done at runtime
// with modern C++ compilers. The traceExecution then fills this memory
@ -142,11 +142,6 @@ public:
return root_->value(values);
}
/// Return value and derivatives
T value(const Values& values, JacobianMap& jacobians) const {
return reverse(values, jacobians);
}
const boost::shared_ptr<ExpressionNode<T> >& root() const {
return root_;
}

View File

@ -124,7 +124,7 @@ public:
}
/**
* Error function *without* the NoiseModel, \f$ z-h(x) \f$.
* Error function *without* the NoiseModel, \f$ h(x)-z \f$.
* We override this method to provide
* both the function evaluation and its derivative(s) in H.
*/
@ -134,18 +134,19 @@ public:
// H should be pre-allocated
assert(H->size()==size());
// 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.push_back(std::make_pair(keys_[i], block));
}
VerticalBlockMatrix Ab(dimensions_, Dim);
// Wrap keys and VerticalBlockMatrix into structure passed to expression_
JacobianMap map(keys_, Ab);
Ab.matrix().setZero();
// Evaluate error to get Jacobians and RHS vector b
T value = expression_.value(x, map); // <<< Reverse AD happens here !
// Copy blocks into the vector of jacobians passed in
for (DenseIndex i = 0; i < size(); i++)
H->at(i) = Ab(i);
T value = expression_.value(x, blocks);
return measurement_.localCoordinates(value);
} else {
const T& value = expression_.value(x);
@ -167,14 +168,11 @@ public:
WriteableJacobianFactor>(keys_, dimensions_,
traits::dimension<T>::value, model);
// Create blocks into Ab_ to be passed to expression_
JacobianMap blocks;
blocks.reserve(size());
for (DenseIndex i = 0; i < size(); i++)
blocks.push_back(std::make_pair(keys_[i], factor->Ab()(i)));
// Wrap keys and VerticalBlockMatrix into structure passed to expression_
JacobianMap map(keys_, factor->Ab());
// Evaluate error to get Jacobians and RHS vector b
T value = expression_.value(x, blocks); // <<< Reverse AD happens here !
T value = expression_.value(x, map); // <<< Reverse AD happens here !
factor->Ab()(size()).col(0) = -measurement_.localCoordinates(value);
// Whiten the corresponding system now

View File

@ -46,11 +46,8 @@ static const Rot3 someR = Rot3::RzRyRx(1, 2, 3);
TEST(Expression, constant) {
Expression<Rot3> R(someR);
Values values;
JacobianMap actualMap;
Rot3 actual = R.value(values, actualMap);
Rot3 actual = R.value(values);
EXPECT(assert_equal(someR, actual));
JacobianMap expected;
EXPECT(actualMap == expected);
EXPECT_LONGS_EQUAL(0, R.traceSize())
}
@ -61,15 +58,8 @@ TEST(Expression, Leaf) {
Values values;
values.insert(100, someR);
JacobianMap expected;
Matrix H = eye(3);
expected.push_back(make_pair(100, H.block(0, 0, 3, 3)));
JacobianMap actualMap2;
actualMap2.push_back(make_pair(100, H.block(0, 0, 3, 3)));
Rot3 actual2 = R.reverse(values, actualMap2);
Rot3 actual2 = R.value(values);
EXPECT(assert_equal(someR, actual2));
EXPECT(actualMap2 == expected);
}
/* ************************************************************************* */

View File

@ -27,7 +27,7 @@
using namespace std;
using namespace gtsam;
#define time timeMultiThreaded
#define time timeSingleThreaded
boost::shared_ptr<Cal3_S2> fixedK(new Cal3_S2());