Documented linear factors better.
Frank Dellaert
parent
0b9c368aca
commit
e0946dfc86
|
|
@ -9,20 +9,21 @@
|
|||
|
||||
namespace gtsam {
|
||||
/**
|
||||
* JacobianFactor for Schur complement that uses the "Nullspace Trick" by Mourikis
|
||||
* JacobianFactor for Schur complement that uses the "Nullspace Trick" by
|
||||
* Mourikis et al.
|
||||
*
|
||||
* This trick is equivalent to the Schur complement, but can be faster.
|
||||
* In essence, the linear factor |E*dp + F*dX - b|, where p is point and X are poses,
|
||||
* is multiplied by Enull, a matrix that spans the left nullspace of E, i.e.,
|
||||
* The mx3 matrix is analyzed with SVD as E = [Erange Enull]*S*V (mxm * mx3 * 3x3)
|
||||
* where Enull is an m x (m-3) matrix
|
||||
* Then Enull'*E*dp = 0, and
|
||||
* In essence, the linear factor |E*dp + F*dX - b|, where p is point and X are
|
||||
* poses, is multiplied by Enull, a matrix that spans the left nullspace of E,
|
||||
* i.e., The mx3 matrix is analyzed with SVD as E = [Erange Enull]*S*V (mxm *
|
||||
* mx3 * 3x3) where Enull is an m x (m-3) matrix Then Enull'*E*dp = 0, and
|
||||
* |Enull'*E*dp + Enull'*F*dX - Enull'*b| == |Enull'*F*dX - Enull'*b|
|
||||
* Normally F is m x 6*numKeys, and Enull'*F yields an (m-3) x 6*numKeys matrix.
|
||||
*
|
||||
* The code below assumes that F is block diagonal and is given as a vector of ZDim*D blocks.
|
||||
* Example: m = 4 (2 measurements), Enull = 4*1, F = 4*12 (for D=6)
|
||||
* Then Enull'*F = 1*4 * 4*12 = 1*12, but each 1*6 piece can be computed as a 1x2 * 2x6 mult
|
||||
* The code below assumes that F is block diagonal and is given as a vector of
|
||||
* ZDim*D blocks. Example: m = 4 (2 measurements), Enull = 4*1, F = 4*12 (for
|
||||
* D=6) Then Enull'*F = 1*4 * 4*12 = 1*12, but each 1*6 piece can be computed as
|
||||
* a 1x2 * 2x6 multiplication.
|
||||
*/
|
||||
template<size_t D, size_t ZDim>
|
||||
class JacobianFactorSVD: public RegularJacobianFactor<D> {
|
||||
|
|
@ -37,10 +38,10 @@ public:
|
|||
JacobianFactorSVD() {
|
||||
}
|
||||
|
||||
/// Empty constructor with keys
|
||||
JacobianFactorSVD(const KeyVector& keys, //
|
||||
const SharedDiagonal& model = SharedDiagonal()) :
|
||||
Base() {
|
||||
/// Empty constructor with keys.
|
||||
JacobianFactorSVD(const KeyVector& keys,
|
||||
const SharedDiagonal& model = SharedDiagonal())
|
||||
: Base() {
|
||||
Matrix zeroMatrix = Matrix::Zero(0, D);
|
||||
Vector zeroVector = Vector::Zero(0);
|
||||
std::vector<KeyMatrix> QF;
|
||||
|
|
@ -51,18 +52,21 @@ public:
|
|||
}
|
||||
|
||||
/**
|
||||
* @brief Constructor
|
||||
* Takes the CameraSet derivatives (as ZDim*D blocks of block-diagonal F)
|
||||
* and a reduced point derivative, Enull
|
||||
* and creates a reduced-rank Jacobian factor on the CameraSet
|
||||
* @brief Construct a new JacobianFactorSVD object, createing a reduced-rank
|
||||
* Jacobian factor on the CameraSet.
|
||||
*
|
||||
* @Fblocks:
|
||||
* @param keys keys associated with F blocks.
|
||||
* @param Fblocks CameraSet derivatives, ZDim*D blocks of block-diagonal F
|
||||
* @param Enull a reduced point derivative
|
||||
* @param b right-hand side
|
||||
* @param model noise model
|
||||
*/
|
||||
JacobianFactorSVD(const KeyVector& keys,
|
||||
const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& Fblocks, const Matrix& Enull,
|
||||
const Vector& b, //
|
||||
const SharedDiagonal& model = SharedDiagonal()) :
|
||||
Base() {
|
||||
JacobianFactorSVD(
|
||||
const KeyVector& keys,
|
||||
const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& Fblocks,
|
||||
const Matrix& Enull, const Vector& b,
|
||||
const SharedDiagonal& model = SharedDiagonal())
|
||||
: Base() {
|
||||
size_t numKeys = Enull.rows() / ZDim;
|
||||
size_t m2 = ZDim * numKeys - 3; // TODO: is this not just Enull.rows()?
|
||||
// PLAIN nullptr SPACE TRICK
|
||||
|
|
@ -74,9 +78,8 @@ public:
|
|||
QF.reserve(numKeys);
|
||||
for (size_t k = 0; k < Fblocks.size(); ++k) {
|
||||
Key key = keys[k];
|
||||
QF.push_back(
|
||||
KeyMatrix(key,
|
||||
(Enull.transpose()).block(0, ZDim * k, m2, ZDim) * Fblocks[k]));
|
||||
QF.emplace_back(
|
||||
key, (Enull.transpose()).block(0, ZDim * k, m2, ZDim) * Fblocks[k]);
|
||||
}
|
||||
JacobianFactor::fillTerms(QF, Enull.transpose() * b, model);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/**
|
||||
* @file RegularImplicitSchurFactor.h
|
||||
* @brief A new type of linear factor (GaussianFactor), which is subclass of GaussianFactor
|
||||
* @brief A subclass of GaussianFactor specialized to structureless SFM.
|
||||
* @author Frank Dellaert
|
||||
* @author Luca Carlone
|
||||
*/
|
||||
|
|
@ -20,6 +20,20 @@ namespace gtsam {
|
|||
|
||||
/**
|
||||
* RegularImplicitSchurFactor
|
||||
*
|
||||
* A specialization of a GaussianFactor to structure-less SFM, which is very
|
||||
* fast in a conjugate gradient (CG) solver. Specifically, as measured in
|
||||
* timeSchurFactors.cpp, it stays very fast for an increasing number of cameras.
|
||||
* The magic is in multiplyHessianAdd, which does the Hessian-vector multiply at
|
||||
* the core of CG, and implements
|
||||
* y += F'*alpha*(I - E*P*E')*F*x
|
||||
* where
|
||||
* - F is the 2mx6m Jacobian of the m 2D measurements wrpt m 6DOF poses
|
||||
* - E is the 2mx3 Jacabian of the m 2D measurements wrpt a 3D point
|
||||
* - P is the covariance on the point
|
||||
* The equation above implicitly executes the Schur complement by removing the
|
||||
* information E*P*E' from the Hessian. It is also very fast as we do not use
|
||||
* the full 6m*6m F matrix, but rather only it's m 6x6 diagonal blocks.
|
||||
*/
|
||||
template<class CAMERA>
|
||||
class RegularImplicitSchurFactor: public GaussianFactor {
|
||||
|
|
@ -38,9 +52,10 @@ protected:
|
|||
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
|
||||
typedef Eigen::Matrix<double, D, D> MatrixDD; ///< camera Hessian
|
||||
typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> > FBlocks;
|
||||
|
||||
const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> > FBlocks_; ///< All ZDim*D F blocks (one for each camera)
|
||||
FBlocks FBlocks_; ///< All ZDim*D F blocks (one for each camera)
|
||||
const Matrix PointCovariance_; ///< the 3*3 matrix P = inv(E'E) (2*2 if degenerate)
|
||||
const Matrix E_; ///< The 2m*3 E Jacobian with respect to the point
|
||||
const Vector b_; ///< 2m-dimensional RHS vector
|
||||
|
|
@ -52,17 +67,25 @@ public:
|
|||
}
|
||||
|
||||
/// Construct from blocks of F, E, inv(E'*E), and RHS vector b
|
||||
RegularImplicitSchurFactor(const KeyVector& keys,
|
||||
const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& FBlocks, const Matrix& E, const Matrix& P,
|
||||
const Vector& b) :
|
||||
GaussianFactor(keys), FBlocks_(FBlocks), PointCovariance_(P), E_(E), b_(b) {
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Construct a new RegularImplicitSchurFactor object.
|
||||
*
|
||||
* @param keys keys corresponding to cameras
|
||||
* @param Fs All ZDim*D F blocks (one for each camera)
|
||||
* @param E Jacobian of measurements wrpt point.
|
||||
* @param P point covariance matrix
|
||||
* @param b RHS vector
|
||||
*/
|
||||
RegularImplicitSchurFactor(const KeyVector& keys, const FBlocks& Fs,
|
||||
const Matrix& E, const Matrix& P, const Vector& b)
|
||||
: GaussianFactor(keys), FBlocks_(Fs), PointCovariance_(P), E_(E), b_(b) {}
|
||||
|
||||
/// Destructor
|
||||
~RegularImplicitSchurFactor() override {
|
||||
}
|
||||
|
||||
std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& FBlocks() const {
|
||||
const FBlocks& Fs() const {
|
||||
return FBlocks_;
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue