Loading...
Searching...
No Matches
RegularImplicitSchurFactor.h
Go to the documentation of this file.
#pragma once
#include <gtsam/geometry/CameraSet.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/VectorValues.h>
#include <iosfwd>
#include <map>
#include <string>
#include <vector>
namespace gtsam {
template<class CAMERA>
class RegularImplicitSchurFactor: public GaussianFactor {
public:
typedef RegularImplicitSchurFactor This;
typedef boost::shared_ptr<This> shared_ptr;
protected:
// This factor is closely related to a CameraSet
typedef CameraSet<CAMERA> Set;
typedef typename CAMERA::Measurement Z;
static const int D = traits<CAMERA>::dimension;
static const int ZDim = traits<Z>::dimension;
typedef Eigen::Matrix<double, ZDim, D> MatrixZD;
typedef Eigen::Matrix<double, D, D> MatrixDD;
typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> > FBlocks;
FBlocks FBlocks_;
const Matrix PointCovariance_;
const Matrix E_;
const Vector b_;
public:
RegularImplicitSchurFactor() {
}
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) {}
~RegularImplicitSchurFactor() override {
}
const FBlocks& Fs() const {
return FBlocks_;
}
const Matrix& E() const {
return E_;
}
const Vector& b() const {
return b_;
}
const Matrix& getPointCovariance() const {
return PointCovariance_;
}
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const override {
std::cout << " RegularImplicitSchurFactor " << std::endl;
Factor::print(s);
for (size_t pos = 0; pos < size(); ++pos) {
std::cout << "Fblock:\n" << FBlocks_[pos] << std::endl;
}
std::cout << "PointCovariance:\n" << PointCovariance_ << std::endl;
std::cout << "E:\n" << E_ << std::endl;
std::cout << "b:\n" << b_.transpose() << std::endl;
}
bool equals(const GaussianFactor& lf, double tol) const override {
const This* f = dynamic_cast<const This*>(&lf);
if (!f)
return false;
for (size_t k = 0; k < FBlocks_.size(); ++k) {
if (keys_[k] != f->keys_[k])
return false;
if (!equal_with_abs_tol(FBlocks_[k], f->FBlocks_[k], tol))
return false;
}
return equal_with_abs_tol(PointCovariance_, f->PointCovariance_, tol)
&& equal_with_abs_tol(E_, f->E_, tol)
&& equal_with_abs_tol(b_, f->b_, tol);
}
DenseIndex getDim(const_iterator variable) const override {
return D;
}
void updateHessian(const KeyVector& keys,
SymmetricBlockMatrix* info) const override {
throw std::runtime_error(
"RegularImplicitSchurFactor::updateHessian non implemented");
}
Matrix augmentedJacobian() const override {
throw std::runtime_error(
"RegularImplicitSchurFactor::augmentedJacobian non implemented");
return Matrix();
}
std::pair<Matrix, Vector> jacobian() const override {
throw std::runtime_error(
"RegularImplicitSchurFactor::jacobian non implemented");
return std::make_pair(Matrix(), Vector());
}
Matrix augmentedInformation() const override {
// Do the Schur complement
SymmetricBlockMatrix augmentedHessian =
Set::SchurComplement(FBlocks_, E_, b_);
return augmentedHessian.selfadjointView();
}
Matrix information() const override {
Matrix augmented = augmentedInformation();
int m = this->keys_.size();
size_t M = D * m;
return augmented.block(0, 0, M, M);
}
using GaussianFactor::hessianDiagonal;
void hessianDiagonalAdd(VectorValues &d) const override {
// diag(Hessian) = diag(F' * (I - E * PointCov * E') * F);
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 ZDim) * (ZDim x 3)
const MatrixZD& Fj = FBlocks_[k];
Eigen::Matrix<double, D, 3> FtE = Fj.transpose()
* E_.block<ZDim, 3>(ZDim * k, 0);
Eigen::Matrix<double, D, 1> dj;
for (int k = 0; k < D; ++k) { // for each diagonal element of the camera hessian
// Vector column_k_Fj = Fj.col(k);
dj(k) = Fj.col(k).squaredNorm(); // dot(column_k_Fj, column_k_Fj);
// Vector column_k_FtE = FtE.row(k);
// (1 x 1) = (1 x 3) * (3 * 3) * (3 x 1)
dj(k) -= FtE.row(k) * PointCovariance_ * FtE.row(k).transpose();
}
auto result = d.emplace(j, dj);
if(!result.second) {
result.first->second += dj;
}
}
}
void hessianDiagonal(double* d) const override {
// diag(Hessian) = diag(F' * (I - E * PointCov * E') * F);
// Use eigen magic to access raw memory
typedef Eigen::Matrix<double, D, 1> DVector;
typedef Eigen::Map<DVector> DMap;
for (size_t pos = 0; pos < size(); ++pos) { // for each camera in the factor
Key j = keys_[pos];
// Calculate Fj'*Ej for the current camera (observing a single point)
// D x 3 = (D x ZDim) * (ZDim x 3)
const MatrixZD& Fj = FBlocks_[pos];
Eigen::Matrix<double, D, 3> FtE = Fj.transpose()
* E_.block<ZDim, 3>(ZDim * pos, 0);
DVector dj;
for (int k = 0; k < D; ++k) { // for each diagonal element of the camera hessian
dj(k) = Fj.col(k).squaredNorm();
// (1 x 1) = (1 x 3) * (3 * 3) * (3 x 1)
dj(k) -= FtE.row(k) * PointCovariance_ * FtE.row(k).transpose();
}
DMap(d + D * j) += dj;
}
}
std::map<Key, Matrix> hessianBlockDiagonal() const override {
std::map<Key, Matrix> blocks;
// F'*(I - E*P*E')*F
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 MatrixZD& Fj = FBlocks_[pos];
// Eigen::Matrix<double, D, 3> FtE = Fj.transpose()
// * E_.block<ZDim, 3>(ZDim * pos, 0);
// blocks[j] = Fj.transpose() * Fj
// - FtE * PointCovariance_ * FtE.transpose();
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, ZDim, ZDim> I2 = eye(ZDim);
// Matrix2 Q = //
// 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;
}
GaussianFactor::shared_ptr clone() const override {
return boost::make_shared<RegularImplicitSchurFactor<CAMERA> >(keys_,
FBlocks_, PointCovariance_, E_, b_);
throw std::runtime_error(
"RegularImplicitSchurFactor::clone non implemented");
}
GaussianFactor::shared_ptr negate() const override {
return boost::make_shared<RegularImplicitSchurFactor<CAMERA> >(keys_,
FBlocks_, PointCovariance_, E_, b_);
throw std::runtime_error(
"RegularImplicitSchurFactor::negate non implemented");
}
// Raw Vector version of y += F'*alpha*(I - E*P*E')*F*x, for testing
static
void multiplyHessianAdd(const Matrix& F, const Matrix& E,
const Matrix& PointCovariance, double alpha, const Vector& x, Vector& y) {
Vector e1 = F * x;
Vector d1 = E.transpose() * e1;
Vector d2 = PointCovariance * d1;
Vector e2 = E * d2;
Vector e3 = alpha * (e1 - e2);
y += F.transpose() * e3;
}
typedef std::vector<Vector2, Eigen::aligned_allocator<Vector2>> Error2s;
void projectError2(const Error2s& e1, Error2s& e2) const {
// d1 = E.transpose() * (e1-ZDim*b) = (3*2m)*2m
Vector3 d1;
d1.setZero();
for (size_t k = 0; k < size(); k++)
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] - ZDim * b_.segment<ZDim>(k * ZDim)
- E_.block<ZDim, 3>(ZDim * k, 0) * d2;
}
/*
* This definition matches the linearized error in the Hessian Factor:
* LinError(x) = x'*H*x - 2*x'*eta + f
* with:
* H = F' * (I-E'*P*E) * F = F' * Q * F
* eta = F' * (I-E'*P*E) * b = F' * Q * b
* f = nonlinear error
* (x'*H*x - 2*x'*eta + f) = x'*F'*Q*F*x - 2*x'*F'*Q *b + f = x'*F'*Q*(F*x - 2*b) + f
*/
double error(const VectorValues& x) const override {
// resize does not do malloc if correct size
e1.resize(size());
e2.resize(size());
// e1 = F * x - b = (2m*dm)*dm
for (size_t k = 0; k < size(); ++k)
e1[k] = FBlocks_[k] * x.at(keys_[k]);
projectError2(e1, e2);
double result = 0;
for (size_t k = 0; k < size(); ++k)
result += dot(e1[k], e2[k]);
double f = b_.squaredNorm();
return 0.5 * (result + f);
}
// needed to be GaussianFactor - (I - E*P*E')*(F*x - b)
// This is wrong and does not match the definition in Hessian,
// but it matches the definition of the Jacobian factor (JF)
double errorJF(const VectorValues& x) const {
// resize does not do malloc if correct size
e1.resize(size());
e2.resize(size());
// e1 = F * x - b = (2m*dm)*dm
for (size_t k = 0; k < size(); ++k)
e1[k] = FBlocks_[k] * x.at(keys_[k]) - b_.segment<ZDim>(k * ZDim);
projectError(e1, e2);
double result = 0;
for (size_t k = 0; k < size(); ++k)
result += dot(e2[k], e2[k]);
// std::cout << "implicitFactor::error result " << result << std::endl;
return 0.5 * result;
}
void projectError(const Error2s& e1, Error2s& e2) const {
// d1 = E.transpose() * e1 = (3*2m)*2m
Vector3 d1;
d1.setZero();
for (size_t k = 0; k < size(); 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<ZDim, 3>(ZDim * k, 0) * d2;
}
mutable Error2s e1, e2;
void multiplyHessianAdd(double alpha, const double* x, double* y) const {
// Use eigen magic to access raw memory
typedef Eigen::Matrix<double, D, 1> DVector;
typedef Eigen::Map<DVector> DMap;
typedef Eigen::Map<const DVector> ConstDMap;
// resize does not do malloc if correct size
e1.resize(size());
e2.resize(size());
// e1 = F * x = (2m*dm)*dm
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
for (size_t k = 0; k < size(); ++k) {
Key key = keys_[k];
DMap(y + D * key) += FBlocks_[k].transpose() * alpha * e2[k];
}
}
void multiplyHessianAdd(double alpha, const double* x, double* y,
std::vector<size_t> keys) const {
}
void multiplyHessianAdd(double alpha, const VectorValues& x,
VectorValues& y) const override {
// resize does not do malloc if correct size
e1.resize(size());
e2.resize(size());
// e1 = F * x = (2m*dm)*dm
for (size_t k = 0; k < size(); ++k)
e1[k] = FBlocks_[k] * x.at(keys_[k]);
projectError(e1, e2);
// y += F.transpose()*e2 = (2d*2m)*2m
for (size_t k = 0; k < size(); ++k) {
Key key = keys_[k];
static const Vector empty;
std::pair<VectorValues::iterator, bool> it = y.tryInsert(key, empty);
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].cols());
yi += FBlocks_[k].transpose() * alpha * e2[k];
}
}
void multiplyHessianDummy(double alpha, const VectorValues& x,
VectorValues& y) const {
for (size_t k = 0; k < size(); ++k) {
static const Vector empty;
Key key = keys_[k];
std::pair<VectorValues::iterator, bool> it = y.tryInsert(key, empty);
Vector& yi = it.first->second;
yi = x.at(key);
}
}
VectorValues gradientAtZero() const override {
// calculate Q*b
e1.resize(size());
e2.resize(size());
for (size_t k = 0; k < size(); 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].transpose() * e2[k]);
}
// return it
return g;
}
void gradientAtZero(double* d) const override {
// Use eigen magic to access raw memory
typedef Eigen::Matrix<double, D, 1> DVector;
typedef Eigen::Map<DVector> DMap;
// calculate Q*b
e1.resize(size());
e2.resize(size());
for (size_t k = 0; k < size(); 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].transpose() * e2[k];
}
}
Vector gradient(Key key, const VectorValues& x) const override {
throw std::runtime_error(
"gradient for RegularImplicitSchurFactor is not implemented yet");
}
};
// end class RegularImplicitSchurFactor
template<class CAMERA>
const int RegularImplicitSchurFactor<CAMERA>::D;
template<class CAMERA>
const int RegularImplicitSchurFactor<CAMERA>::ZDim;
// traits
template<class CAMERA> struct traits<RegularImplicitSchurFactor<CAMERA> > : public Testable<
RegularImplicitSchurFactor<CAMERA> > {
};
}
CameraSet.h
Base class to create smart factors on poses or cameras.
VectorValues.h
Factor Graph Values.
JacobianFactor.h
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::DenseIndex
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition types.h:106
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
gtsam::dot
double dot(const V1 &a, const V2 &b)
Dot product.
Definition Vector.h:195
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
gtsam::equal_with_abs_tol
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
equals with a tolerance
Definition Matrix.h:81
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
gtsam::SymmetricBlockMatrix
This class stores a dense matrix and allows it to be accessed as a collection of blocks.
Definition SymmetricBlockMatrix.h:52
gtsam::SymmetricBlockMatrix::selfadjointView
Eigen::SelfAdjointView< constBlock, Eigen::Upper > selfadjointView(DenseIndex I, DenseIndex J) const
Return the square sub-matrix that contains blocks(i:j, i:j).
Definition SymmetricBlockMatrix.h:156
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
gtsam::CameraSet
A set of cameras, all with their own calibration.
Definition CameraSet.h:36
gtsam::CameraSet::SchurComplement
static SymmetricBlockMatrix SchurComplement(const std::vector< Eigen::Matrix< double, ZDim, ND >, Eigen::aligned_allocator< Eigen::Matrix< double, ZDim, ND > > > &Fs, const Matrix &E, const Eigen::Matrix< double, N, N > &P, const Vector &b)
Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix G = F' * F - F' * E * P * ...
Definition CameraSet.h:150
gtsam::Factor::keys
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition Factor.h:140
gtsam::Factor::keys_
KeyVector keys_
The keys involved in this factor.
Definition Factor.h:85
gtsam::Factor::empty
bool empty() const
Whether the factor is empty (involves zero variables).
Definition Factor.h:128
gtsam::Factor::print
virtual void print(const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
print
Definition Factor.cpp:29
gtsam::Factor::const_iterator
KeyVector::const_iterator const_iterator
Const iterator over keys.
Definition Factor.h:80
gtsam::Factor::size
size_t size() const
Definition Factor.h:157
gtsam::GaussianFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition GaussianFactor.h:42
gtsam::GaussianFactor::GaussianFactor
GaussianFactor()
Default constructor creates empty factor.
Definition GaussianFactor.h:46
gtsam::GaussianFactor::hessianDiagonal
VectorValues hessianDiagonal() const
Return the diagonal of the Hessian for this factor.
Definition GaussianFactor.cpp:35
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
gtsam::VectorValues::insert
iterator insert(const std::pair< Key, Vector > &key_value)
Insert a vector value with key j.
Definition VectorValues.cpp:91
gtsam::VectorValues::emplace
std::pair< VectorValues::iterator, bool > emplace(Key j, Args &&... args)
Emplace a vector value with key j.
Definition VectorValues.h:185
gtsam::VectorValues::at
Vector & at(Key j)
Read/write access to the vector value with key j, throws std::out_of_range if j does not exist,...
Definition VectorValues.h:139
gtsam::VectorValues::tryInsert
std::pair< iterator, bool > tryInsert(Key j, const Vector &value)
insert that mimics the STL map insert - if the value already exists, the map is not modified and an i...
Definition VectorValues.h:209
gtsam::RegularImplicitSchurFactor
RegularImplicitSchurFactor.
Definition RegularImplicitSchurFactor.h:39
gtsam::RegularImplicitSchurFactor::E_
const Matrix E_
The 2m*3 E Jacobian with respect to the point.
Definition RegularImplicitSchurFactor.h:60
gtsam::RegularImplicitSchurFactor::projectError
void projectError(const Error2s &e1, Error2s &e2) const
Calculate corrected error Q*e = (I - E*P*E')*e.
Definition RegularImplicitSchurFactor.h:357
gtsam::RegularImplicitSchurFactor::clone
GaussianFactor::shared_ptr clone() const override
Clone a factor (make a deep copy).
Definition RegularImplicitSchurFactor.h:256
gtsam::RegularImplicitSchurFactor::hessianDiagonalAdd
void hessianDiagonalAdd(VectorValues &d) const override
Add the diagonal of the Hessian for this factor to existing VectorValues.
Definition RegularImplicitSchurFactor.h:174
gtsam::RegularImplicitSchurFactor::RegularImplicitSchurFactor
RegularImplicitSchurFactor(const KeyVector &keys, const FBlocks &Fs, const Matrix &E, const Matrix &P, const Vector &b)
Construct from blocks of F, E, inv(E'*E), and RHS vector b.
Definition RegularImplicitSchurFactor.h:80
gtsam::RegularImplicitSchurFactor::RegularImplicitSchurFactor
RegularImplicitSchurFactor()
Constructor.
Definition RegularImplicitSchurFactor.h:66
gtsam::RegularImplicitSchurFactor::b_
const Vector b_
2m-dimensional RHS vector
Definition RegularImplicitSchurFactor.h:61
gtsam::RegularImplicitSchurFactor::jacobian
std::pair< Matrix, Vector > jacobian() const override
Return the dense Jacobian and right-hand-side , with the noise models baked into A and b.
Definition RegularImplicitSchurFactor.h:148
gtsam::RegularImplicitSchurFactor::MatrixDD
Eigen::Matrix< double, D, D > MatrixDD
camera Hessian
Definition RegularImplicitSchurFactor.h:55
gtsam::RegularImplicitSchurFactor::gradientAtZero
void gradientAtZero(double *d) const override
Calculate gradient, which is -F'Q*b, see paper - RAW MEMORY ACCESS.
Definition RegularImplicitSchurFactor.h:479
gtsam::RegularImplicitSchurFactor::ZDim
static const int ZDim
Measurement dimension.
Definition RegularImplicitSchurFactor.h:52
gtsam::RegularImplicitSchurFactor::information
Matrix information() const override
Compute full information matrix
Definition RegularImplicitSchurFactor.h:163
gtsam::RegularImplicitSchurFactor::~RegularImplicitSchurFactor
~RegularImplicitSchurFactor() override
Destructor.
Definition RegularImplicitSchurFactor.h:85
gtsam::RegularImplicitSchurFactor::updateHessian
void updateHessian(const KeyVector &keys, SymmetricBlockMatrix *info) const override
Update an information matrix by adding the information corresponding to this factor (used internally ...
Definition RegularImplicitSchurFactor.h:138
gtsam::RegularImplicitSchurFactor::FBlocks_
FBlocks FBlocks_
All ZDim*D F blocks (one for each camera).
Definition RegularImplicitSchurFactor.h:58
gtsam::RegularImplicitSchurFactor::gradientAtZero
VectorValues gradientAtZero() const override
Calculate gradient, which is -F'Q*b, see paper.
Definition RegularImplicitSchurFactor.h:457
gtsam::RegularImplicitSchurFactor::MatrixZD
Eigen::Matrix< double, ZDim, D > MatrixZD
type of an F block
Definition RegularImplicitSchurFactor.h:54
gtsam::RegularImplicitSchurFactor::PointCovariance_
const Matrix PointCovariance_
the 3*3 matrix P = inv(E'E) (2*2 if degenerate)
Definition RegularImplicitSchurFactor.h:59
gtsam::RegularImplicitSchurFactor::multiplyHessianAdd
void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) const override
Hessian-vector multiply, i.e.
Definition RegularImplicitSchurFactor.h:413
gtsam::RegularImplicitSchurFactor::hessianBlockDiagonal
std::map< Key, Matrix > hessianBlockDiagonal() const override
Return the block diagonal of the Hessian for this factor.
Definition RegularImplicitSchurFactor.h:231
gtsam::RegularImplicitSchurFactor::augmentedInformation
Matrix augmentedInformation() const override
Compute full augmented information matrix
Definition RegularImplicitSchurFactor.h:155
gtsam::RegularImplicitSchurFactor::negate
GaussianFactor::shared_ptr negate() const override
Construct the corresponding anti-factor to negate information stored stored in this factor.
Definition RegularImplicitSchurFactor.h:263
gtsam::RegularImplicitSchurFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition RegularImplicitSchurFactor.h:43
gtsam::RegularImplicitSchurFactor::e1
Error2s e1
Scratch space for multiplyHessianAdd.
Definition RegularImplicitSchurFactor.h:374
gtsam::RegularImplicitSchurFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition RegularImplicitSchurFactor.h:105
gtsam::RegularImplicitSchurFactor::getDim
DenseIndex getDim(const_iterator variable) const override
Degrees of freedom of camera.
Definition RegularImplicitSchurFactor.h:134
gtsam::RegularImplicitSchurFactor::equals
bool equals(const GaussianFactor &lf, double tol) const override
equals
Definition RegularImplicitSchurFactor.h:118
gtsam::RegularImplicitSchurFactor::augmentedJacobian
Matrix augmentedJacobian() const override
Return a dense Jacobian matrix, augmented with b with the noise models baked into A and b.
Definition RegularImplicitSchurFactor.h:143
gtsam::RegularImplicitSchurFactor::This
RegularImplicitSchurFactor This
Typedef to this class.
Definition RegularImplicitSchurFactor.h:42
gtsam::RegularImplicitSchurFactor::D
static const int D
Camera dimension.
Definition RegularImplicitSchurFactor.h:51
gtsam::RegularImplicitSchurFactor::projectError2
void projectError2(const Error2s &e1, Error2s &e2) const
Calculate corrected error Q*(e-ZDim*b) = (I - E*P*E')*(e-ZDim*b).
Definition RegularImplicitSchurFactor.h:287
gtsam::RegularImplicitSchurFactor::multiplyHessianAdd
void multiplyHessianAdd(double alpha, const double *x, double *y) const
double* Hessian-vector multiply, i.e.
Definition RegularImplicitSchurFactor.h:380
gtsam::RegularImplicitSchurFactor::hessianDiagonal
void hessianDiagonal(double *d) const override
add the contribution of this factor to the diagonal of the hessian d(output) = d(input) + deltaHessia...
Definition RegularImplicitSchurFactor.h:205
gtsam::RegularImplicitSchurFactor::multiplyHessianDummy
void multiplyHessianDummy(double alpha, const VectorValues &x, VectorValues &y) const
Dummy version to measure overhead of key access.
Definition RegularImplicitSchurFactor.h:442
gtsam::RegularImplicitSchurFactor::gradient
Vector gradient(Key key, const VectorValues &x) const override
Gradient wrt a key at any values.
Definition RegularImplicitSchurFactor.h:499
VectorValues
The Factor::error simply extracts the.