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

Grabbed some methods from JacobianSchurFactor, added VectorValues versions

release/4.3a0
dellaert 2015-02-22 08:10:34 +01:00
parent 3d8f980577
commit e15cfb3d33
1 changed files with 106 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

165
gtsam/slam/RegularJacobianFactor.h
View File

@ -21,25 +21,34 @@
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/VectorValues.h>
#include <boost/foreach.hpp>
#include <vector>
namespace gtsam {
/**
* JacobianFactor with constant sized blocks
* Provides raw memory access versions of linear operator.
* Is base class for JacobianQFactor, JacobianFactorQR, and JacobianFactorSVD
*/
template<size_t D>
class RegularJacobianFactor: public JacobianFactor {
private:
/** Use eigen magic to access raw memory */
// Use eigen magic to access raw memory
typedef Eigen::Matrix<double, D, 1> DVector;
typedef Eigen::Map<DVector> DMap;
typedef Eigen::Map<const DVector> ConstDMap;
public:
/// Default constructor
RegularJacobianFactor() {}
/** Construct an n-ary factor
* @tparam TERMS A container whose 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.
* TODO Verify terms are regular
*/
template<typename TERMS>
RegularJacobianFactor(const TERMS& terms, const Vector& b,
const SharedDiagonal& model = SharedDiagonal()) :
@ -49,7 +58,9 @@ public:
/** Constructor with arbitrary number keys, and where the augmented matrix is given all together
* instead of in block terms. Note that only the active view of the provided augmented matrix
* is used, and that the matrix data is copied into a newly-allocated matrix in the constructed
* factor. */
* factor.
* TODO Verify complies to regular
*/
template<typename KEYS>
RegularJacobianFactor(const KEYS& keys,
const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& sigmas =
@ -63,52 +74,11 @@ public:
JacobianFactor::multiplyHessianAdd(alpha, x, y);
}
/** Raw memory access version of multiplyHessianAdd y += alpha * A'*A*x
* Note: this is not assuming a fixed dimension for the variables,
* but requires the vector accumulatedDims to tell the dimension of
* each variable: e.g.: x0 has dim 3, x2 has dim 6, x3 has dim 2,
* then accumulatedDims is [0 3 9 11 13]
* NOTE: size of accumulatedDims is size of keys + 1!! */
void multiplyHessianAdd(double alpha, const double* x, double* y,
const std::vector<size_t>& accumulatedDims) const {
/// Use eigen magic to access raw memory
typedef Eigen::Matrix<double, Eigen::Dynamic, 1> VectorD;
typedef Eigen::Map<VectorD> MapD;
typedef Eigen::Map<const VectorD> ConstMapD;
if (empty())
return;
Vector Ax = zero(Ab_.rows());
/// Just iterate over all A matrices and multiply in correct config part (looping over keys)
/// E.g.: Jacobian A = [A0 A1 A2] multiplies x = [x0 x1 x2]'
/// Hence: Ax = A0 x0 + A1 x1 + A2 x2 (hence we loop over the keys and accumulate)
for (size_t pos = 0; pos < size(); ++pos) {
Ax += Ab_(pos)
* ConstMapD(x + accumulatedDims[keys_[pos]],
accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]);
}
/// Deal with noise properly, need to Double* whiten as we are dividing by variance
if (model_) {
model_->whitenInPlace(Ax);
model_->whitenInPlace(Ax);
}
/// multiply with alpha
Ax *= alpha;
/// Again iterate over all A matrices and insert Ai^T into y
for (size_t pos = 0; pos < size(); ++pos) {
MapD(y + accumulatedDims[keys_[pos]],
accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]) += Ab_(
pos).transpose() * Ax;
}
}
/** Raw memory access version of multiplyHessianAdd */
/**
* @brief double* Hessian-vector multiply, i.e. y += A'*(A*x)
* RAW memory access! Assumes keys start at 0 and go to M-1, and x and and y are laid out that way
*/
void multiplyHessianAdd(double alpha, const double* x, double* y) const {
if (empty())
return;
Vector Ax = zero(Ab_.rows());
@ -131,10 +101,13 @@ public:
DMap(y + D * keys_[pos]) += Ab_(pos).transpose() * Ax;
}
/** Raw memory access version of hessianDiagonal
* TODO: currently assumes all variables of the same size D (templated) and keys arranged from 0 to n
*/
virtual void hessianDiagonal(double* d) const {
/// Expose base class hessianDiagonal
virtual VectorValues hessianDiagonal() const {
return JacobianFactor::hessianDiagonal();
}
/// Raw memory access version of hessianDiagonal
void hessianDiagonal(double* d) const {
// Loop over all variables in the factor
for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) {
// Get the diagonal block, and insert its diagonal
@ -152,10 +125,13 @@ public:
}
}
/** Raw memory access version of gradientAtZero
* TODO: currently assumes all variables of the same size D (templated) and keys arranged from 0 to n
*/
virtual void gradientAtZero(double* d) const {
/// Expose base class gradientAtZero
virtual VectorValues gradientAtZero() const {
return JacobianFactor::gradientAtZero();
}
/// Raw memory access version of gradientAtZero
void gradientAtZero(double* d) const {
// Get vector b not weighted
Vector b = getb();
@ -179,7 +155,78 @@ public:
}
}
/**
* @brief double* Transpose Matrix-vector multiply, i.e. x += A'*e
* RAW memory access! Assumes keys start at 0 and go to M-1, and y is laid out that way
*/
void transposeMultiplyAdd(double alpha, const Vector& e, double* x) const {
Vector E = alpha * (model_ ? model_->whiten(e) : e);
// Just iterate over all A matrices and insert Ai^e into y
for (size_t pos = 0; pos < size(); ++pos)
DMap(x + D * keys_[pos]) += Ab_(pos).transpose() * E;
}
/**
* @brief double* Matrix-vector multiply, i.e. y = A*x
* RAW memory access! Assumes keys start at 0 and go to M-1, and x is laid out that way
*/
Vector operator*(const double* x) const {
Vector Ax = zero(Ab_.rows());
if (empty())
return Ax;
// Just iterate over all A matrices and multiply in correct config part
for (size_t pos = 0; pos < size(); ++pos)
Ax += Ab_(pos) * ConstDMap(x + D * keys_[pos]);
return model_ ? model_->whiten(Ax) : Ax;
}
/** Raw memory access version of multiplyHessianAdd y += alpha * A'*A*x
* Note: this is not assuming a fixed dimension for the variables,
* but requires the vector accumulatedDims to tell the dimension of
* each variable: e.g.: x0 has dim 3, x2 has dim 6, x3 has dim 2,
* then accumulatedDims is [0 3 9 11 13]
* NOTE: size of accumulatedDims is size of keys + 1!!
* TODO Frank asks: why is this here if not regular ????
*/
void multiplyHessianAdd(double alpha, const double* x, double* y,
const std::vector<size_t>& accumulatedDims) const {
/// Use Eigen magic to access raw memory
typedef Eigen::Map<Vector> VectorMap;
typedef Eigen::Map<const Vector> ConstVectorMap;
if (empty())
return;
Vector Ax = zero(Ab_.rows());
/// Just iterate over all A matrices and multiply in correct config part (looping over keys)
/// E.g.: Jacobian A = [A0 A1 A2] multiplies x = [x0 x1 x2]'
/// Hence: Ax = A0 x0 + A1 x1 + A2 x2 (hence we loop over the keys and accumulate)
for (size_t pos = 0; pos < size(); ++pos) {
size_t offset = accumulatedDims[keys_[pos]];
size_t dim = accumulatedDims[keys_[pos] + 1] - offset;
Ax += Ab_(pos) * ConstVectorMap(x + offset, dim);
}
/// Deal with noise properly, need to Double* whiten as we are dividing by variance
if (model_) {
model_->whitenInPlace(Ax);
model_->whitenInPlace(Ax);
}
/// multiply with alpha
Ax *= alpha;
/// Again iterate over all A matrices and insert Ai^T into y
for (size_t pos = 0; pos < size(); ++pos) {
size_t offset = accumulatedDims[keys_[pos]];
size_t dim = accumulatedDims[keys_[pos] + 1] - offset;
VectorMap(y + offset, dim) += Ab_(pos).transpose() * Ax;
}
}
};
// end class RegularJacobianFactor
}
}// end namespace gtsam