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

JacobianSchurFactor is merged into RegularJacobianFactor. Derived classes from JacobianSchurFactor are changed to be derived from RegularJacobianFactor.

release/4.3a0
Sungtae An 2014-12-29 14:48:48 -05:00
parent 82a2d7f029
commit 7e0033208c
5 changed files with 77 additions and 119 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
gtsam/slam/JacobianFactorQ.h
View File

@ -6,16 +6,20 @@
#pragma once
#include "JacobianSchurFactor.h"
#include <gtsam/slam/RegularJacobianFactor.h>
namespace gtsam {
/**
* JacobianFactor for Schur complement that uses Q noise model
*/
template<size_t D>
class JacobianFactorQ: public JacobianSchurFactor<D> {
class JacobianFactorQ: public RegularJacobianFactor<D> {
typedef JacobianSchurFactor<D> Base;
private:
typedef RegularJacobianFactor<D> Base;
typedef Eigen::Matrix<double, 2, D> Matrix2D;
typedef std::pair<Key, Matrix2D> KeyMatrix2D;
public:
@ -25,7 +29,7 @@ public:
/// Empty constructor with keys
JacobianFactorQ(const FastVector<Key>& keys,
const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor<D>() {
const SharedDiagonal& model = SharedDiagonal()) : RegularJacobianFactor<D>() {
Matrix zeroMatrix = Matrix::Zero(0,D);
Vector zeroVector = Vector::Zero(0);
typedef std::pair<Key, Matrix> KeyMatrix;
@ -37,10 +41,10 @@ public:
}
/// Constructor
JacobianFactorQ(const std::vector<typename Base::KeyMatrix2D>& Fblocks,
JacobianFactorQ(const std::vector<KeyMatrix2D>& Fblocks,
const Matrix& E, const Matrix3& P, const Vector& b,
const SharedDiagonal& model = SharedDiagonal()) :
JacobianSchurFactor<D>() {
RegularJacobianFactor<D>() {
size_t j = 0, m2 = E.rows(), m = m2 / 2;
// Calculate projector Q
Matrix Q = eye(m2) - E * P * E.transpose();
@ -50,7 +54,7 @@ public:
std::vector < KeyMatrix > QF;
QF.reserve(m);
// Below, we compute each 2m*D block A_j = Q_j * F_j = (2m*2) * (2*D)
BOOST_FOREACH(const typename Base::KeyMatrix2D& it, Fblocks)
BOOST_FOREACH(const KeyMatrix2D& it, Fblocks)
QF.push_back(KeyMatrix(it.first, Q.block(0, 2 * j++, m2, 2) * it.second));
// Which is then passed to the normal JacobianFactor constructor
JacobianFactor::fillTerms(QF, Q * b, model);

16
gtsam/slam/JacobianFactorQR.h
View File

@ -6,31 +6,35 @@
*/
#pragma once
#include <gtsam/slam/JacobianSchurFactor.h>
#include <gtsam/slam/RegularJacobianFactor.h>
namespace gtsam {
/**
* JacobianFactor for Schur complement that uses Q noise model
*/
template<size_t D>
class JacobianFactorQR: public JacobianSchurFactor<D> {
class JacobianFactorQR: public RegularJacobianFactor<D> {
typedef JacobianSchurFactor<D> Base;
private:
typedef RegularJacobianFactor<D> Base;
typedef Eigen::Matrix<double, 2, D> Matrix2D;
typedef std::pair<Key, Matrix2D> KeyMatrix2D;
public:
/**
* Constructor
*/
JacobianFactorQR(const std::vector<typename Base::KeyMatrix2D>& Fblocks,
JacobianFactorQR(const std::vector<KeyMatrix2D>& Fblocks,
const Matrix& E, const Matrix3& P, const Vector& b,
const SharedDiagonal& model = SharedDiagonal()) :
JacobianSchurFactor<D>() {
RegularJacobianFactor<D>() {
// Create a number of Jacobian factors in a factor graph
GaussianFactorGraph gfg;
Symbol pointKey('p', 0);
size_t i = 0;
BOOST_FOREACH(const typename Base::KeyMatrix2D& it, Fblocks) {
BOOST_FOREACH(const KeyMatrix2D& it, Fblocks) {
gfg.add(pointKey, E.block<2, 3>(2 * i, 0), it.first, it.second,
b.segment<2>(2 * i), model);
i += 1;

12
gtsam/slam/JacobianFactorSVD.h
View File

@ -5,27 +5,29 @@
*/
#pragma once
#include "gtsam/slam/JacobianSchurFactor.h"
#include <gtsam/slam/RegularJacobianFactor.h>
namespace gtsam {
/**
* JacobianFactor for Schur complement that uses Q noise model
*/
template<size_t D>
class JacobianFactorSVD: public JacobianSchurFactor<D> {
class JacobianFactorSVD: public RegularJacobianFactor<D> {
public:
private:
typedef Eigen::Matrix<double, 2, D> Matrix2D;
typedef std::pair<Key, Matrix2D> KeyMatrix2D;
typedef std::pair<Key, Matrix> KeyMatrix;
public:
/// Default constructor
JacobianFactorSVD() {}
/// Empty constructor with keys
JacobianFactorSVD(const FastVector<Key>& keys,
const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor<D>() {
const SharedDiagonal& model = SharedDiagonal()) : RegularJacobianFactor<D>() {
Matrix zeroMatrix = Matrix::Zero(0,D);
Vector zeroVector = Vector::Zero(0);
std::vector<KeyMatrix> QF;
@ -37,7 +39,7 @@ public:
/// Constructor
JacobianFactorSVD(const std::vector<KeyMatrix2D>& Fblocks, const Matrix& Enull, const Vector& b,
const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor<D>() {
const SharedDiagonal& model = SharedDiagonal()) : RegularJacobianFactor<D>() {
size_t numKeys = Enull.rows() / 2;
size_t j = 0, m2 = 2*numKeys-3;
// PLAIN NULL SPACE TRICK

93
gtsam/slam/JacobianSchurFactor.h
View File

@ -1,93 +0,0 @@
/*
* @file JacobianSchurFactor.h
* @brief Jacobianfactor that combines and eliminates points
* @date Oct 27, 2013
* @uthor Frank Dellaert
*/
#pragma once
#include <gtsam/inference/Symbol.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <boost/foreach.hpp>
namespace gtsam {
/**
* JacobianFactor for Schur complement that uses Q noise model
*/
template<size_t D>
class JacobianSchurFactor: public JacobianFactor {
public:
typedef Eigen::Matrix<double, 2, D> Matrix2D;
typedef std::pair<Key, Matrix2D> KeyMatrix2D;
// 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;
/**
* @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;
}
/**
* @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;
}
/** y += alpha * A'*A*x */
void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const {
JacobianFactor::multiplyHessianAdd(alpha,x,y);
}
/**
* @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 {
// Vector Ax = (*this)*x;
// this->transposeMultiplyAdd(alpha,Ax,y);
if (empty()) return;
Vector Ax = zero(Ab_.rows());
// 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]);
// 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^e into y
for(size_t pos=0; pos<size(); ++pos)
DMap(y + D * keys_[pos]) += Ab_(pos).transpose() * Ax;
}
}; // class
} // gtsam

57
gtsam/slam/RegularJacobianFactor.h
View File

@ -18,6 +18,8 @@
#pragma once
#include <gtsam/inference/Symbol.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/VectorValues.h>
#include <boost/foreach.hpp>
@ -30,6 +32,9 @@ class RegularJacobianFactor: public JacobianFactor {
private:
typedef JacobianFactor Base;
typedef RegularJacobianFactor<D> This;
/** Use eigen magic to access raw memory */
typedef Eigen::Matrix<double, D, 1> DVector;
typedef Eigen::Map<DVector> DMap;
@ -37,6 +42,10 @@ private:
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. */
@ -57,6 +66,33 @@ public:
JacobianFactor(keys, augmentedMatrix, sigmas) {
}
/**
* @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;
}
/**
* @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;
}
/** 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
@ -66,11 +102,6 @@ public:
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());
@ -81,7 +112,7 @@ public:
for (size_t pos = 0; pos < size(); ++pos)
{
Ax += Ab_(pos)
* ConstMapD(x + accumulatedDims[keys_[pos]], accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]);
* ConstDMap(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_) {
@ -94,12 +125,15 @@ public:
/// 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_(
DMap(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;
@ -168,6 +202,13 @@ public:
}
}
/** Non-RAW memory access versions for testRegularImplicitSchurFactor
* TODO: They should be removed from Regular Factors
*/
using Base::multiplyHessianAdd;
using Base::hessianDiagonal;
using Base::gradientAtZero;
};
}