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

To resolve conflicts first, revert the commitment of merging JacobianSchurFactor into RegularJacobianFactor. 

JacobianSchurFactor is merged into RegularJacobianFactor. Derived classes from JacobianSchurFactor are changed to be derived from RegularJacobianFactor. (reverted from commit 7e0033208c)
release/4.3a0
Sungtae An 2015-01-01 17:35:58 -05:00
parent 7e0033208c
commit 7091c2bd2e
5 changed files with 119 additions and 77 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
gtsam/slam/JacobianFactorQ.h
View File

@ -6,20 +6,16 @@
#pragma once
#include <gtsam/slam/RegularJacobianFactor.h>
#include "JacobianSchurFactor.h"
namespace gtsam {
/**
* JacobianFactor for Schur complement that uses Q noise model
*/
template<size_t D>
class JacobianFactorQ: public RegularJacobianFactor<D> {
class JacobianFactorQ: public JacobianSchurFactor<D> {
private:
typedef RegularJacobianFactor<D> Base;
typedef Eigen::Matrix<double, 2, D> Matrix2D;
typedef std::pair<Key, Matrix2D> KeyMatrix2D;
typedef JacobianSchurFactor<D> Base;
public:
@ -29,7 +25,7 @@ public:
/// Empty constructor with keys
JacobianFactorQ(const FastVector<Key>& keys,
const SharedDiagonal& model = SharedDiagonal()) : RegularJacobianFactor<D>() {
const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor<D>() {
Matrix zeroMatrix = Matrix::Zero(0,D);
Vector zeroVector = Vector::Zero(0);
typedef std::pair<Key, Matrix> KeyMatrix;
@ -41,10 +37,10 @@ public:
}
/// Constructor
JacobianFactorQ(const std::vector<KeyMatrix2D>& Fblocks,
JacobianFactorQ(const std::vector<typename Base::KeyMatrix2D>& Fblocks,
const Matrix& E, const Matrix3& P, const Vector& b,
const SharedDiagonal& model = SharedDiagonal()) :
RegularJacobianFactor<D>() {
JacobianSchurFactor<D>() {
size_t j = 0, m2 = E.rows(), m = m2 / 2;
// Calculate projector Q
Matrix Q = eye(m2) - E * P * E.transpose();
@ -54,7 +50,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 KeyMatrix2D& it, Fblocks)
BOOST_FOREACH(const typename Base::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,35 +6,31 @@
*/
#pragma once
#include <gtsam/slam/RegularJacobianFactor.h>
#include <gtsam/slam/JacobianSchurFactor.h>
namespace gtsam {
/**
* JacobianFactor for Schur complement that uses Q noise model
*/
template<size_t D>
class JacobianFactorQR: public RegularJacobianFactor<D> {
class JacobianFactorQR: public JacobianSchurFactor<D> {
private:
typedef RegularJacobianFactor<D> Base;
typedef Eigen::Matrix<double, 2, D> Matrix2D;
typedef std::pair<Key, Matrix2D> KeyMatrix2D;
typedef JacobianSchurFactor<D> Base;
public:
/**
* Constructor
*/
JacobianFactorQR(const std::vector<KeyMatrix2D>& Fblocks,
JacobianFactorQR(const std::vector<typename Base::KeyMatrix2D>& Fblocks,
const Matrix& E, const Matrix3& P, const Vector& b,
const SharedDiagonal& model = SharedDiagonal()) :
RegularJacobianFactor<D>() {
JacobianSchurFactor<D>() {
// Create a number of Jacobian factors in a factor graph
GaussianFactorGraph gfg;
Symbol pointKey('p', 0);
size_t i = 0;
BOOST_FOREACH(const KeyMatrix2D& it, Fblocks) {
BOOST_FOREACH(const typename Base::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,29 +5,27 @@
*/
#pragma once
#include <gtsam/slam/RegularJacobianFactor.h>
#include "gtsam/slam/JacobianSchurFactor.h"
namespace gtsam {
/**
* JacobianFactor for Schur complement that uses Q noise model
*/
template<size_t D>
class JacobianFactorSVD: public RegularJacobianFactor<D> {
class JacobianFactorSVD: public JacobianSchurFactor<D> {
private:
public:
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()) : RegularJacobianFactor<D>() {
const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor<D>() {
Matrix zeroMatrix = Matrix::Zero(0,D);
Vector zeroVector = Vector::Zero(0);
std::vector<KeyMatrix> QF;
@ -39,7 +37,7 @@ public:
/// Constructor
JacobianFactorSVD(const std::vector<KeyMatrix2D>& Fblocks, const Matrix& Enull, const Vector& b,
const SharedDiagonal& model = SharedDiagonal()) : RegularJacobianFactor<D>() {
const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor<D>() {
size_t numKeys = Enull.rows() / 2;
size_t j = 0, m2 = 2*numKeys-3;
// PLAIN NULL SPACE TRICK

93
gtsam/slam/JacobianSchurFactor.h Normal file
View File

@ -0,0 +1,93 @@
/*
* @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,8 +18,6 @@
#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>
@ -32,9 +30,6 @@ 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;
@ -42,10 +37,6 @@ 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. */
@ -66,33 +57,6 @@ 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
@ -102,6 +66,11 @@ 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());
@ -112,7 +81,7 @@ public:
for (size_t pos = 0; pos < size(); ++pos)
{
Ax += Ab_(pos)
* ConstDMap(x + accumulatedDims[keys_[pos]], accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[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_) {
@ -125,15 +94,12 @@ public:
/// Again iterate over all A matrices and insert Ai^T into y
for (size_t pos = 0; pos < size(); ++pos){
DMap(y + accumulatedDims[keys_[pos]], accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]) += Ab_(
MapD(y + accumulatedDims[keys_[pos]], accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]) += Ab_(
pos).transpose() * Ax;
}
}
/**
* @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
*/
/** Raw memory access version of multiplyHessianAdd */
void multiplyHessianAdd(double alpha, const double* x, double* y) const {
if (empty()) return;
@ -202,13 +168,6 @@ 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;
};
}