last updates from smartFactors project (these files are now removed from that project)
Luca
parent
02fc860d9e
commit
310fce3be9
|
|
@ -1,7 +1,8 @@
|
|||
/**
|
||||
* @file ImplicitSchurFactor.h
|
||||
* @brief A new type of linear factor (GaussianFactor), which is subclass of JacobiaFactor
|
||||
* @brief A new type of linear factor (GaussianFactor), which is subclass of GaussianFactor
|
||||
* @author Frank Dellaert
|
||||
* @author Luca Carlone
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
|
|
|||
|
|
@ -0,0 +1,53 @@
|
|||
/*
|
||||
* @file JacobianFactorQR.h
|
||||
* @brief Jacobianfactor that combines and eliminates points
|
||||
* @date Oct 27, 2013
|
||||
* @uthor Frank Dellaert
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
#include <gtsam_unstable/slam/JacobianSchurFactor.h>
|
||||
|
||||
namespace gtsam {
|
||||
/**
|
||||
* JacobianFactor for Schur complement that uses Q noise model
|
||||
*/
|
||||
template<size_t D>
|
||||
class JacobianFactorQR: public JacobianSchurFactor<D> {
|
||||
|
||||
typedef JacobianSchurFactor<D> Base;
|
||||
|
||||
public:
|
||||
|
||||
/**
|
||||
* Constructor
|
||||
*/
|
||||
JacobianFactorQR(const std::vector<typename Base::KeyMatrix2D>& Fblocks,
|
||||
const Matrix& E, const Matrix3& P, const Vector& b,
|
||||
const SharedDiagonal& model = SharedDiagonal()) :
|
||||
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 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;
|
||||
}
|
||||
//gfg.print("gfg");
|
||||
|
||||
// eliminate the point
|
||||
GaussianBayesNet::shared_ptr bn;
|
||||
GaussianFactorGraph::shared_ptr fg;
|
||||
std::vector < Key > variables;
|
||||
variables.push_back(pointKey);
|
||||
boost::tie(bn, fg) = gfg.eliminatePartialSequential(variables, EliminateQR);
|
||||
//fg->print("fg");
|
||||
|
||||
JacobianFactor::operator=(JacobianFactor(*fg));
|
||||
}
|
||||
};
|
||||
// class
|
||||
|
||||
}// gtsam
|
||||
|
|
@ -0,0 +1,44 @@
|
|||
/*
|
||||
* @file JacobianFactorSVD.h
|
||||
* @date Oct 27, 2013
|
||||
* @uthor Frank Dellaert
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
#include "gtsam_unstable/slam/JacobianSchurFactor.h"
|
||||
|
||||
namespace gtsam {
|
||||
/**
|
||||
* JacobianFactor for Schur complement that uses Q noise model
|
||||
*/
|
||||
template<size_t D>
|
||||
class JacobianFactorSVD: public JacobianSchurFactor<D> {
|
||||
|
||||
public:
|
||||
|
||||
typedef Eigen::Matrix<double, 2, D> Matrix2D;
|
||||
typedef std::pair<Key, Matrix2D> KeyMatrix2D;
|
||||
|
||||
/// Default constructor
|
||||
JacobianFactorSVD() {}
|
||||
|
||||
/// Constructor
|
||||
JacobianFactorSVD(const std::vector<KeyMatrix2D>& Fblocks, const Matrix& Enull, const Vector& b,
|
||||
const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor<D>() {
|
||||
size_t numKeys = Enull.rows() / 2;
|
||||
size_t j = 0, m2 = 2*numKeys-3;
|
||||
// PLAIN NULL SPACE TRICK
|
||||
// Matrix Q = Enull * Enull.transpose();
|
||||
// BOOST_FOREACH(const KeyMatrix2D& it, Fblocks)
|
||||
// QF.push_back(KeyMatrix(it.first, Q.block(0, 2 * j++, m2, 2) * it.second));
|
||||
// JacobianFactor factor(QF, Q * b);
|
||||
typedef std::pair<Key, Matrix> KeyMatrix;
|
||||
std::vector<KeyMatrix> QF;
|
||||
QF.reserve(numKeys);
|
||||
BOOST_FOREACH(const KeyMatrix2D& it, Fblocks)
|
||||
QF.push_back(KeyMatrix(it.first, (Enull.transpose()).block(0, 2 * j++, m2, 2) * it.second));
|
||||
JacobianFactor::fillTerms(QF, Enull.transpose() * b, model);
|
||||
}
|
||||
};
|
||||
|
||||
}
|
||||
|
|
@ -51,6 +51,7 @@ protected:
|
|||
typedef Eigen::Matrix<double, D, 2> MatrixD2; // F'
|
||||
typedef std::pair<Key, Matrix2D> KeyMatrix2D; // Fblocks
|
||||
typedef Eigen::Matrix<double, D, D> MatrixDD; // camera hessian block
|
||||
typedef Eigen::Matrix<double, 2, 3> Matrix23;
|
||||
typedef Eigen::Matrix<double, D, 1> VectorD;
|
||||
typedef Eigen::Matrix<double, 2, 2> Matrix2;
|
||||
|
||||
|
|
@ -321,7 +322,7 @@ public:
|
|||
const double lambda = 0.0) const {
|
||||
|
||||
int numKeys = this->keys_.size();
|
||||
std::vector<KeyMatrix2D> Fblocks;
|
||||
std::vector < KeyMatrix2D > Fblocks;
|
||||
double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point,
|
||||
lambda);
|
||||
F = zeros(2 * numKeys, D * numKeys);
|
||||
|
|
@ -335,12 +336,13 @@ public:
|
|||
// ****************************************************************************************************
|
||||
/// SVD version
|
||||
double computeJacobiansSVD(std::vector<KeyMatrix2D>& Fblocks, Matrix& Enull,
|
||||
Vector& b, const Cameras& cameras, const Point3& point,
|
||||
double lambda = 0.0, bool diagonalDamping = false) const {
|
||||
Vector& b, const Cameras& cameras, const Point3& point, double lambda =
|
||||
0.0, bool diagonalDamping = false) const {
|
||||
|
||||
Matrix E;
|
||||
Matrix3 PointCov; // useless
|
||||
double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, diagonalDamping); // diagonalDamping should have no effect (only on PointCov)
|
||||
double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda,
|
||||
diagonalDamping); // diagonalDamping should have no effect (only on PointCov)
|
||||
|
||||
// Do SVD on A
|
||||
Eigen::JacobiSVD < Matrix > svd(E, Eigen::ComputeFullU);
|
||||
|
|
@ -359,7 +361,7 @@ public:
|
|||
const Cameras& cameras, const Point3& point) const {
|
||||
|
||||
int numKeys = this->keys_.size();
|
||||
std::vector<KeyMatrix2D> Fblocks;
|
||||
std::vector < KeyMatrix2D > Fblocks;
|
||||
double f = computeJacobiansSVD(Fblocks, Enull, b, cameras, point);
|
||||
F.resize(2 * numKeys, D * numKeys);
|
||||
F.setZero();
|
||||
|
|
@ -378,13 +380,15 @@ public:
|
|||
|
||||
int numKeys = this->keys_.size();
|
||||
|
||||
std::vector<KeyMatrix2D> Fblocks;
|
||||
std::vector < KeyMatrix2D > Fblocks;
|
||||
Matrix E;
|
||||
Matrix3 PointCov;
|
||||
Vector b;
|
||||
double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda,
|
||||
diagonalDamping);
|
||||
|
||||
//#define HESSIAN_BLOCKS
|
||||
#ifdef HESSIAN_BLOCKS
|
||||
// Create structures for Hessian Factors
|
||||
std::vector < Matrix > Gs(numKeys * (numKeys + 1) / 2);
|
||||
std::vector < Vector > gs(numKeys);
|
||||
|
|
@ -397,6 +401,42 @@ public:
|
|||
|
||||
return boost::make_shared < RegularHessianFactor<D>
|
||||
> (this->keys_, Gs, gs, f);
|
||||
#else
|
||||
size_t n1 = D * numKeys + 1;
|
||||
std::vector<DenseIndex> dims(numKeys + 1); // this also includes the b term
|
||||
std::fill(dims.begin(), dims.end() - 1, D);
|
||||
dims.back() = 1;
|
||||
|
||||
SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(n1, n1)); // for 10 cameras, size should be (10*D+1 x 10*D+1)
|
||||
sparseSchurComplement(Fblocks, E, PointCov, b, augmentedHessian); // augmentedHessian.matrix().block<D,D> (i1,i2) = ...
|
||||
augmentedHessian(numKeys,numKeys)(0,0) = f;
|
||||
return boost::make_shared<RegularHessianFactor<D> >(
|
||||
this->keys_, augmentedHessian);
|
||||
|
||||
#endif
|
||||
}
|
||||
|
||||
// ****************************************************************************************************
|
||||
boost::shared_ptr<RegularHessianFactor<D> > updateAugmentedHessian(
|
||||
const Cameras& cameras, const Point3& point, const double lambda,
|
||||
bool diagonalDamping, SymmetricBlockMatrix& augmentedHessian) const {
|
||||
|
||||
int numKeys = this->keys_.size();
|
||||
|
||||
std::vector < KeyMatrix2D > Fblocks;
|
||||
Matrix E;
|
||||
Matrix3 PointCov;
|
||||
Vector b;
|
||||
double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda,
|
||||
diagonalDamping);
|
||||
|
||||
std::vector<DenseIndex> dims(numKeys + 1); // this also includes the b term
|
||||
std::fill(dims.begin(), dims.end() - 1, D);
|
||||
dims.back() = 1;
|
||||
|
||||
updateSparseSchurComplement(Fblocks, E, PointCov, b, augmentedHessian); // augmentedHessian.matrix().block<D,D> (i1,i2) = ...
|
||||
std::cout << "f "<< f <<std::endl;
|
||||
augmentedHessian(numKeys,numKeys)(0,0) += f;
|
||||
}
|
||||
|
||||
// ****************************************************************************************************
|
||||
|
|
@ -437,47 +477,133 @@ public:
|
|||
}
|
||||
|
||||
// ****************************************************************************************************
|
||||
void sparseSchurComplement(const std::vector<KeyMatrix2D>& Fblocks,
|
||||
const Matrix& E, const Matrix& PointCov, const Vector& b,
|
||||
/*output ->*/std::vector<Matrix>& Gs, std::vector<Vector>& gs) const {
|
||||
void updateSparseSchurComplement(const std::vector<KeyMatrix2D>& Fblocks,
|
||||
const Matrix& E, const Matrix& P /*Point Covariance*/, const Vector& b,
|
||||
/*output ->*/SymmetricBlockMatrix& augmentedHessian) const {
|
||||
// Schur complement trick
|
||||
// Gs = F' * F - F' * E * inv(E'*E) * E' * F
|
||||
// gs = F' * (b - E * inv(E'*E) * E' * b)
|
||||
// Gs = F' * F - F' * E * P * E' * F
|
||||
// gs = F' * (b - E * P * E' * b)
|
||||
|
||||
// a single point is observed in numKeys cameras
|
||||
size_t numKeys = this->keys_.size(); // cameras observing current point
|
||||
size_t aug_numKeys = augmentedHessian.rows() - 1; // all cameras in the group
|
||||
|
||||
// Blockwise Schur complement
|
||||
for (size_t i1 = 0; i1 < numKeys; i1++) { // for each camera
|
||||
|
||||
const Matrix2D& Fi1 = Fblocks.at(i1).second;
|
||||
const Matrix23 Ei1_P = E.block<2, 3>(2 * i1, 0) * P;
|
||||
|
||||
// D = (Dx2) * (2)
|
||||
// (augmentedHessian.matrix()).block<D,1> (i1,numKeys+1) = Fi1.transpose() * b.segment < 2 > (2 * i1); // F' * b
|
||||
size_t aug_i1 = this->keys_[i1];
|
||||
std::cout << "i1 "<< i1 <<std::endl;
|
||||
std::cout << "aug_i1 "<< aug_i1 <<std::endl;
|
||||
std::cout << "aug_numKeys "<< aug_numKeys <<std::endl;
|
||||
augmentedHessian(aug_i1,aug_numKeys) = //augmentedHessian(aug_i1,aug_numKeys) +
|
||||
Fi1.transpose() * b.segment < 2 > (2 * i1) // F' * b
|
||||
- Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (Dx2) * (2x3) * (3*2m) * (2m x 1)
|
||||
|
||||
// (DxD) = (Dx2) * ( (2xD) - (2x3) * (3x2) * (2xD) )
|
||||
std::cout << "filled 1 " <<std::endl;
|
||||
augmentedHessian(aug_i1,aug_i1) = //augmentedHessian(aug_i1,aug_i1) +
|
||||
Fi1.transpose() * (Fi1 - Ei1_P * E.block<2, 3>(2 * i1, 0).transpose() * Fi1);
|
||||
|
||||
// upper triangular part of the hessian
|
||||
for (size_t i2 = i1+1; i2 < numKeys; i2++) { // for each camera
|
||||
const Matrix2D& Fi2 = Fblocks.at(i2).second;
|
||||
size_t aug_i2 = this->keys_[i2];
|
||||
std::cout << "i2 "<< i2 <<std::endl;
|
||||
std::cout << "aug_i2 "<< aug_i2 <<std::endl;
|
||||
|
||||
// (DxD) = (Dx2) * ( (2x2) * (2xD) )
|
||||
augmentedHessian(aug_i1, aug_i2) = //augmentedHessian(aug_i1, aug_i2)
|
||||
- Fi1.transpose() * (Ei1_P * E.block<2, 3>(2 * i2, 0).transpose() * Fi2);
|
||||
}
|
||||
} // end of for over cameras
|
||||
}
|
||||
|
||||
// ****************************************************************************************************
|
||||
void sparseSchurComplement(const std::vector<KeyMatrix2D>& Fblocks,
|
||||
const Matrix& E, const Matrix& P /*Point Covariance*/, const Vector& b,
|
||||
/*output ->*/SymmetricBlockMatrix& augmentedHessian) const {
|
||||
// Schur complement trick
|
||||
// Gs = F' * F - F' * E * P * E' * F
|
||||
// gs = F' * (b - E * P * E' * b)
|
||||
|
||||
// a single point is observed in numKeys cameras
|
||||
size_t numKeys = this->keys_.size();
|
||||
|
||||
// Blockwise Schur complement
|
||||
int GsCount = 0;
|
||||
for (size_t i1 = 0; i1 < numKeys; i1++) { // for each camera
|
||||
const Matrix2D& Fi1 = Fblocks.at(i1).second;
|
||||
// D = (Dx2) * (2)
|
||||
gs.at(i1) = Fi1.transpose() * b.segment < 2 > (2 * i1); // F' * b
|
||||
|
||||
for (size_t i2 = 0; i2 < numKeys; i2++) {
|
||||
const Matrix2D& Fi1 = Fblocks.at(i1).second;
|
||||
const Matrix23 Ei1_P = E.block<2, 3>(2 * i1, 0) * P;
|
||||
|
||||
// D = (Dx2) * (2)
|
||||
// (augmentedHessian.matrix()).block<D,1> (i1,numKeys+1) = Fi1.transpose() * b.segment < 2 > (2 * i1); // F' * b
|
||||
augmentedHessian(i1,numKeys) = Fi1.transpose() * b.segment < 2 > (2 * i1) // F' * b
|
||||
- Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (Dx2) * (2x3) * (3*2m) * (2m x 1)
|
||||
|
||||
// (DxD) = (Dx2) * ( (2xD) - (2x3) * (3x2) * (2xD) )
|
||||
augmentedHessian(i1,i1) =
|
||||
Fi1.transpose() * (Fi1 - Ei1_P * E.block<2, 3>(2 * i1, 0).transpose() * Fi1);
|
||||
|
||||
// upper triangular part of the hessian
|
||||
for (size_t i2 = i1+1; i2 < numKeys; i2++) { // for each camera
|
||||
const Matrix2D& Fi2 = Fblocks.at(i2).second;
|
||||
|
||||
// Compute (Ei1 * PointCov * Ei2')
|
||||
// (2x2) = (2x3) * (3x3) * (3x2)
|
||||
Matrix2 E_invEtE_Et = E.block<2, 3>(2 * i1, 0) * PointCov * (E.block<2, 3>(2 * i2, 0)).transpose();
|
||||
|
||||
// D = (Dx2) * (2x2) * (2)
|
||||
gs.at(i1) -= Fi1.transpose() * ( E_invEtE_Et * b.segment < 2 > (2 * i2) );
|
||||
|
||||
if (i2 == i1) { // diagonal entries
|
||||
// (DxD) = (Dx2) * ( (2xD) - (2x2) * (2xD) )
|
||||
Gs.at(GsCount) = Fi1.transpose() * (Fi1 - E_invEtE_Et * Fi2);
|
||||
GsCount++;
|
||||
}
|
||||
if (i2 > i1) { // off diagonal
|
||||
// (DxD) = (Dx2) * ( (2x2) * (2xD) )
|
||||
Gs.at(GsCount) = - Fi1.transpose() * ( E_invEtE_Et * Fi2 );
|
||||
GsCount++;
|
||||
}
|
||||
}
|
||||
augmentedHessian(i1,i2) =
|
||||
-Fi1.transpose() * (Ei1_P * E.block<2, 3>(2 * i2, 0).transpose() * Fi2);
|
||||
}
|
||||
} // end of for over cameras
|
||||
}
|
||||
|
||||
// ****************************************************************************************************
|
||||
void sparseSchurComplement(const std::vector<KeyMatrix2D>& Fblocks,
|
||||
const Matrix& E, const Matrix& P /*Point Covariance*/, const Vector& b,
|
||||
/*output ->*/std::vector<Matrix>& Gs, std::vector<Vector>& gs) const {
|
||||
// Schur complement trick
|
||||
// Gs = F' * F - F' * E * P * E' * F
|
||||
// gs = F' * (b - E * P * E' * b)
|
||||
|
||||
// a single point is observed in numKeys cameras
|
||||
size_t numKeys = this->keys_.size();
|
||||
|
||||
int GsIndex = 0;
|
||||
// Blockwise Schur complement
|
||||
for (size_t i1 = 0; i1 < numKeys; i1++) { // for each camera
|
||||
// GsIndex points to the upper triangular blocks
|
||||
// 0 1 2 3 4
|
||||
// X 5 6 7 8
|
||||
// X X 9 10 11
|
||||
// X X X 12 13
|
||||
// X X X X 14
|
||||
const Matrix2D& Fi1 = Fblocks.at(i1).second;
|
||||
|
||||
const Matrix23 Ei1_P = E.block<2, 3>(2 * i1, 0) * P;
|
||||
|
||||
{ // for i1 = i2
|
||||
// D = (Dx2) * (2)
|
||||
gs.at(i1) = Fi1.transpose() * b.segment < 2 > (2 * i1); // F' * b
|
||||
// D = (Dx2) * (2x3) * (3*2m) * (2m x 1)
|
||||
gs.at(i1) -= Fi1.transpose() * (Ei1_P * (E.transpose() * b));
|
||||
|
||||
// (DxD) = (Dx2) * ( (2xD) - (2x3) * (3x2) * (2xD) )
|
||||
Gs.at(GsIndex) = Fi1.transpose() * (Fi1 - Ei1_P * E.block<2, 3>(2 * i1, 0).transpose() * Fi1);
|
||||
GsIndex++;
|
||||
}
|
||||
// upper triangular part of the hessian
|
||||
for (size_t i2 = i1+1; i2 < numKeys; i2++) { // for each camera
|
||||
const Matrix2D& Fi2 = Fblocks.at(i2).second;
|
||||
|
||||
// (DxD) = (Dx2) * ( (2x2) * (2xD) )
|
||||
Gs.at(GsIndex) = -Fi1.transpose() * (Ei1_P * E.block<2, 3>(2 * i2, 0).transpose() * Fi2);
|
||||
GsIndex++;
|
||||
}
|
||||
} // end of for over cameras
|
||||
}
|
||||
// ****************************************************************************************************
|
||||
boost::shared_ptr<ImplicitSchurFactor<D> > createImplicitSchurFactor(
|
||||
const Cameras& cameras, const Point3& point, double lambda = 0.0,
|
||||
|
|
@ -494,7 +620,7 @@ public:
|
|||
boost::shared_ptr<JacobianFactorQ<D> > createJacobianQFactor(
|
||||
const Cameras& cameras, const Point3& point, double lambda = 0.0,
|
||||
bool diagonalDamping = false) const {
|
||||
std::vector<KeyMatrix2D> Fblocks;
|
||||
std::vector < KeyMatrix2D > Fblocks;
|
||||
Matrix E;
|
||||
Matrix3 PointCov;
|
||||
Vector b;
|
||||
|
|
|
|||
Loading…
Reference in New Issue