50 typedef typename CAMERA::Measurement Z;
54 typedef Eigen::Matrix<double, ZDim, D>
MatrixZD;
56 typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> > FBlocks;
81 const Matrix& E,
const Matrix& P,
const Vector& b)
88 const FBlocks& Fs()
const {
92 const Matrix& E()
const {
96 const Vector& b()
const {
100 const Matrix& getPointCovariance()
const {
106 DefaultKeyFormatter)
const override {
107 std::cout <<
" RegularImplicitSchurFactor " << std::endl;
109 for (
size_t pos = 0; pos <
size(); ++pos) {
110 std::cout <<
"Fblock:\n" <<
FBlocks_[pos] << std::endl;
113 std::cout <<
"E:\n" <<
E_ << std::endl;
114 std::cout <<
"b:\n" <<
b_.transpose() << std::endl;
119 const This* f =
dynamic_cast<const This*
>(&lf);
122 for (
size_t k = 0; k <
FBlocks_.size(); ++k) {
140 throw std::runtime_error(
141 "RegularImplicitSchurFactor::updateHessian non implemented");
144 throw std::runtime_error(
145 "RegularImplicitSchurFactor::augmentedJacobian non implemented");
148 std::pair<Matrix, Vector>
jacobian()
const override {
149 throw std::runtime_error(
150 "RegularImplicitSchurFactor::jacobian non implemented");
151 return std::make_pair(Matrix(), Vector());
165 int m = this->
keys_.size();
167 return augmented.block(0, 0, M, M);
176 for (
size_t k = 0; k <
size(); ++k) {
182 Eigen::Matrix<double, D, 3> FtE = Fj.transpose()
185 Eigen::Matrix<double, D, 1> dj;
186 for (
int k = 0; k <
D; ++k) {
188 dj(k) = Fj.col(k).squaredNorm();
194 auto result = d.
emplace(j, dj);
196 result.first->second += dj;
208 typedef Eigen::Matrix<double, D, 1> DVector;
209 typedef Eigen::Map<DVector> DMap;
211 for (
size_t pos = 0; pos <
size(); ++pos) {
217 Eigen::Matrix<double, D, 3> FtE = Fj.transpose()
221 for (
int k = 0; k <
D; ++k) {
222 dj(k) = Fj.col(k).squaredNorm();
226 DMap(d +
D * j) += dj;
232 std::map<Key, Matrix> blocks;
234 for (
size_t pos = 0; pos <
size(); ++pos) {
243 const Matrix23& Ej =
E_.block<
ZDim, 3>(
ZDim * pos, 0);
244 blocks[j] = Fj.transpose()
257 return boost::make_shared<RegularImplicitSchurFactor<CAMERA> >(
keys_,
259 throw std::runtime_error(
260 "RegularImplicitSchurFactor::clone non implemented");
264 return boost::make_shared<RegularImplicitSchurFactor<CAMERA> >(
keys_,
266 throw std::runtime_error(
267 "RegularImplicitSchurFactor::negate non implemented");
272 void multiplyHessianAdd(
const Matrix& F,
const Matrix& E,
273 const Matrix& PointCovariance,
double alpha,
const Vector& x, Vector& y) {
275 Vector d1 = E.transpose() *
e1;
276 Vector d2 = PointCovariance * d1;
278 Vector e3 = alpha * (
e1 - e2);
279 y += F.transpose() * e3;
282 typedef std::vector<Vector2, Eigen::aligned_allocator<Vector2>> Error2s;
292 for (
size_t k = 0; k <
size(); k++)
293 d1 +=
E_.block<
ZDim, 3>(
ZDim * k, 0).transpose()
300 for (
size_t k = 0; k <
size(); k++)
321 for (
size_t k = 0; k <
size(); ++k)
326 for (
size_t k = 0; k <
size(); ++k)
327 result +=
dot(
e1[k], e2[k]);
329 double f =
b_.squaredNorm();
330 return 0.5 * (result + f);
343 for (
size_t k = 0; k <
size(); ++k)
348 for (
size_t k = 0; k <
size(); ++k)
349 result +=
dot(e2[k], e2[k]);
362 for (
size_t k = 0; k <
size(); k++)
363 d1 +=
E_.block<
ZDim, 3>(
ZDim * k, 0).transpose() *
e1[k];
369 for (
size_t k = 0; k <
size(); k++)
374 mutable Error2s
e1, e2;
383 typedef Eigen::Matrix<double, D, 1> DVector;
384 typedef Eigen::Map<DVector> DMap;
385 typedef Eigen::Map<const DVector> ConstDMap;
392 for (
size_t k = 0; k <
size(); ++k) {
400 for (
size_t k = 0; k <
size(); ++k) {
402 DMap(y +
D * key) +=
FBlocks_[k].transpose() * alpha * e2[k];
406 void multiplyHessianAdd(
double alpha,
const double* x,
double* y,
407 std::vector<size_t>
keys)
const {
421 for (
size_t k = 0; k <
size(); ++k)
427 for (
size_t k = 0; k <
size(); ++k) {
429 static const Vector
empty;
430 std::pair<VectorValues::iterator, bool> it = y.
tryInsert(key,
empty);
431 Vector& yi = it.first->second;
434 yi = Vector::Zero(
FBlocks_[k].cols());
435 yi +=
FBlocks_[k].transpose() * alpha * e2[k];
445 for (
size_t k = 0; k <
size(); ++k) {
446 static const Vector
empty;
448 std::pair<VectorValues::iterator, bool> it = y.
tryInsert(key,
empty);
449 Vector& yi = it.first->second;
461 for (
size_t k = 0; k <
size(); k++)
467 for (
size_t k = 0; k <
size(); ++k) {
482 typedef Eigen::Matrix<double, D, 1> DVector;
483 typedef Eigen::Map<DVector> DMap;
488 for (
size_t k = 0; k <
size(); k++)
492 for (
size_t k = 0; k <
size(); ++k) {
494 DMap(d +
D * j) += -
FBlocks_[k].transpose() * e2[k];
500 throw std::runtime_error(
501 "gradient for RegularImplicitSchurFactor is not implemented yet");
507template<
class CAMERA>
510template<
class CAMERA>
515 RegularImplicitSchurFactor<CAMERA> > {
Base class to create smart factors on poses or cameras.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition: types.h:106
double dot(const V1 &a, const V2 &b)
Dot product.
Definition: Vector.h:195
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:100
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
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
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
This class stores a dense matrix and allows it to be accessed as a collection of blocks.
Definition: SymmetricBlockMatrix.h:52
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
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:151
A set of cameras, all with their own calibration.
Definition: CameraSet.h:36
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
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition: Factor.h:140
KeyVector keys_
The keys involved in this factor.
Definition: Factor.h:85
bool empty() const
Whether the factor is empty (involves zero variables).
Definition: Factor.h:128
virtual void print(const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
print
Definition: Factor.cpp:29
KeyVector::const_iterator const_iterator
Const iterator over keys.
Definition: Factor.h:80
size_t size() const
Definition: Factor.h:157
An abstract virtual base class for JacobianFactor and HessianFactor.
Definition: GaussianFactor.h:39
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactor.h:42
VectorValues hessianDiagonal() const
Return the diagonal of the Hessian for this factor.
Definition: GaussianFactor.cpp:35
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition: VectorValues.h:74
iterator insert(const std::pair< Key, Vector > &key_value)
Insert a vector value with key j.
Definition: VectorValues.cpp:91
std::pair< VectorValues::iterator, bool > emplace(Key j, Args &&... args)
Emplace a vector value with key j.
Definition: VectorValues.h:185
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
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
RegularImplicitSchurFactor.
Definition: RegularImplicitSchurFactor.h:39
const Matrix E_
The 2m*3 E Jacobian with respect to the point.
Definition: RegularImplicitSchurFactor.h:60
void projectError(const Error2s &e1, Error2s &e2) const
Calculate corrected error Q*e = (I - E*P*E')*e.
Definition: RegularImplicitSchurFactor.h:357
GaussianFactor::shared_ptr clone() const override
Clone a factor (make a deep copy)
Definition: RegularImplicitSchurFactor.h:256
void hessianDiagonalAdd(VectorValues &d) const override
Add the diagonal of the Hessian for this factor to existing VectorValues.
Definition: RegularImplicitSchurFactor.h:174
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
RegularImplicitSchurFactor()
Constructor.
Definition: RegularImplicitSchurFactor.h:66
const Vector b_
2m-dimensional RHS vector
Definition: RegularImplicitSchurFactor.h:61
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
Eigen::Matrix< double, D, D > MatrixDD
camera Hessian
Definition: RegularImplicitSchurFactor.h:55
void gradientAtZero(double *d) const override
Calculate gradient, which is -F'Q*b, see paper - RAW MEMORY ACCESS.
Definition: RegularImplicitSchurFactor.h:479
static const int ZDim
Measurement dimension.
Definition: RegularImplicitSchurFactor.h:52
Matrix information() const override
Compute full information matrix
Definition: RegularImplicitSchurFactor.h:163
~RegularImplicitSchurFactor() override
Destructor.
Definition: RegularImplicitSchurFactor.h:85
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
FBlocks FBlocks_
All ZDim*D F blocks (one for each camera)
Definition: RegularImplicitSchurFactor.h:58
VectorValues gradientAtZero() const override
Calculate gradient, which is -F'Q*b, see paper.
Definition: RegularImplicitSchurFactor.h:457
Eigen::Matrix< double, ZDim, D > MatrixZD
type of an F block
Definition: RegularImplicitSchurFactor.h:54
const Matrix PointCovariance_
the 3*3 matrix P = inv(E'E) (2*2 if degenerate)
Definition: RegularImplicitSchurFactor.h:59
void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) const override
Hessian-vector multiply, i.e.
Definition: RegularImplicitSchurFactor.h:413
std::map< Key, Matrix > hessianBlockDiagonal() const override
Return the block diagonal of the Hessian for this factor.
Definition: RegularImplicitSchurFactor.h:231
Matrix augmentedInformation() const override
Compute full augmented information matrix
Definition: RegularImplicitSchurFactor.h:155
GaussianFactor::shared_ptr negate() const override
Construct the corresponding anti-factor to negate information stored stored in this factor.
Definition: RegularImplicitSchurFactor.h:263
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: RegularImplicitSchurFactor.h:43
Error2s e1
Scratch space for multiplyHessianAdd.
Definition: RegularImplicitSchurFactor.h:374
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: RegularImplicitSchurFactor.h:105
DenseIndex getDim(const_iterator variable) const override
Degrees of freedom of camera.
Definition: RegularImplicitSchurFactor.h:134
bool equals(const GaussianFactor &lf, double tol) const override
equals
Definition: RegularImplicitSchurFactor.h:118
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
RegularImplicitSchurFactor This
Typedef to this class.
Definition: RegularImplicitSchurFactor.h:42
static const int D
Camera dimension.
Definition: RegularImplicitSchurFactor.h:51
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
void multiplyHessianAdd(double alpha, const double *x, double *y) const
double* Hessian-vector multiply, i.e.
Definition: RegularImplicitSchurFactor.h:380
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
void multiplyHessianDummy(double alpha, const VectorValues &x, VectorValues &y) const
Dummy version to measure overhead of key access.
Definition: RegularImplicitSchurFactor.h:442
Vector gradient(Key key, const VectorValues &x) const override
Gradient wrt a key at any values.
Definition: RegularImplicitSchurFactor.h:499
The Factor::error simply extracts the.