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

Merge branch 'feature/BAD_stackAb' into feature/BAD

release/4.3a0
dellaert 2014-10-15 01:20:01 +02:00
parent 625b939b66 9b1c9bbf37
commit 00d0a7b045
2 changed files with 14 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/base/VerticalBlockMatrix.h
View File

@ -74,8 +74,8 @@ namespace gtsam {
} }
/** Construct from a container of the sizes of each vertical block and a pre-prepared matrix. */ /** Construct from a container of the sizes of each vertical block and a pre-prepared matrix. */
template<typename CONTAINER> template<typename CONTAINER, typename DERIVED>
VerticalBlockMatrix(const CONTAINER& dimensions, const Matrix& matrix, bool appendOneDimension = false) : VerticalBlockMatrix(const CONTAINER& dimensions, const Eigen::MatrixBase<DERIVED>& matrix, bool appendOneDimension = false) :
matrix_(matrix), rowStart_(0), rowEnd_(matrix.rows()), blockStart_(0) matrix_(matrix), rowStart_(0), rowEnd_(matrix.rows()), blockStart_(0)
{ {
fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension); fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension);

27
gtsam_unstable/nonlinear/ExpressionFactor.h
View File

@ -20,6 +20,7 @@
#include <gtsam_unstable/nonlinear/Expression.h> #include <gtsam_unstable/nonlinear/Expression.h>
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <numeric>
namespace gtsam { namespace gtsam {
@ -78,30 +79,26 @@ public:
} }
} }
// Internal function to allocate a VerticalBlockMatrix and virtual boost::shared_ptr<GaussianFactor> linearize(const Values& x) const {
// create Eigen::Block<Matrix> views into it
VerticalBlockMatrix prepareBlocks(JacobianMap& blocks) const { // Construct VerticalBlockMatrix and views into it
// Get dimensions of Jacobian matrices // Get dimensions of Jacobian matrices
std::vector<size_t> dims = expression_.dimensions(); std::vector<size_t> dims = expression_.dimensions();
// Allocate memory on stack and create a view on it (saves a malloc)
size_t m1 = std::accumulate(dims.begin(),dims.end(),1);
double memory[T::dimension*m1];
Eigen::Map<Eigen::Matrix<double,T::dimension,Eigen::Dynamic> > matrix(memory,T::dimension,m1);
matrix.setZero(); // zero out
// Construct block matrix, is of right size but un-initialized // Construct block matrix, is of right size but un-initialized
VerticalBlockMatrix Ab(dims, T::dimension, true); VerticalBlockMatrix Ab(dims, matrix, true);
Ab.matrix().setZero(); // zero out
// Create blocks to be passed to expression_ // Create blocks to be passed to expression_
JacobianMap blocks;
for(DenseIndex i=0;i<size();i++) for(DenseIndex i=0;i<size();i++)
blocks.insert(std::make_pair(keys_[i], Ab(i))); blocks.insert(std::make_pair(keys_[i], Ab(i)));
return Ab;
}
virtual boost::shared_ptr<GaussianFactor> linearize(const Values& x) const {
// Construct VerticalBlockMatrix and views into it
JacobianMap blocks;
VerticalBlockMatrix Ab = prepareBlocks(blocks);
// Evaluate error to get Jacobians and RHS vector b // Evaluate error to get Jacobians and RHS vector b
T value = expression_.value(x, blocks); T value = expression_.value(x, blocks);
Ab(size()).col(0) = -measurement_.localCoordinates(value); Ab(size()).col(0) = -measurement_.localCoordinates(value);