Switched to non-keyed Fblocks
dellaert
parent
0a287e25e7
commit
175dae30ec
|
|
@ -28,7 +28,7 @@ class JacobianFactorQ: public RegularJacobianFactor<D> {
|
|||
|
||||
typedef RegularJacobianFactor<D> Base;
|
||||
typedef Eigen::Matrix<double, ZDim, D> MatrixZD;
|
||||
typedef std::pair<Key, MatrixZD> KeyMatrixZD;
|
||||
typedef std::pair<Key, Matrix> KeyMatrix;
|
||||
|
||||
public:
|
||||
|
||||
|
|
@ -42,7 +42,6 @@ public:
|
|||
Base() {
|
||||
Matrix zeroMatrix = Matrix::Zero(0, D);
|
||||
Vector zeroVector = Vector::Zero(0);
|
||||
typedef std::pair<Key, Matrix> KeyMatrix;
|
||||
std::vector<KeyMatrix> QF;
|
||||
QF.reserve(keys.size());
|
||||
BOOST_FOREACH(const Key& key, keys)
|
||||
|
|
@ -51,22 +50,23 @@ public:
|
|||
}
|
||||
|
||||
/// Constructor
|
||||
JacobianFactorQ(const std::vector<KeyMatrixZD>& Fblocks, const Matrix& E,
|
||||
const Matrix3& P, const Vector& b, const SharedDiagonal& model =
|
||||
SharedDiagonal()) :
|
||||
JacobianFactorQ(const FastVector<Key>& keys,
|
||||
const std::vector<MatrixZD>& FBlocks, const Matrix& E, const Matrix3& P,
|
||||
const Vector& b, const SharedDiagonal& model = SharedDiagonal()) :
|
||||
Base() {
|
||||
size_t j = 0, m2 = E.rows(), m = m2 / ZDim;
|
||||
// Calculate projector Q
|
||||
Matrix Q = eye(m2) - E * P * E.transpose();
|
||||
// Calculate pre-computed Jacobian matrices
|
||||
// TODO: can we do better ?
|
||||
typedef std::pair<Key, Matrix> KeyMatrix;
|
||||
std::vector<KeyMatrix> QF;
|
||||
QF.reserve(m);
|
||||
// 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(
|
||||
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
|
||||
JacobianFactor::fillTerms(QF, Q * b, model);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -7,8 +7,8 @@
|
|||
|
||||
#pragma once
|
||||
#include <gtsam/linear/GaussianFactorGraph.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam/linear/RegularJacobianFactor.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -22,25 +22,24 @@ class JacobianFactorQR: public RegularJacobianFactor<D> {
|
|||
|
||||
typedef RegularJacobianFactor<D> Base;
|
||||
typedef Eigen::Matrix<double, ZDim, D> MatrixZD;
|
||||
typedef std::pair<Key, MatrixZD> KeyMatrixZD;
|
||||
|
||||
public:
|
||||
|
||||
/**
|
||||
* Constructor
|
||||
*/
|
||||
JacobianFactorQR(const std::vector<KeyMatrixZD>& Fblocks, const Matrix& E,
|
||||
const Matrix3& P, const Vector& b, //
|
||||
JacobianFactorQR(const FastVector<Key>& keys,
|
||||
const std::vector<MatrixZD>& FBlocks, const Matrix& E, const Matrix3& P,
|
||||
const Vector& b, //
|
||||
const SharedDiagonal& model = SharedDiagonal()) :
|
||||
Base() {
|
||||
// Create a number of Jacobian factors in a factor graph
|
||||
GaussianFactorGraph gfg;
|
||||
Symbol pointKey('p', 0);
|
||||
size_t i = 0;
|
||||
BOOST_FOREACH(const KeyMatrixZD& it, Fblocks) {
|
||||
gfg.add(pointKey, E.block<ZDim, 3>(ZDim * i, 0), it.first, it.second,
|
||||
b.segment<ZDim>(ZDim * i), model);
|
||||
i += 1;
|
||||
for (size_t k = 0; k < FBlocks.size(); ++k) {
|
||||
Key key = keys[k];
|
||||
gfg.add(pointKey, E.block<ZDim, 3>(ZDim * k, 0), key, FBlocks[k],
|
||||
b.segment < ZDim > (ZDim * k), model);
|
||||
}
|
||||
//gfg.print("gfg");
|
||||
|
||||
|
|
|
|||
|
|
@ -7,9 +7,9 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/geometry/CameraSet.h>
|
||||
#include <gtsam/linear/JacobianFactor.h>
|
||||
#include <gtsam/linear/VectorValues.h>
|
||||
#include <gtsam/base/SymmetricBlockMatrix.h>
|
||||
#include <boost/foreach.hpp>
|
||||
#include <iosfwd>
|
||||
|
||||
|
|
@ -18,7 +18,7 @@ namespace gtsam {
|
|||
/**
|
||||
* RegularImplicitSchurFactor
|
||||
*/
|
||||
template<size_t D, size_t Z = 2> //
|
||||
template<class CAMERA>
|
||||
class RegularImplicitSchurFactor: public GaussianFactor {
|
||||
|
||||
public:
|
||||
|
|
@ -27,22 +27,21 @@ public:
|
|||
|
||||
protected:
|
||||
|
||||
typedef Eigen::Matrix<double, Z, D> Matrix2D; ///< type of an F block
|
||||
typedef Eigen::Matrix<double, D, D> MatrixDD; ///< camera hessian
|
||||
typedef std::pair<Key, Matrix2D> KeyMatrix2D; ///< named F block
|
||||
// This factor is closely related to a CameraSet
|
||||
typedef CameraSet<CAMERA> Set;
|
||||
|
||||
const std::vector<KeyMatrix2D> Fblocks_; ///< All Z*D F blocks (one for each camera)
|
||||
const Matrix3 PointCovariance_; ///< the 3*3 matrix P = inv(E'E) (Z*Z if degenerate)
|
||||
typedef typename CAMERA::Measurement Z;
|
||||
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 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:
|
||||
|
||||
/// Constructor
|
||||
|
|
@ -50,29 +49,29 @@ public:
|
|||
}
|
||||
|
||||
/// Construct from blocks of F, E, inv(E'*E), and RHS vector b
|
||||
RegularImplicitSchurFactor(const std::vector<KeyMatrix2D>& Fblocks,
|
||||
const Matrix& E, const Matrix3& P, const Vector& b) :
|
||||
Fblocks_(Fblocks), PointCovariance_(P), E_(E), b_(b) {
|
||||
initKeys();
|
||||
RegularImplicitSchurFactor(const FastVector<Key>& keys,
|
||||
const std::vector<MatrixZD>& FBlocks, const Matrix& E, const Matrix3& P,
|
||||
const Vector& b) :
|
||||
GaussianFactor(keys), FBlocks_(FBlocks), PointCovariance_(P), E_(E), b_(b) {
|
||||
}
|
||||
|
||||
/// Destructor
|
||||
virtual ~RegularImplicitSchurFactor() {
|
||||
}
|
||||
|
||||
inline std::vector<KeyMatrix2D>& Fblocks() const {
|
||||
return Fblocks_;
|
||||
std::vector<MatrixZD>& FBlocks() const {
|
||||
return FBlocks_;
|
||||
}
|
||||
|
||||
inline const Matrix& E() const {
|
||||
const Matrix& E() const {
|
||||
return E_;
|
||||
}
|
||||
|
||||
inline const Vector& b() const {
|
||||
const Vector& b() const {
|
||||
return b_;
|
||||
}
|
||||
|
||||
inline const Matrix3& getPointCovariance() const {
|
||||
const Matrix3& getPointCovariance() const {
|
||||
return PointCovariance_;
|
||||
}
|
||||
|
||||
|
|
@ -82,7 +81,7 @@ public:
|
|||
std::cout << " RegularImplicitSchurFactor " << std::endl;
|
||||
Factor::print(s);
|
||||
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 << "E:\n" << E_ << std::endl;
|
||||
|
|
@ -94,13 +93,12 @@ public:
|
|||
const This* f = dynamic_cast<const This*>(&lf);
|
||||
if (!f)
|
||||
return false;
|
||||
for (size_t pos = 0; pos < size(); ++pos) {
|
||||
if (keys_[pos] != f->keys_[pos])
|
||||
for (size_t k = 0; k < FBlocks_.size(); ++k) {
|
||||
if (keys_[k] != f->keys_[k])
|
||||
return false;
|
||||
if (Fblocks_[pos].first != f->Fblocks_[pos].first)
|
||||
if (FBlocks_[k] != f->FBlocks_[k])
|
||||
return false;
|
||||
if (!equal_with_abs_tol(Fblocks_[pos].second, f->Fblocks_[pos].second,
|
||||
tol))
|
||||
if (!equal_with_abs_tol(FBlocks_[k], f->FBlocks_[k], tol))
|
||||
return false;
|
||||
}
|
||||
return equal_with_abs_tol(PointCovariance_, f->PointCovariance_, tol)
|
||||
|
|
@ -124,131 +122,12 @@ public:
|
|||
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
|
||||
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
|
||||
SparseSchurComplement(Fblocks_, E_, PointCovariance_, b_, augmentedHessian);
|
||||
SymmetricBlockMatrix augmentedHessian = Set::SchurComplement(FBlocks_, E_,
|
||||
PointCovariance_, b_);
|
||||
return augmentedHessian.matrix();
|
||||
}
|
||||
|
||||
|
|
@ -265,14 +144,14 @@ public:
|
|||
// diag(Hessian) = diag(F' * (I - E * PointCov * E') * F);
|
||||
VectorValues d;
|
||||
|
||||
for (size_t pos = 0; pos < size(); ++pos) { // for each camera
|
||||
Key j = keys_[pos];
|
||||
for (size_t k = 0; k < size(); ++k) { // for each camera
|
||||
Key j = keys_[k];
|
||||
|
||||
// Calculate Fj'*Ej for the current camera (observing a single point)
|
||||
// D x 3 = (D x Z) * (Z x 3)
|
||||
const Matrix2D& Fj = Fblocks_[pos].second;
|
||||
// D x 3 = (D x ZDim) * (ZDim x 3)
|
||||
const MatrixZD& Fj = FBlocks_[k];
|
||||
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;
|
||||
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];
|
||||
|
||||
// Calculate Fj'*Ej for the current camera (observing a single point)
|
||||
// D x 3 = (D x Z) * (Z x 3)
|
||||
const Matrix2D& Fj = Fblocks_[pos].second;
|
||||
// D x 3 = (D x ZDim) * (ZDim x 3)
|
||||
const MatrixZD& Fj = FBlocks_[pos];
|
||||
Eigen::Matrix<double, D, 3> FtE = Fj.transpose()
|
||||
* E_.block<Z, 3>(Z * pos, 0);
|
||||
* E_.block<ZDim, 3>(ZDim * pos, 0);
|
||||
|
||||
DVector dj;
|
||||
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) {
|
||||
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)
|
||||
const Matrix2D& Fj = Fblocks_[pos].second;
|
||||
const MatrixZD& Fj = FBlocks_[pos];
|
||||
// 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
|
||||
// - 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()
|
||||
* (Fj - Ej * PointCovariance_ * Ej.transpose() * Fj);
|
||||
|
||||
// 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 = //
|
||||
// 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;
|
||||
}
|
||||
return blocks;
|
||||
}
|
||||
|
||||
virtual GaussianFactor::shared_ptr clone() const {
|
||||
return boost::make_shared<RegularImplicitSchurFactor<D, Z> >(Fblocks_,
|
||||
PointCovariance_, E_, b_);
|
||||
return boost::make_shared<RegularImplicitSchurFactor<CAMERA> >(keys_,
|
||||
FBlocks_, PointCovariance_, E_, b_);
|
||||
throw std::runtime_error(
|
||||
"RegularImplicitSchurFactor::clone non implemented");
|
||||
}
|
||||
|
|
@ -353,8 +232,8 @@ public:
|
|||
}
|
||||
|
||||
virtual GaussianFactor::shared_ptr negate() const {
|
||||
return boost::make_shared<RegularImplicitSchurFactor<D, Z> >(Fblocks_,
|
||||
PointCovariance_, E_, b_);
|
||||
return boost::make_shared<RegularImplicitSchurFactor<CAMERA> >(keys_,
|
||||
FBlocks_, PointCovariance_, E_, b_);
|
||||
throw std::runtime_error(
|
||||
"RegularImplicitSchurFactor::negate non implemented");
|
||||
}
|
||||
|
|
@ -374,23 +253,24 @@ public:
|
|||
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 {
|
||||
|
||||
// d1 = E.transpose() * (e1-Z*b) = (3*2m)*2m
|
||||
// d1 = E.transpose() * (e1-ZDim*b) = (3*2m)*2m
|
||||
Vector3 d1;
|
||||
d1.setZero();
|
||||
for (size_t k = 0; k < size(); k++)
|
||||
d1 += E_.block<Z, 3>(Z * k, 0).transpose()
|
||||
* (e1[k] - Z * b_.segment<Z>(k * Z));
|
||||
d1 += E_.block<ZDim, 3>(ZDim * k, 0).transpose()
|
||||
* (e1[k] - ZDim * b_.segment<ZDim>(k * ZDim));
|
||||
|
||||
// d2 = E.transpose() * e1 = (3*2m)*2m
|
||||
Vector3 d2 = PointCovariance_ * d1;
|
||||
|
||||
// e3 = alpha*(e1 - E*d2) = 1*[2m-(2m*3)*3]
|
||||
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
|
||||
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);
|
||||
|
||||
double result = 0;
|
||||
|
|
@ -432,7 +312,7 @@ public:
|
|||
|
||||
// e1 = F * x - b = (2m*dm)*dm
|
||||
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);
|
||||
|
||||
double result = 0;
|
||||
|
|
@ -451,14 +331,14 @@ public:
|
|||
Vector3 d1;
|
||||
d1.setZero();
|
||||
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
|
||||
Vector3 d2 = PointCovariance_ * d1;
|
||||
|
||||
// e3 = alpha*(e1 - E*d2) = 1*[2m-(2m*3)*3]
|
||||
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
|
||||
|
|
@ -480,19 +360,17 @@ public:
|
|||
e2.resize(size());
|
||||
|
||||
// e1 = F * x = (2m*dm)*dm
|
||||
size_t k = 0;
|
||||
BOOST_FOREACH(const KeyMatrix2D& it, Fblocks_) {
|
||||
Key key = it.first;
|
||||
e1[k++] = it.second * ConstDMap(x + D * key);
|
||||
for (size_t k = 0; k < size(); ++k) {
|
||||
Key key = keys_[k];
|
||||
e1[k] = FBlocks_[k] * ConstDMap(x + D * key);
|
||||
}
|
||||
|
||||
projectError(e1, e2);
|
||||
|
||||
// y += F.transpose()*e2 = (2d*2m)*2m
|
||||
k = 0;
|
||||
BOOST_FOREACH(const KeyMatrix2D& it, Fblocks_) {
|
||||
Key key = it.first;
|
||||
DMap(y + D * key) += it.second.transpose() * alpha * e2[k++];
|
||||
for (size_t k = 0; k < size(); ++k) {
|
||||
Key key = keys_[k];
|
||||
DMap(y + D * key) += FBlocks_[k].transpose() * alpha * e2[k];
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -513,7 +391,7 @@ public:
|
|||
|
||||
// e1 = F * x = (2m*dm)*dm
|
||||
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);
|
||||
|
||||
|
|
@ -525,8 +403,8 @@ public:
|
|||
Vector& yi = it.first->second;
|
||||
// Create the value as a zero vector if it does not exist.
|
||||
if (it.second)
|
||||
yi = Vector::Zero(Fblocks_[k].second.cols());
|
||||
yi += Fblocks_[k].second.transpose() * alpha * e2[k];
|
||||
yi = Vector::Zero(FBlocks_[k].cols());
|
||||
yi += FBlocks_[k].transpose() * alpha * e2[k];
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -536,9 +414,9 @@ public:
|
|||
void multiplyHessianDummy(double alpha, const VectorValues& x,
|
||||
VectorValues& y) const {
|
||||
|
||||
BOOST_FOREACH(const KeyMatrix2D& Fi, Fblocks_) {
|
||||
for (size_t k = 0; k < size(); ++k) {
|
||||
static const Vector empty;
|
||||
Key key = Fi.first;
|
||||
Key key = keys_[k];
|
||||
std::pair<VectorValues::iterator, bool> it = y.tryInsert(key, empty);
|
||||
Vector& yi = it.first->second;
|
||||
yi = x.at(key);
|
||||
|
|
@ -553,14 +431,14 @@ public:
|
|||
e1.resize(size());
|
||||
e2.resize(size());
|
||||
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);
|
||||
|
||||
// g = F.transpose()*e2
|
||||
VectorValues g;
|
||||
for (size_t k = 0; k < size(); ++k) {
|
||||
Key key = keys_[k];
|
||||
g.insert(key, -Fblocks_[k].second.transpose() * e2[k]);
|
||||
g.insert(key, -FBlocks_[k].transpose() * e2[k]);
|
||||
}
|
||||
|
||||
// return it
|
||||
|
|
@ -580,12 +458,12 @@ public:
|
|||
e1.resize(size());
|
||||
e2.resize(size());
|
||||
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);
|
||||
|
||||
for (size_t k = 0; k < size(); ++k) { // for each camera in the factor
|
||||
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
|
||||
|
||||
template<class CAMERA>
|
||||
const int RegularImplicitSchurFactor<CAMERA>::D;
|
||||
|
||||
template<class CAMERA>
|
||||
const int RegularImplicitSchurFactor<CAMERA>::ZDim;
|
||||
|
||||
// traits
|
||||
template<size_t Z, size_t D> struct traits<RegularImplicitSchurFactor<D, Z> > : public Testable<
|
||||
RegularImplicitSchurFactor<D, Z> > {
|
||||
template<class CAMERA> struct traits<RegularImplicitSchurFactor<CAMERA> > : public Testable<
|
||||
RegularImplicitSchurFactor<CAMERA> > {
|
||||
};
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -95,8 +95,7 @@ protected:
|
|||
public:
|
||||
|
||||
// Definitions for blocks of F, externally visible
|
||||
typedef Eigen::Matrix<double, ZDim, Dim> Matrix2D; // F
|
||||
typedef std::pair<Key, Matrix2D> KeyMatrix2D; // Fblocks
|
||||
typedef Eigen::Matrix<double, ZDim, Dim> MatrixZD; // F
|
||||
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
|
||||
|
|
@ -283,29 +282,16 @@ public:
|
|||
* 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.
|
||||
*/
|
||||
double computeJacobians(std::vector<KeyMatrix2D>& Fblocks, Matrix& E,
|
||||
Vector& b, const Cameras& cameras, const Point3& point) const {
|
||||
|
||||
double computeJacobians(std::vector<MatrixZD>& Fblocks, Matrix& E, Vector& b,
|
||||
const Cameras& cameras, const Point3& point) const {
|
||||
// Project into Camera set and calculate derivatives
|
||||
typename Cameras::FBlocks F;
|
||||
b = reprojectionError(cameras, point, F, E);
|
||||
|
||||
// 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;
|
||||
return b.squaredNorm();
|
||||
}
|
||||
|
||||
/// 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 {
|
||||
|
||||
Matrix E;
|
||||
|
|
@ -328,7 +314,7 @@ public:
|
|||
bool diagonalDamping = false) const {
|
||||
|
||||
int m = this->keys_.size();
|
||||
std::vector<KeyMatrix2D> Fblocks;
|
||||
std::vector<MatrixZD> Fblocks;
|
||||
Matrix E;
|
||||
Vector b;
|
||||
double f = computeJacobians(Fblocks, E, b, cameras, point);
|
||||
|
|
@ -342,8 +328,7 @@ public:
|
|||
SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1));
|
||||
|
||||
// build augmented hessian
|
||||
RegularImplicitSchurFactor<Dim>::SparseSchurComplement(Fblocks, E, P, b,
|
||||
augmentedHessian);
|
||||
CameraSet<CAMERA>::SchurComplement(Fblocks, E, P, b, augmentedHessian);
|
||||
augmentedHessian(m, m)(0, 0) = f;
|
||||
|
||||
return boost::make_shared<RegularHessianFactor<Dim> >(keys_,
|
||||
|
|
@ -361,36 +346,35 @@ public:
|
|||
const FastVector<Key> allKeys) const {
|
||||
Matrix E;
|
||||
Vector b;
|
||||
std::vector<KeyMatrix2D> Fblocks;
|
||||
double f = computeJacobians(Fblocks, E, b, cameras, point);
|
||||
std::vector<MatrixZD> Fblocks;
|
||||
computeJacobians(Fblocks, E, b, cameras, point);
|
||||
Matrix3 P = PointCov(E, lambda, diagonalDamping);
|
||||
RegularImplicitSchurFactor<Dim, ZDim>::UpdateSparseSchurComplement(Fblocks,
|
||||
E, P, b, f, allKeys, keys_, augmentedHessian);
|
||||
CameraSet<CAMERA>::UpdateSchurComplement(Fblocks, E, P, b, allKeys, keys_,
|
||||
augmentedHessian);
|
||||
}
|
||||
|
||||
/// Whiten the Jacobians computed by computeJacobians using noiseModel_
|
||||
void whitenJacobians(std::vector<KeyMatrix2D>& F, Matrix& E,
|
||||
Vector& b) const {
|
||||
void whitenJacobians(std::vector<MatrixZD>& F, Matrix& E, Vector& b) const {
|
||||
noiseModel_->WhitenSystem(E, b);
|
||||
// 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);
|
||||
}
|
||||
|
||||
/**
|
||||
* 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,
|
||||
double lambda = 0.0, bool diagonalDamping = false) const {
|
||||
Matrix E;
|
||||
Vector b;
|
||||
std::vector<KeyMatrix2D> F;
|
||||
std::vector<MatrixZD> F;
|
||||
computeJacobians(F, E, b, cameras, point);
|
||||
whitenJacobians(F, E, b);
|
||||
Matrix3 P = PointCov(E, lambda, diagonalDamping);
|
||||
return boost::make_shared<RegularImplicitSchurFactor<Dim, ZDim> >(F, E, P,
|
||||
b);
|
||||
return boost::make_shared<RegularImplicitSchurFactor<CAMERA> >(keys_, F, E,
|
||||
P, b);
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -401,7 +385,7 @@ public:
|
|||
bool diagonalDamping = false) const {
|
||||
Matrix E;
|
||||
Vector b;
|
||||
std::vector<KeyMatrix2D> F;
|
||||
std::vector<MatrixZD> F;
|
||||
computeJacobians(F, E, b, cameras, point);
|
||||
const size_t M = b.size();
|
||||
Matrix3 P = PointCov(E, lambda, diagonalDamping);
|
||||
|
|
@ -416,7 +400,7 @@ public:
|
|||
boost::shared_ptr<JacobianFactor> createJacobianSVDFactor(
|
||||
const Cameras& cameras, const Point3& point, double lambda = 0.0) const {
|
||||
size_t m = this->keys_.size();
|
||||
std::vector<KeyMatrix2D> F;
|
||||
std::vector<MatrixZD> F;
|
||||
Vector b;
|
||||
const size_t M = ZDim * m;
|
||||
Matrix E0(M, M - 3);
|
||||
|
|
@ -427,8 +411,7 @@ public:
|
|||
}
|
||||
|
||||
/// Create BIG block-diagonal matrix F from Fblocks
|
||||
static void FillDiagonalF(const std::vector<KeyMatrix2D>& Fblocks,
|
||||
Matrix& F) {
|
||||
static void FillDiagonalF(const std::vector<MatrixZD>& Fblocks, Matrix& F) {
|
||||
size_t m = Fblocks.size();
|
||||
F.resize(ZDim * m, Dim * m);
|
||||
F.setZero();
|
||||
|
|
|
|||
|
|
@ -19,11 +19,13 @@
|
|||
#include <gtsam/slam/JacobianFactorQ.h>
|
||||
#include <gtsam/slam/JacobianFactorQR.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/NoiseModel.h>
|
||||
#include <gtsam/linear/GaussianFactor.h>
|
||||
#include <gtsam/base/timing.h>
|
||||
|
||||
#include <boost/assign/list_of.hpp>
|
||||
#include <boost/assign/std/vector.hpp>
|
||||
|
|
@ -39,8 +41,8 @@ using namespace gtsam;
|
|||
const Matrix26 F0 = Matrix26::Ones();
|
||||
const Matrix26 F1 = 2 * Matrix26::Ones();
|
||||
const Matrix26 F3 = 3 * Matrix26::Ones();
|
||||
const vector<pair<Key, Matrix26> > Fblocks = list_of<pair<Key, Matrix> > //
|
||||
(make_pair(0, F0))(make_pair(1, F1))(make_pair(3, F3));
|
||||
const vector<Matrix26> FBlocks = list_of<Matrix26>(F0)(F1)(F3);
|
||||
const FastVector<Key> keys = list_of<Key>(0)(1)(3);
|
||||
// RHS and sigmas
|
||||
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,3>(2, 0) = 2 * ones(2, 3);
|
||||
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();
|
||||
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;
|
||||
|
||||
// Calculate expected result F'*alpha*(I - E*P*E')*F*x
|
||||
FastVector<Key> keys;
|
||||
keys += 0,1,2,3;
|
||||
Vector x = xvalues.vector(keys);
|
||||
FastVector<Key> keys2;
|
||||
keys2 += 0,1,2,3;
|
||||
Vector x = xvalues.vector(keys2);
|
||||
Vector expected = zero(24);
|
||||
RegularImplicitSchurFactor<6>::multiplyHessianAdd(F, E, P, alpha, x, expected);
|
||||
EXPECT(assert_equal(expected, yExpected.vector(keys), 1e-8));
|
||||
RegularImplicitSchurFactor<CalibratedCamera>::multiplyHessianAdd(F, E, P, alpha, x, expected);
|
||||
EXPECT(assert_equal(expected, yExpected.vector(keys2), 1e-8));
|
||||
|
||||
// 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
|
||||
{ // First Version
|
||||
|
|
@ -122,7 +124,7 @@ TEST( regularImplicitSchurFactor, addHessianMultiply ) {
|
|||
|
||||
// Create JacobianFactor with same error
|
||||
const SharedDiagonal model;
|
||||
JacobianFactorQ<6, 2> jf(Fblocks, E, P, b, model);
|
||||
JacobianFactorQ<6, 2> jf(keys, FBlocks, E, P, b, model);
|
||||
|
||||
{ // error
|
||||
double expectedError = jf.error(xvalues);
|
||||
|
|
@ -172,7 +174,7 @@ TEST( regularImplicitSchurFactor, addHessianMultiply ) {
|
|||
}
|
||||
|
||||
// Create JacobianFactorQR
|
||||
JacobianFactorQR<6, 2> jfq(Fblocks, E, P, b, model);
|
||||
JacobianFactorQR<6, 2> jfq(keys, FBlocks, E, P, b, model);
|
||||
{
|
||||
const SharedDiagonal model;
|
||||
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>(4, 0) << 0.5,1,2,3,4,5;
|
||||
Matrix3 P = (E.transpose() * E).inverse();
|
||||
RegularImplicitSchurFactor<6> factor(Fblocks, E, P, b);
|
||||
RegularImplicitSchurFactor<CalibratedCamera> factor(keys, FBlocks, E, P, b);
|
||||
|
||||
// hessianDiagonal
|
||||
VectorValues expected;
|
||||
|
|
|
|||
Loading…
Reference in New Issue