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

Switched to non-keyed Fblocks

release/4.3a0
dellaert 2015-03-04 21:39:35 -08:00
parent 0a287e25e7
commit 175dae30ec
5 changed files with 130 additions and 262 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
gtsam/slam/JacobianFactorQ.h
View File

@ -28,7 +28,7 @@ class JacobianFactorQ: public RegularJacobianFactor<D> {
typedef RegularJacobianFactor<D> Base; typedef RegularJacobianFactor<D> Base;
typedef Eigen::Matrix<double, ZDim, D> MatrixZD; typedef Eigen::Matrix<double, ZDim, D> MatrixZD;
typedef std::pair<Key, MatrixZD> KeyMatrixZD; typedef std::pair<Key, Matrix> KeyMatrix;
public: public:
@ -42,7 +42,6 @@ public:
Base() { Base() {
Matrix zeroMatrix = Matrix::Zero(0, D); Matrix zeroMatrix = Matrix::Zero(0, D);
Vector zeroVector = Vector::Zero(0); Vector zeroVector = Vector::Zero(0);
typedef std::pair<Key, Matrix> KeyMatrix;
std::vector<KeyMatrix> QF; std::vector<KeyMatrix> QF;
QF.reserve(keys.size()); QF.reserve(keys.size());
BOOST_FOREACH(const Key& key, keys) BOOST_FOREACH(const Key& key, keys)
@ -51,22 +50,23 @@ public:
} }
/// Constructor /// Constructor
JacobianFactorQ(const std::vector<KeyMatrixZD>& Fblocks, const Matrix& E, JacobianFactorQ(const FastVector<Key>& keys,
const Matrix3& P, const Vector& b, const SharedDiagonal& model = const std::vector<MatrixZD>& FBlocks, const Matrix& E, const Matrix3& P,
SharedDiagonal()) : const Vector& b, const SharedDiagonal& model = SharedDiagonal()) :
Base() { Base() {
size_t j = 0, m2 = E.rows(), m = m2 / ZDim; size_t j = 0, m2 = E.rows(), m = m2 / ZDim;
// Calculate projector Q // Calculate projector Q
Matrix Q = eye(m2) - E * P * E.transpose(); Matrix Q = eye(m2) - E * P * E.transpose();
// Calculate pre-computed Jacobian matrices // Calculate pre-computed Jacobian matrices
// TODO: can we do better ? // TODO: can we do better ?
typedef std::pair<Key, Matrix> KeyMatrix;
std::vector<KeyMatrix> QF; std::vector<KeyMatrix> QF;
QF.reserve(m); QF.reserve(m);
// Below, we compute each mZDim*D block A_j = Q_j * F_j = (mZDim*ZDim) * (Zdim*D) // Below, we compute each mZDim*D block A_j = Q_j * F_j = (mZDim*ZDim) * (Zdim*D)
BOOST_FOREACH(const KeyMatrixZD& it, Fblocks) for (size_t k = 0; k < FBlocks.size(); ++k) {
Key key = keys[k];
QF.push_back( QF.push_back(
KeyMatrix(it.first, Q.block(0, ZDim * j++, m2, ZDim) * it.second)); KeyMatrix(key, Q.block(0, ZDim * j++, m2, ZDim) * FBlocks[k]));
}
// Which is then passed to the normal JacobianFactor constructor // Which is then passed to the normal JacobianFactor constructor
JacobianFactor::fillTerms(QF, Q * b, model); JacobianFactor::fillTerms(QF, Q * b, model);
} }

17
gtsam/slam/JacobianFactorQR.h
View File

@ -7,8 +7,8 @@
#pragma once #pragma once
#include <gtsam/linear/GaussianFactorGraph.h> #include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/linear/RegularJacobianFactor.h> #include <gtsam/linear/RegularJacobianFactor.h>
#include <gtsam/inference/Symbol.h>
namespace gtsam { namespace gtsam {
@ -22,25 +22,24 @@ class JacobianFactorQR: public RegularJacobianFactor<D> {
typedef RegularJacobianFactor<D> Base; typedef RegularJacobianFactor<D> Base;
typedef Eigen::Matrix<double, ZDim, D> MatrixZD; typedef Eigen::Matrix<double, ZDim, D> MatrixZD;
typedef std::pair<Key, MatrixZD> KeyMatrixZD;
public: public:
/** /**
* Constructor * Constructor
*/ */
JacobianFactorQR(const std::vector<KeyMatrixZD>& Fblocks, const Matrix& E, JacobianFactorQR(const FastVector<Key>& keys,
const Matrix3& P, const Vector& b, // const std::vector<MatrixZD>& FBlocks, const Matrix& E, const Matrix3& P,
const Vector& b, //
const SharedDiagonal& model = SharedDiagonal()) : const SharedDiagonal& model = SharedDiagonal()) :
Base() { Base() {
// Create a number of Jacobian factors in a factor graph // Create a number of Jacobian factors in a factor graph
GaussianFactorGraph gfg; GaussianFactorGraph gfg;
Symbol pointKey('p', 0); Symbol pointKey('p', 0);
size_t i = 0; for (size_t k = 0; k < FBlocks.size(); ++k) {
BOOST_FOREACH(const KeyMatrixZD& it, Fblocks) { Key key = keys[k];
gfg.add(pointKey, E.block<ZDim, 3>(ZDim * i, 0), it.first, it.second, gfg.add(pointKey, E.block<ZDim, 3>(ZDim * k, 0), key, FBlocks[k],
b.segment<ZDim>(ZDim * i), model); b.segment < ZDim > (ZDim * k), model);
i += 1;
} }
//gfg.print("gfg"); //gfg.print("gfg");

274
gtsam/slam/RegularImplicitSchurFactor.h
View File

@ -7,9 +7,9 @@
#pragma once #pragma once
#include <gtsam/geometry/CameraSet.h>
#include <gtsam/linear/JacobianFactor.h> #include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/VectorValues.h> #include <gtsam/linear/VectorValues.h>
#include <gtsam/base/SymmetricBlockMatrix.h>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <iosfwd> #include <iosfwd>
@ -18,7 +18,7 @@ namespace gtsam {
/** /**
* RegularImplicitSchurFactor * RegularImplicitSchurFactor
*/ */
template<size_t D, size_t Z = 2> // template<class CAMERA>
class RegularImplicitSchurFactor: public GaussianFactor { class RegularImplicitSchurFactor: public GaussianFactor {
public: public:
@ -27,22 +27,21 @@ public:
protected: protected:
typedef Eigen::Matrix<double, Z, D> Matrix2D; ///< type of an F block // This factor is closely related to a CameraSet
typedef Eigen::Matrix<double, D, D> MatrixDD; ///< camera hessian typedef CameraSet<CAMERA> Set;
typedef std::pair<Key, Matrix2D> KeyMatrix2D; ///< named F block
const std::vector<KeyMatrix2D> Fblocks_; ///< All Z*D F blocks (one for each camera) typedef typename CAMERA::Measurement Z;
const Matrix3 PointCovariance_; ///< the 3*3 matrix P = inv(E'E) (Z*Z if degenerate) static const int D = traits<CAMERA>::dimension; ///< Camera dimension
static const int ZDim = traits<Z>::dimension; ///< Measurement dimension
typedef Eigen::Matrix<double, ZDim, D> MatrixZD; ///< type of an F block
typedef Eigen::Matrix<double, D, D> MatrixDD; ///< camera hessian
const std::vector<MatrixZD> FBlocks_; ///< All ZDim*D F blocks (one for each camera)
const Matrix3 PointCovariance_; ///< the 3*3 matrix P = inv(E'E) (ZDim*ZDim if degenerate)
const Matrix E_; ///< The 2m*3 E Jacobian with respect to the point const Matrix E_; ///< The 2m*3 E Jacobian with respect to the point
const Vector b_; ///< 2m-dimensional RHS vector const Vector b_; ///< 2m-dimensional RHS vector
/// initialize keys from Fblocks
void initKeys() {
keys_.reserve(Fblocks_.size());
BOOST_FOREACH(const KeyMatrix2D& it, Fblocks_)
keys_.push_back(it.first);
}
public: public:
/// Constructor /// Constructor
@ -50,29 +49,29 @@ public:
} }
/// Construct from blocks of F, E, inv(E'*E), and RHS vector b /// Construct from blocks of F, E, inv(E'*E), and RHS vector b
RegularImplicitSchurFactor(const std::vector<KeyMatrix2D>& Fblocks, RegularImplicitSchurFactor(const FastVector<Key>& keys,
const Matrix& E, const Matrix3& P, const Vector& b) : const std::vector<MatrixZD>& FBlocks, const Matrix& E, const Matrix3& P,
Fblocks_(Fblocks), PointCovariance_(P), E_(E), b_(b) { const Vector& b) :
initKeys(); GaussianFactor(keys), FBlocks_(FBlocks), PointCovariance_(P), E_(E), b_(b) {
} }
/// Destructor /// Destructor
virtual ~RegularImplicitSchurFactor() { virtual ~RegularImplicitSchurFactor() {
} }
inline std::vector<KeyMatrix2D>& Fblocks() const { std::vector<MatrixZD>& FBlocks() const {
return Fblocks_; return FBlocks_;
} }
inline const Matrix& E() const { const Matrix& E() const {
return E_; return E_;
} }
inline const Vector& b() const { const Vector& b() const {
return b_; return b_;
} }
inline const Matrix3& getPointCovariance() const { const Matrix3& getPointCovariance() const {
return PointCovariance_; return PointCovariance_;
} }
@ -82,7 +81,7 @@ public:
std::cout << " RegularImplicitSchurFactor " << std::endl; std::cout << " RegularImplicitSchurFactor " << std::endl;
Factor::print(s); Factor::print(s);
for (size_t pos = 0; pos < size(); ++pos) { for (size_t pos = 0; pos < size(); ++pos) {
std::cout << "Fblock:\n" << Fblocks_[pos].second << std::endl; std::cout << "Fblock:\n" << FBlocks_[pos] << std::endl;
} }
std::cout << "PointCovariance:\n" << PointCovariance_ << std::endl; std::cout << "PointCovariance:\n" << PointCovariance_ << std::endl;
std::cout << "E:\n" << E_ << std::endl; std::cout << "E:\n" << E_ << std::endl;
@ -94,13 +93,12 @@ public:
const This* f = dynamic_cast<const This*>(&lf); const This* f = dynamic_cast<const This*>(&lf);
if (!f) if (!f)
return false; return false;
for (size_t pos = 0; pos < size(); ++pos) { for (size_t k = 0; k < FBlocks_.size(); ++k) {
if (keys_[pos] != f->keys_[pos]) if (keys_[k] != f->keys_[k])
return false; return false;
if (Fblocks_[pos].first != f->Fblocks_[pos].first) if (FBlocks_[k] != f->FBlocks_[k])
return false; return false;
if (!equal_with_abs_tol(Fblocks_[pos].second, f->Fblocks_[pos].second, if (!equal_with_abs_tol(FBlocks_[k], f->FBlocks_[k], tol))
tol))
return false; return false;
} }
return equal_with_abs_tol(PointCovariance_, f->PointCovariance_, tol) return equal_with_abs_tol(PointCovariance_, f->PointCovariance_, tol)
@ -124,131 +122,12 @@ public:
return std::make_pair(Matrix(), Vector()); return std::make_pair(Matrix(), Vector());
} }
/**
* Do Schur complement, given Jacobian as F,E,P, return SymmetricBlockMatrix
* Fast version - works on with sparsity
*/
static void SparseSchurComplement(const std::vector<KeyMatrix2D>& Fblocks,
const Matrix& E, const Matrix3& P /*Point Covariance*/, const Vector& b,
/*output ->*/SymmetricBlockMatrix& augmentedHessian) {
// Schur complement trick
// G = F' * F - F' * E * P * E' * F
// g = F' * (b - E * P * E' * b)
// a single point is observed in m cameras
size_t m = Fblocks.size();
// Blockwise Schur complement
for (size_t i = 0; i < m; i++) { // for each camera
const Matrix2D& Fi = Fblocks.at(i).second;
const Matrix23 Ei_P = E.block<Z, 3>(Z * i, 0) * P;
// D = (Dx2) * (Z)
augmentedHessian(i, m) = Fi.transpose() * b.segment<Z>(Z * i) // F' * b
- Fi.transpose() * (Ei_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1)
// (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
augmentedHessian(i, i) = Fi.transpose()
* (Fi - Ei_P * E.block<Z, 3>(Z * i, 0).transpose() * Fi);
// upper triangular part of the hessian
for (size_t j = i + 1; j < m; j++) { // for each camera
const Matrix2D& Fj = Fblocks.at(j).second;
// (DxD) = (Dx2) * ( (2x2) * (2xD) )
augmentedHessian(i, j) = -Fi.transpose()
* (Ei_P * E.block<Z, 3>(Z * j, 0).transpose() * Fj);
}
} // end of for over cameras
}
/**
* Applies Schur complement (exploiting block structure) to get a smart factor on cameras,
* and adds the contribution of the smart factor to a pre-allocated augmented Hessian.
*/
static void UpdateSparseSchurComplement(
const std::vector<KeyMatrix2D>& Fblocks, const Matrix& E,
const Matrix3& P /*Point Covariance*/, const Vector& b, const double f,
const FastVector<Key>& allKeys, const FastVector<Key>& keys,
/*output ->*/SymmetricBlockMatrix& augmentedHessian) {
FastMap<Key, size_t> KeySlotMap;
for (size_t slot = 0; slot < allKeys.size(); slot++)
KeySlotMap.insert(std::make_pair(allKeys[slot], slot));
// Schur complement trick
// G = F' * F - F' * E * P * E' * F
// g = F' * (b - E * P * E' * b)
MatrixDD matrixBlock;
typedef SymmetricBlockMatrix::Block Block; ///< A block from the Hessian matrix
// a single point is observed in m cameras
size_t m = Fblocks.size(); // cameras observing current point
size_t aug_m = (augmentedHessian.rows() - 1) / D; // all cameras in the group
// Blockwise Schur complement
for (size_t i = 0; i < m; i++) { // for each camera in the current factor
const Matrix2D& Fi = Fblocks.at(i).second;
const Matrix23 Ei_P = E.block<Z, 3>(Z * i, 0) * P;
// D = (DxZDim) * (Z)
// allKeys are the list of all camera keys in the group, e.g, (1,3,4,5,7)
// we should map those to a slot in the local (grouped) hessian (0,1,2,3,4)
// Key cameraKey_i = this->keys_[i];
DenseIndex aug_i = KeySlotMap.at(keys[i]);
// information vector - store previous vector
// vectorBlock = augmentedHessian(aug_i, aug_m).knownOffDiagonal();
// add contribution of current factor
augmentedHessian(aug_i, aug_m) =
augmentedHessian(aug_i, aug_m).knownOffDiagonal()
+ Fi.transpose() * b.segment<Z>(Z * i) // F' * b
- Fi.transpose() * (Ei_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1)
// (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
// main block diagonal - store previous block
matrixBlock = augmentedHessian(aug_i, aug_i);
// add contribution of current factor
augmentedHessian(aug_i, aug_i) = matrixBlock
+ (Fi.transpose()
* (Fi - Ei_P * E.block<Z, 3>(Z * i, 0).transpose() * Fi));
// upper triangular part of the hessian
for (size_t j = i + 1; j < m; j++) { // for each camera
const Matrix2D& Fj = Fblocks.at(j).second;
//Key cameraKey_j = this->keys_[j];
DenseIndex aug_j = KeySlotMap.at(keys[j]);
// (DxD) = (DxZDim) * ( (ZDimxZDim) * (ZDimxD) )
// off diagonal block - store previous block
// matrixBlock = augmentedHessian(aug_i, aug_j).knownOffDiagonal();
// add contribution of current factor
augmentedHessian(aug_i, aug_j) =
augmentedHessian(aug_i, aug_j).knownOffDiagonal()
- Fi.transpose()
* (Ei_P * E.block<Z, 3>(Z * j, 0).transpose() * Fj);
}
} // end of for over cameras
augmentedHessian(aug_m, aug_m)(0, 0) += f;
}
/// *Compute* full augmented information matrix /// *Compute* full augmented information matrix
virtual Matrix augmentedInformation() const { virtual Matrix augmentedInformation() const {
// Create a SymmetricBlockMatrix
int m = this->keys_.size();
size_t M1 = D * m + 1;
std::vector<DenseIndex> dims(m + 1); // this also includes the b term
std::fill(dims.begin(), dims.end() - 1, D);
dims.back() = 1;
SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1));
// Do the Schur complement // Do the Schur complement
SparseSchurComplement(Fblocks_, E_, PointCovariance_, b_, augmentedHessian); SymmetricBlockMatrix augmentedHessian = Set::SchurComplement(FBlocks_, E_,
PointCovariance_, b_);
return augmentedHessian.matrix(); return augmentedHessian.matrix();
} }
@ -265,14 +144,14 @@ public:
// diag(Hessian) = diag(F' * (I - E * PointCov * E') * F); // diag(Hessian) = diag(F' * (I - E * PointCov * E') * F);
VectorValues d; VectorValues d;
for (size_t pos = 0; pos < size(); ++pos) { // for each camera for (size_t k = 0; k < size(); ++k) { // for each camera
Key j = keys_[pos]; Key j = keys_[k];
// Calculate Fj'*Ej for the current camera (observing a single point) // Calculate Fj'*Ej for the current camera (observing a single point)
// D x 3 = (D x Z) * (Z x 3) // D x 3 = (D x ZDim) * (ZDim x 3)
const Matrix2D& Fj = Fblocks_[pos].second; const MatrixZD& Fj = FBlocks_[k];
Eigen::Matrix<double, D, 3> FtE = Fj.transpose() Eigen::Matrix<double, D, 3> FtE = Fj.transpose()
* E_.block<Z, 3>(Z * pos, 0); * E_.block<ZDim, 3>(ZDim * k, 0);
Eigen::Matrix<double, D, 1> dj; Eigen::Matrix<double, D, 1> dj;
for (size_t k = 0; k < D; ++k) { // for each diagonal element of the camera hessian for (size_t k = 0; k < D; ++k) { // for each diagonal element of the camera hessian
@ -301,10 +180,10 @@ public:
Key j = keys_[pos]; Key j = keys_[pos];
// Calculate Fj'*Ej for the current camera (observing a single point) // Calculate Fj'*Ej for the current camera (observing a single point)
// D x 3 = (D x Z) * (Z x 3) // D x 3 = (D x ZDim) * (ZDim x 3)
const Matrix2D& Fj = Fblocks_[pos].second; const MatrixZD& Fj = FBlocks_[pos];
Eigen::Matrix<double, D, 3> FtE = Fj.transpose() Eigen::Matrix<double, D, 3> FtE = Fj.transpose()
* E_.block<Z, 3>(Z * pos, 0); * E_.block<ZDim, 3>(ZDim * pos, 0);
DVector dj; DVector dj;
for (size_t k = 0; k < D; ++k) { // for each diagonal element of the camera hessian for (size_t k = 0; k < D; ++k) { // for each diagonal element of the camera hessian
@ -323,28 +202,28 @@ public:
for (size_t pos = 0; pos < size(); ++pos) { for (size_t pos = 0; pos < size(); ++pos) {
Key j = keys_[pos]; Key j = keys_[pos];
// F'*F - F'*E*P*E'*F e.g. (9*2)*(2*9) - (9*2)*(2*3)*(3*3)*(3*2)*(2*9) // F'*F - F'*E*P*E'*F e.g. (9*2)*(2*9) - (9*2)*(2*3)*(3*3)*(3*2)*(2*9)
const Matrix2D& Fj = Fblocks_[pos].second; const MatrixZD& Fj = FBlocks_[pos];
// Eigen::Matrix<double, D, 3> FtE = Fj.transpose() // Eigen::Matrix<double, D, 3> FtE = Fj.transpose()
// * E_.block<Z, 3>(Z * pos, 0); // * E_.block<ZDim, 3>(ZDim * pos, 0);
// blocks[j] = Fj.transpose() * Fj // blocks[j] = Fj.transpose() * Fj
// - FtE * PointCovariance_ * FtE.transpose(); // - FtE * PointCovariance_ * FtE.transpose();
const Matrix23& Ej = E_.block<Z, 3>(Z * pos, 0); const Matrix23& Ej = E_.block<ZDim, 3>(ZDim * pos, 0);
blocks[j] = Fj.transpose() blocks[j] = Fj.transpose()
* (Fj - Ej * PointCovariance_ * Ej.transpose() * Fj); * (Fj - Ej * PointCovariance_ * Ej.transpose() * Fj);
// F'*(I - E*P*E')*F, TODO: this should work, but it does not :-( // F'*(I - E*P*E')*F, TODO: this should work, but it does not :-(
// static const Eigen::Matrix<double, Z, Z> I2 = eye(Z); // static const Eigen::Matrix<double, ZDim, ZDim> I2 = eye(ZDim);
// Matrix2 Q = // // Matrix2 Q = //
// I2 - E_.block<Z, 3>(Z * pos, 0) * PointCovariance_ * E_.block<Z, 3>(Z * pos, 0).transpose(); // I2 - E_.block<ZDim, 3>(ZDim * pos, 0) * PointCovariance_ * E_.block<ZDim, 3>(ZDim * pos, 0).transpose();
// blocks[j] = Fj.transpose() * Q * Fj; // blocks[j] = Fj.transpose() * Q * Fj;
} }
return blocks; return blocks;
} }
virtual GaussianFactor::shared_ptr clone() const { virtual GaussianFactor::shared_ptr clone() const {
return boost::make_shared<RegularImplicitSchurFactor<D, Z> >(Fblocks_, return boost::make_shared<RegularImplicitSchurFactor<CAMERA> >(keys_,
PointCovariance_, E_, b_); FBlocks_, PointCovariance_, E_, b_);
throw std::runtime_error( throw std::runtime_error(
"RegularImplicitSchurFactor::clone non implemented"); "RegularImplicitSchurFactor::clone non implemented");
} }
@ -353,8 +232,8 @@ public:
} }
virtual GaussianFactor::shared_ptr negate() const { virtual GaussianFactor::shared_ptr negate() const {
return boost::make_shared<RegularImplicitSchurFactor<D, Z> >(Fblocks_, return boost::make_shared<RegularImplicitSchurFactor<CAMERA> >(keys_,
PointCovariance_, E_, b_); FBlocks_, PointCovariance_, E_, b_);
throw std::runtime_error( throw std::runtime_error(
"RegularImplicitSchurFactor::negate non implemented"); "RegularImplicitSchurFactor::negate non implemented");
} }
@ -374,23 +253,24 @@ public:
typedef std::vector<Vector2> Error2s; typedef std::vector<Vector2> Error2s;
/** /**
* @brief Calculate corrected error Q*(e-Z*b) = (I - E*P*E')*(e-Z*b) * @brief Calculate corrected error Q*(e-ZDim*b) = (I - E*P*E')*(e-ZDim*b)
*/ */
void projectError2(const Error2s& e1, Error2s& e2) const { void projectError2(const Error2s& e1, Error2s& e2) const {
// d1 = E.transpose() * (e1-Z*b) = (3*2m)*2m // d1 = E.transpose() * (e1-ZDim*b) = (3*2m)*2m
Vector3 d1; Vector3 d1;
d1.setZero(); d1.setZero();
for (size_t k = 0; k < size(); k++) for (size_t k = 0; k < size(); k++)
d1 += E_.block<Z, 3>(Z * k, 0).transpose() d1 += E_.block<ZDim, 3>(ZDim * k, 0).transpose()
* (e1[k] - Z * b_.segment<Z>(k * Z)); * (e1[k] - ZDim * b_.segment<ZDim>(k * ZDim));
// d2 = E.transpose() * e1 = (3*2m)*2m // d2 = E.transpose() * e1 = (3*2m)*2m
Vector3 d2 = PointCovariance_ * d1; Vector3 d2 = PointCovariance_ * d1;
// e3 = alpha*(e1 - E*d2) = 1*[2m-(2m*3)*3] // e3 = alpha*(e1 - E*d2) = 1*[2m-(2m*3)*3]
for (size_t k = 0; k < size(); k++) for (size_t k = 0; k < size(); k++)
e2[k] = e1[k] - Z * b_.segment<Z>(k * Z) - E_.block<Z, 3>(Z * k, 0) * d2; e2[k] = e1[k] - ZDim * b_.segment<ZDim>(k * ZDim)
- E_.block<ZDim, 3>(ZDim * k, 0) * d2;
} }
/* /*
@ -410,7 +290,7 @@ public:
// e1 = F * x - b = (2m*dm)*dm // e1 = F * x - b = (2m*dm)*dm
for (size_t k = 0; k < size(); ++k) for (size_t k = 0; k < size(); ++k)
e1[k] = Fblocks_[k].second * x.at(keys_[k]); e1[k] = FBlocks_[k] * x.at(keys_[k]);
projectError2(e1, e2); projectError2(e1, e2);
double result = 0; double result = 0;
@ -432,7 +312,7 @@ public:
// e1 = F * x - b = (2m*dm)*dm // e1 = F * x - b = (2m*dm)*dm
for (size_t k = 0; k < size(); ++k) for (size_t k = 0; k < size(); ++k)
e1[k] = Fblocks_[k].second * x.at(keys_[k]) - b_.segment<Z>(k * Z); e1[k] = FBlocks_[k] * x.at(keys_[k]) - b_.segment<ZDim>(k * ZDim);
projectError(e1, e2); projectError(e1, e2);
double result = 0; double result = 0;
@ -451,14 +331,14 @@ public:
Vector3 d1; Vector3 d1;
d1.setZero(); d1.setZero();
for (size_t k = 0; k < size(); k++) for (size_t k = 0; k < size(); k++)
d1 += E_.block<Z, 3>(Z * k, 0).transpose() * e1[k]; d1 += E_.block<ZDim, 3>(ZDim * k, 0).transpose() * e1[k];
// d2 = E.transpose() * e1 = (3*2m)*2m // d2 = E.transpose() * e1 = (3*2m)*2m
Vector3 d2 = PointCovariance_ * d1; Vector3 d2 = PointCovariance_ * d1;
// e3 = alpha*(e1 - E*d2) = 1*[2m-(2m*3)*3] // e3 = alpha*(e1 - E*d2) = 1*[2m-(2m*3)*3]
for (size_t k = 0; k < size(); k++) for (size_t k = 0; k < size(); k++)
e2[k] = e1[k] - E_.block<Z, 3>(Z * k, 0) * d2; e2[k] = e1[k] - E_.block<ZDim, 3>(ZDim * k, 0) * d2;
} }
/// Scratch space for multiplyHessianAdd /// Scratch space for multiplyHessianAdd
@ -480,19 +360,17 @@ public:
e2.resize(size()); e2.resize(size());
// e1 = F * x = (2m*dm)*dm // e1 = F * x = (2m*dm)*dm
size_t k = 0; for (size_t k = 0; k < size(); ++k) {
BOOST_FOREACH(const KeyMatrix2D& it, Fblocks_) { Key key = keys_[k];
Key key = it.first; e1[k] = FBlocks_[k] * ConstDMap(x + D * key);
e1[k++] = it.second * ConstDMap(x + D * key);
} }
projectError(e1, e2); projectError(e1, e2);
// y += F.transpose()*e2 = (2d*2m)*2m // y += F.transpose()*e2 = (2d*2m)*2m
k = 0; for (size_t k = 0; k < size(); ++k) {
BOOST_FOREACH(const KeyMatrix2D& it, Fblocks_) { Key key = keys_[k];
Key key = it.first; DMap(y + D * key) += FBlocks_[k].transpose() * alpha * e2[k];
DMap(y + D * key) += it.second.transpose() * alpha * e2[k++];
} }
} }
@ -513,7 +391,7 @@ public:
// e1 = F * x = (2m*dm)*dm // e1 = F * x = (2m*dm)*dm
for (size_t k = 0; k < size(); ++k) for (size_t k = 0; k < size(); ++k)
e1[k] = Fblocks_[k].second * x.at(keys_[k]); e1[k] = FBlocks_[k] * x.at(keys_[k]);
projectError(e1, e2); projectError(e1, e2);
@ -525,8 +403,8 @@ public:
Vector& yi = it.first->second; Vector& yi = it.first->second;
// Create the value as a zero vector if it does not exist. // Create the value as a zero vector if it does not exist.
if (it.second) if (it.second)
yi = Vector::Zero(Fblocks_[k].second.cols()); yi = Vector::Zero(FBlocks_[k].cols());
yi += Fblocks_[k].second.transpose() * alpha * e2[k]; yi += FBlocks_[k].transpose() * alpha * e2[k];
} }
} }
@ -536,9 +414,9 @@ public:
void multiplyHessianDummy(double alpha, const VectorValues& x, void multiplyHessianDummy(double alpha, const VectorValues& x,
VectorValues& y) const { VectorValues& y) const {
BOOST_FOREACH(const KeyMatrix2D& Fi, Fblocks_) { for (size_t k = 0; k < size(); ++k) {
static const Vector empty; static const Vector empty;
Key key = Fi.first; Key key = keys_[k];
std::pair<VectorValues::iterator, bool> it = y.tryInsert(key, empty); std::pair<VectorValues::iterator, bool> it = y.tryInsert(key, empty);
Vector& yi = it.first->second; Vector& yi = it.first->second;
yi = x.at(key); yi = x.at(key);
@ -553,14 +431,14 @@ public:
e1.resize(size()); e1.resize(size());
e2.resize(size()); e2.resize(size());
for (size_t k = 0; k < size(); k++) for (size_t k = 0; k < size(); k++)
e1[k] = b_.segment<Z>(Z * k); e1[k] = b_.segment<ZDim>(ZDim * k);
projectError(e1, e2); projectError(e1, e2);
// g = F.transpose()*e2 // g = F.transpose()*e2
VectorValues g; VectorValues g;
for (size_t k = 0; k < size(); ++k) { for (size_t k = 0; k < size(); ++k) {
Key key = keys_[k]; Key key = keys_[k];
g.insert(key, -Fblocks_[k].second.transpose() * e2[k]); g.insert(key, -FBlocks_[k].transpose() * e2[k]);
} }
// return it // return it
@ -580,12 +458,12 @@ public:
e1.resize(size()); e1.resize(size());
e2.resize(size()); e2.resize(size());
for (size_t k = 0; k < size(); k++) for (size_t k = 0; k < size(); k++)
e1[k] = b_.segment<Z>(Z * k); e1[k] = b_.segment<ZDim>(ZDim * k);
projectError(e1, e2); projectError(e1, e2);
for (size_t k = 0; k < size(); ++k) { // for each camera in the factor for (size_t k = 0; k < size(); ++k) { // for each camera in the factor
Key j = keys_[k]; Key j = keys_[k];
DMap(d + D * j) += -Fblocks_[k].second.transpose() * e2[k]; DMap(d + D * j) += -FBlocks_[k].transpose() * e2[k];
} }
} }
@ -598,9 +476,15 @@ public:
}; };
// end class RegularImplicitSchurFactor // end class RegularImplicitSchurFactor
template<class CAMERA>
const int RegularImplicitSchurFactor<CAMERA>::D;
template<class CAMERA>
const int RegularImplicitSchurFactor<CAMERA>::ZDim;
// traits // traits
template<size_t Z, size_t D> struct traits<RegularImplicitSchurFactor<D, Z> > : public Testable< template<class CAMERA> struct traits<RegularImplicitSchurFactor<CAMERA> > : public Testable<
RegularImplicitSchurFactor<D, Z> > { RegularImplicitSchurFactor<CAMERA> > {
}; };
} }

57
gtsam/slam/SmartFactorBase.h
View File

@ -95,8 +95,7 @@ protected:
public: public:
// Definitions for blocks of F, externally visible // Definitions for blocks of F, externally visible
typedef Eigen::Matrix<double, ZDim, Dim> Matrix2D; // F typedef Eigen::Matrix<double, ZDim, Dim> MatrixZD; // F
typedef std::pair<Key, Matrix2D> KeyMatrix2D; // Fblocks
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
@ -283,29 +282,16 @@ public:
* F is a vector of derivatives wrpt the cameras, and E the stacked derivatives * F is a vector of derivatives wrpt the cameras, and E the stacked derivatives
* with respect to the point. The value of cameras/point are passed as parameters. * with respect to the point. The value of cameras/point are passed as parameters.
*/ */
double computeJacobians(std::vector<KeyMatrix2D>& Fblocks, Matrix& E, double computeJacobians(std::vector<MatrixZD>& Fblocks, Matrix& E, Vector& b,
Vector& b, const Cameras& cameras, const Point3& point) const { const Cameras& cameras, const Point3& point) const {
// Project into Camera set and calculate derivatives // Project into Camera set and calculate derivatives
typename Cameras::FBlocks F; typename Cameras::FBlocks F;
b = reprojectionError(cameras, point, F, E); b = reprojectionError(cameras, point, F, E);
return b.squaredNorm();
// Now calculate f and divide up the F derivatives into Fblocks
double f = 0.0;
size_t m = keys_.size();
for (size_t i = 0, row = 0; i < m; i++, row += ZDim) {
// Accumulate normalized error
f += b.segment<ZDim>(row).squaredNorm();
// Push piece of F onto Fblocks with associated key
Fblocks.push_back(KeyMatrix2D(keys_[i], F[i]));
}
return f;
} }
/// SVD version /// SVD version
double computeJacobiansSVD(std::vector<KeyMatrix2D>& Fblocks, Matrix& Enull, double computeJacobiansSVD(std::vector<MatrixZD>& Fblocks, Matrix& Enull,
Vector& b, const Cameras& cameras, const Point3& point) const { Vector& b, const Cameras& cameras, const Point3& point) const {
Matrix E; Matrix E;
@ -328,7 +314,7 @@ public:
bool diagonalDamping = false) const { bool diagonalDamping = false) const {
int m = this->keys_.size(); int m = this->keys_.size();
std::vector<KeyMatrix2D> Fblocks; std::vector<MatrixZD> Fblocks;
Matrix E; Matrix E;
Vector b; Vector b;
double f = computeJacobians(Fblocks, E, b, cameras, point); double f = computeJacobians(Fblocks, E, b, cameras, point);
@ -342,8 +328,7 @@ public:
SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1)); SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1));
// build augmented hessian // build augmented hessian
RegularImplicitSchurFactor<Dim>::SparseSchurComplement(Fblocks, E, P, b, CameraSet<CAMERA>::SchurComplement(Fblocks, E, P, b, augmentedHessian);
augmentedHessian);
augmentedHessian(m, m)(0, 0) = f; augmentedHessian(m, m)(0, 0) = f;
return boost::make_shared<RegularHessianFactor<Dim> >(keys_, return boost::make_shared<RegularHessianFactor<Dim> >(keys_,
@ -361,36 +346,35 @@ public:
const FastVector<Key> allKeys) const { const FastVector<Key> allKeys) const {
Matrix E; Matrix E;
Vector b; Vector b;
std::vector<KeyMatrix2D> Fblocks; std::vector<MatrixZD> Fblocks;
double f = computeJacobians(Fblocks, E, b, cameras, point); computeJacobians(Fblocks, E, b, cameras, point);
Matrix3 P = PointCov(E, lambda, diagonalDamping); Matrix3 P = PointCov(E, lambda, diagonalDamping);
RegularImplicitSchurFactor<Dim, ZDim>::UpdateSparseSchurComplement(Fblocks, CameraSet<CAMERA>::UpdateSchurComplement(Fblocks, E, P, b, allKeys, keys_,
E, P, b, f, allKeys, keys_, augmentedHessian); augmentedHessian);
} }
/// Whiten the Jacobians computed by computeJacobians using noiseModel_ /// Whiten the Jacobians computed by computeJacobians using noiseModel_
void whitenJacobians(std::vector<KeyMatrix2D>& F, Matrix& E, void whitenJacobians(std::vector<MatrixZD>& F, Matrix& E, Vector& b) const {
Vector& b) const {
noiseModel_->WhitenSystem(E, b); noiseModel_->WhitenSystem(E, b);
// TODO make WhitenInPlace work with any dense matrix type // TODO make WhitenInPlace work with any dense matrix type
BOOST_FOREACH(KeyMatrix2D& Fblock,F) BOOST_FOREACH(MatrixZD& Fblock,F)
Fblock.second = noiseModel_->Whiten(Fblock.second); Fblock.second = noiseModel_->Whiten(Fblock.second);
} }
/** /**
* Return Jacobians as RegularImplicitSchurFactor with raw access * Return Jacobians as RegularImplicitSchurFactor with raw access
*/ */
boost::shared_ptr<RegularImplicitSchurFactor<Dim, ZDim> > // boost::shared_ptr<RegularImplicitSchurFactor<CAMERA> > //
createRegularImplicitSchurFactor(const Cameras& cameras, const Point3& point, createRegularImplicitSchurFactor(const Cameras& cameras, const Point3& point,
double lambda = 0.0, bool diagonalDamping = false) const { double lambda = 0.0, bool diagonalDamping = false) const {
Matrix E; Matrix E;
Vector b; Vector b;
std::vector<KeyMatrix2D> F; std::vector<MatrixZD> F;
computeJacobians(F, E, b, cameras, point); computeJacobians(F, E, b, cameras, point);
whitenJacobians(F, E, b); whitenJacobians(F, E, b);
Matrix3 P = PointCov(E, lambda, diagonalDamping); Matrix3 P = PointCov(E, lambda, diagonalDamping);
return boost::make_shared<RegularImplicitSchurFactor<Dim, ZDim> >(F, E, P, return boost::make_shared<RegularImplicitSchurFactor<CAMERA> >(keys_, F, E,
b); P, b);
} }
/** /**
@ -401,7 +385,7 @@ public:
bool diagonalDamping = false) const { bool diagonalDamping = false) const {
Matrix E; Matrix E;
Vector b; Vector b;
std::vector<KeyMatrix2D> F; std::vector<MatrixZD> F;
computeJacobians(F, E, b, cameras, point); computeJacobians(F, E, b, cameras, point);
const size_t M = b.size(); const size_t M = b.size();
Matrix3 P = PointCov(E, lambda, diagonalDamping); Matrix3 P = PointCov(E, lambda, diagonalDamping);
@ -416,7 +400,7 @@ public:
boost::shared_ptr<JacobianFactor> createJacobianSVDFactor( boost::shared_ptr<JacobianFactor> createJacobianSVDFactor(
const Cameras& cameras, const Point3& point, double lambda = 0.0) const { const Cameras& cameras, const Point3& point, double lambda = 0.0) const {
size_t m = this->keys_.size(); size_t m = this->keys_.size();
std::vector<KeyMatrix2D> F; std::vector<MatrixZD> F;
Vector b; Vector b;
const size_t M = ZDim * m; const size_t M = ZDim * m;
Matrix E0(M, M - 3); Matrix E0(M, M - 3);
@ -427,8 +411,7 @@ public:
} }
/// Create BIG block-diagonal matrix F from Fblocks /// Create BIG block-diagonal matrix F from Fblocks
static void FillDiagonalF(const std::vector<KeyMatrix2D>& Fblocks, static void FillDiagonalF(const std::vector<MatrixZD>& Fblocks, Matrix& F) {
Matrix& F) {
size_t m = Fblocks.size(); size_t m = Fblocks.size();
F.resize(ZDim * m, Dim * m); F.resize(ZDim * m, Dim * m);
F.setZero(); F.setZero();

28
gtsam/slam/tests/testRegularImplicitSchurFactor.cpp
View File

@ -19,11 +19,13 @@
#include <gtsam/slam/JacobianFactorQ.h> #include <gtsam/slam/JacobianFactorQ.h>
#include <gtsam/slam/JacobianFactorQR.h> #include <gtsam/slam/JacobianFactorQR.h>
#include <gtsam/slam/RegularImplicitSchurFactor.h> #include <gtsam/slam/RegularImplicitSchurFactor.h>
#include <gtsam/geometry/CalibratedCamera.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/base/timing.h>
#include <gtsam/linear/VectorValues.h> #include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/NoiseModel.h> #include <gtsam/linear/NoiseModel.h>
#include <gtsam/linear/GaussianFactor.h> #include <gtsam/linear/GaussianFactor.h>
#include <gtsam/base/timing.h>
#include <boost/assign/list_of.hpp> #include <boost/assign/list_of.hpp>
#include <boost/assign/std/vector.hpp> #include <boost/assign/std/vector.hpp>
@ -39,8 +41,8 @@ using namespace gtsam;
const Matrix26 F0 = Matrix26::Ones(); const Matrix26 F0 = Matrix26::Ones();
const Matrix26 F1 = 2 * Matrix26::Ones(); const Matrix26 F1 = 2 * Matrix26::Ones();
const Matrix26 F3 = 3 * Matrix26::Ones(); const Matrix26 F3 = 3 * Matrix26::Ones();
const vector<pair<Key, Matrix26> > Fblocks = list_of<pair<Key, Matrix> > // const vector<Matrix26> FBlocks = list_of<Matrix26>(F0)(F1)(F3);
(make_pair(0, F0))(make_pair(1, F1))(make_pair(3, F3)); const FastVector<Key> keys = list_of<Key>(0)(1)(3);
// RHS and sigmas // RHS and sigmas
const Vector b = (Vector(6) << 1., 2., 3., 4., 5., 6.).finished(); const Vector b = (Vector(6) << 1., 2., 3., 4., 5., 6.).finished();
@ -51,7 +53,7 @@ TEST( regularImplicitSchurFactor, creation ) {
E.block<2,2>(0, 0) = eye(2); E.block<2,2>(0, 0) = eye(2);
E.block<2,3>(2, 0) = 2 * ones(2, 3); E.block<2,3>(2, 0) = 2 * ones(2, 3);
Matrix3 P = (E.transpose() * E).inverse(); Matrix3 P = (E.transpose() * E).inverse();
RegularImplicitSchurFactor<6> expected(Fblocks, E, P, b); RegularImplicitSchurFactor<CalibratedCamera> expected(keys, FBlocks, E, P, b);
Matrix expectedP = expected.getPointCovariance(); Matrix expectedP = expected.getPointCovariance();
EXPECT(assert_equal(expectedP, P)); EXPECT(assert_equal(expectedP, P));
} }
@ -84,15 +86,15 @@ TEST( regularImplicitSchurFactor, addHessianMultiply ) {
F << F0, zeros(2, d * 3), zeros(2, d), F1, zeros(2, d*2), zeros(2, d * 3), F3; F << F0, zeros(2, d * 3), zeros(2, d), F1, zeros(2, d*2), zeros(2, d * 3), F3;
// Calculate expected result F'*alpha*(I - E*P*E')*F*x // Calculate expected result F'*alpha*(I - E*P*E')*F*x
FastVector<Key> keys; FastVector<Key> keys2;
keys += 0,1,2,3; keys2 += 0,1,2,3;
Vector x = xvalues.vector(keys); Vector x = xvalues.vector(keys2);
Vector expected = zero(24); Vector expected = zero(24);
RegularImplicitSchurFactor<6>::multiplyHessianAdd(F, E, P, alpha, x, expected); RegularImplicitSchurFactor<CalibratedCamera>::multiplyHessianAdd(F, E, P, alpha, x, expected);
EXPECT(assert_equal(expected, yExpected.vector(keys), 1e-8)); EXPECT(assert_equal(expected, yExpected.vector(keys2), 1e-8));
// Create ImplicitSchurFactor // Create ImplicitSchurFactor
RegularImplicitSchurFactor<6> implicitFactor(Fblocks, E, P, b); RegularImplicitSchurFactor<CalibratedCamera> implicitFactor(keys, FBlocks, E, P, b);
VectorValues zero = 0 * yExpected;// quick way to get zero w right structure VectorValues zero = 0 * yExpected;// quick way to get zero w right structure
{ // First Version { // First Version
@ -122,7 +124,7 @@ TEST( regularImplicitSchurFactor, addHessianMultiply ) {
// Create JacobianFactor with same error // Create JacobianFactor with same error
const SharedDiagonal model; const SharedDiagonal model;
JacobianFactorQ<6, 2> jf(Fblocks, E, P, b, model); JacobianFactorQ<6, 2> jf(keys, FBlocks, E, P, b, model);
{ // error { // error
double expectedError = jf.error(xvalues); double expectedError = jf.error(xvalues);
@ -172,7 +174,7 @@ TEST( regularImplicitSchurFactor, addHessianMultiply ) {
} }
// Create JacobianFactorQR // Create JacobianFactorQR
JacobianFactorQR<6, 2> jfq(Fblocks, E, P, b, model); JacobianFactorQR<6, 2> jfq(keys, FBlocks, E, P, b, model);
{ {
const SharedDiagonal model; const SharedDiagonal model;
VectorValues yActual = zero; VectorValues yActual = zero;
@ -214,7 +216,7 @@ TEST(regularImplicitSchurFactor, hessianDiagonal)
E.block<2,3>(2, 0) << 1,2,3,4,5,6; E.block<2,3>(2, 0) << 1,2,3,4,5,6;
E.block<2,3>(4, 0) << 0.5,1,2,3,4,5; E.block<2,3>(4, 0) << 0.5,1,2,3,4,5;
Matrix3 P = (E.transpose() * E).inverse(); Matrix3 P = (E.transpose() * E).inverse();
RegularImplicitSchurFactor<6> factor(Fblocks, E, P, b); RegularImplicitSchurFactor<CalibratedCamera> factor(keys, FBlocks, E, P, b);
// hessianDiagonal // hessianDiagonal
VectorValues expected; VectorValues expected;