Go to the documentation of this file.
38 template<
class CAMERA>
56 typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >
FBlocks;
107 std::cout <<
" RegularImplicitSchurFactor " << std::endl;
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) {
123 if (
keys_[k] !=
f->keys_[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");
165 int m = this->
keys_.size();
167 return augmented.block(0, 0,
M,
M);
176 for (
size_t k = 0; k <
size(); ++k) {
186 for (
int k = 0; k <
D; ++k) {
188 dj(k) = Fj.col(k).squaredNorm();
196 result.first->second += dj;
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;
244 blocks[
j] = Fj.transpose()
257 return std::make_shared<RegularImplicitSchurFactor<CAMERA> >(
keys_,
259 throw std::runtime_error(
260 "RegularImplicitSchurFactor::clone non implemented");
264 return std::make_shared<RegularImplicitSchurFactor<CAMERA> >(
keys_,
266 throw std::runtime_error(
267 "RegularImplicitSchurFactor::negate non implemented");
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)
329 double f =
b_.squaredNorm();
343 for (
size_t k = 0; k <
size(); ++k)
348 for (
size_t k = 0; k <
size(); ++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++)
392 for (
size_t k = 0; k <
size(); ++k) {
400 for (
size_t k = 0; k <
size(); ++k) {
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) {
430 std::pair<VectorValues::iterator, bool> it =
y.tryInsert(
key,
empty);
431 Vector& yi = it.first->second;
445 for (
size_t k = 0; k <
size(); ++k) {
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) {
488 for (
size_t k = 0; k <
size(); k++)
492 for (
size_t k = 0; k <
size(); ++k) {
500 throw std::runtime_error(
501 "gradient for RegularImplicitSchurFactor is not implemented yet");
507 template<
class CAMERA>
510 template<
class CAMERA>
515 RegularImplicitSchurFactor<CAMERA> > {
void updateHessian(const KeyVector &keys, SymmetricBlockMatrix *info) const override
GaussianFactor::shared_ptr negate() const override
DenseIndex getDim(const_iterator variable) const override
Degrees of freedom of camera.
const Vector b_
2m-dimensional RHS vector
Base class to create smart factors on poses or cameras.
double errorJF(const VectorValues &x) const
static const double d[K][N]
void multiplyHessianAdd(double alpha, const double *x, double *y) const
double* Hessian-vector multiply, i.e. y += F'alpha(I - E*P*E')*F*x RAW memory access!...
GaussianFactor::shared_ptr clone() const override
void multiplyHessianAdd(double alpha, const double *x, double *y, std::vector< size_t > keys) const
void hessianDiagonal(double *d) const override
add the contribution of this factor to the diagonal of the hessian d(output) = d(input) + deltaHessia...
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
void multiplyHessianDummy(double alpha, const VectorValues &x, VectorValues &y) const
Dummy version to measure overhead of key access.
void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) const override
Hessian-vector multiply, i.e. y += F'alpha(I - E*P*E')*F*x.
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
bool empty() const
Whether the factor is empty (involves zero variables).
Eigen::SelfAdjointView< constBlock, Eigen::Upper > selfadjointView(DenseIndex I, DenseIndex J) const
Return the square sub-matrix that contains blocks(i:j, i:j).
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
RegularImplicitSchurFactor()
Constructor.
RegularImplicitSchurFactor This
Typedef to this class.
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
A set of cameras, all with their own calibration.
static void multiplyHessianAdd(const Matrix &F, const Matrix &E, const Matrix &PointCovariance, double alpha, const Vector &x, Vector &y)
Eigen::Matrix< double, ZDim, D > MatrixZD
type of an F block
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
VectorValues hessianDiagonal() const
Return the diagonal of the Hessian for this factor.
std::vector< Vector2, Eigen::aligned_allocator< Vector2 > > Error2s
void hessianDiagonalAdd(VectorValues &d) const override
Add the diagonal of the Hessian for this factor to existing VectorValues.
GenericMeasurement< Point2, Point2 > Measurement
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)
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)
double error(const VectorValues &x) const override
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
const Matrix E_
The 2m*3 E Jacobian with respect to the point.
Error2s e1
Scratch space for multiplyHessianAdd.
Eigen::Matrix< double, D, D > MatrixDD
camera Hessian
std::shared_ptr< This > shared_ptr
shared_ptr to this class
double dot(const V1 &a, const V2 &b)
std::shared_ptr< This > shared_ptr
shared_ptr to this class
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.
void projectError2(const Error2s &e1, Error2s &e2) const
Calculate corrected error Q*(e-ZDim*b) = (I - E*P*E')*(e-ZDim*b)
void gradientAtZero(double *d) const override
const FBlocks & Fs() const
Vector gradient(Key key, const VectorValues &x) const override
Gradient wrt a key at any values.
A matrix or vector expression mapping an existing array of data.
Matrix augmentedJacobian() const override
void projectError(const Error2s &e1, Error2s &e2) const
Calculate corrected error Q*e = (I - E*P*E')*e.
void g(const string &key, int i)
const gtsam::Symbol key('X', 0)
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
static const int ZDim
Measurement dimension.
KeyVector::const_iterator const_iterator
Const iterator over keys.
KeyVector keys_
The keys involved in this factor.
std::vector< MatrixZD, Eigen::aligned_allocator< MatrixZD > > FBlocks
std::map< Key, Matrix > hessianBlockDiagonal() const override
Return the block diagonal of the Hessian for this factor.
bool equals(const GaussianFactor &lf, double tol) const override
equals
const KeyVector & keys() const
Access the factor's involved variable keys.
ptrdiff_t DenseIndex
The index type for Eigen objects.
~RegularImplicitSchurFactor() override
Destructor.
VectorValues gradientAtZero() const override
virtual void print(const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
print
Matrix augmentedInformation() const override
Compute full augmented information matrix
std::pair< Matrix, Vector > jacobian() const override
The matrix class, also used for vectors and row-vectors.
Matrix information() const override
Compute full information matrix
std::uint64_t Key
Integer nonlinear key type.
static const int D
Camera dimension.
const Matrix & getPointCovariance() const
Matrix< RealScalar, Dynamic, Dynamic > M
gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:34:56