A set of cameras, all with their own calibration. More...
#include <CameraSet.h>
Public Types | |
using | FBlocks = std::vector< MatrixZD, Eigen::aligned_allocator< MatrixZD > > |
using | MatrixZD = Eigen::Matrix< double, ZDim, D > |
Definitions for blocks of F. More... | |
Public Member Functions | |
Base ()=default | |
Base (const Base &)=delete | |
bool | equals (const CameraSet &p, double tol=1e-9) const |
equals More... | |
virtual void | print (const std::string &s="") const |
template<class POINT > | |
ZVector | project2 (const POINT &point, FBlocks *Fs=nullptr, Matrix *E=nullptr) const |
template<class POINT , class... OptArgs> | |
std::enable_if<(sizeof...(OptArgs) !=0), ZVector >::type | project2 (const POINT &point, OptArgs &... args) const |
template<class POINT > | |
Vector | reprojectionError (const POINT &point, const ZVector &measured, FBlocks *Fs=nullptr, Matrix *E=nullptr) const |
Calculate vector [project2(point)-z] of re-projection errors. More... | |
template<class POINT , class... OptArgs, typename = std::enable_if_t<sizeof...(OptArgs)!=0>> | |
Vector | reprojectionError (const POINT &point, const ZVector &measured, OptArgs &... args) const |
virtual | ~CameraSet ()=default |
Destructor. More... | |
Static Public Member Functions | |
template<int N> | |
static void | ComputePointCovariance (Eigen::Matrix< double, N, N > &P, const Matrix &E, double lambda, bool diagonalDamping=false) |
Computes Point Covariance P, with lambda parameter. More... | |
static Matrix | PointCov (const Matrix &E, const double lambda=0.0, bool diagonalDamping=false) |
Computes Point Covariance P, with lambda parameter, dynamic version. More... | |
static SymmetricBlockMatrix | SchurComplement (const FBlocks &Fblocks, const Matrix &E, const Vector &b, const double lambda=0.0, bool diagonalDamping=false) |
template<int N, int ND> | |
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) |
template<int N, int ND, int NDD> | |
static SymmetricBlockMatrix | SchurComplementAndRearrangeBlocks (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, const KeyVector &jacobianKeys, const KeyVector &hessianKeys) |
template<int N> | |
static void | UpdateSchurComplement (const FBlocks &Fs, const Matrix &E, const Eigen::Matrix< double, N, N > &P, const Vector &b, const KeyVector &allKeys, const KeyVector &keys, SymmetricBlockMatrix &augmentedHessian) |
Protected Types | |
using | Base = std::vector< CAMERA, typename Eigen::aligned_allocator< CAMERA > > |
typedef CAMERA::Measurement | Z |
typedef CAMERA::MeasurementVector | ZVector |
Static Protected Member Functions | |
static Vector | ErrorVector (const ZVector &predicted, const ZVector &measured) |
Make a vector of re-projection errors. More... | |
Static Protected Attributes | |
static const int | D = traits<CAMERA>::dimension |
Camera dimension. More... | |
static const int | ZDim = traits<Z>::dimension |
Measurement dimension. More... | |
A set of cameras, all with their own calibration.
Definition at line 36 of file CameraSet.h.
|
protected |
Definition at line 38 of file CameraSet.h.
using gtsam::CameraSet< CAMERA >::FBlocks = std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> > |
Definition at line 78 of file CameraSet.h.
using gtsam::CameraSet< CAMERA >::MatrixZD = Eigen::Matrix<double, ZDim, D> |
Definitions for blocks of F.
Definition at line 77 of file CameraSet.h.
|
protected |
2D measurement and noise model for each of the m views The order is kept the same as the keys that we use to create the factor.
Definition at line 44 of file CameraSet.h.
|
protected |
Definition at line 45 of file CameraSet.h.
|
virtualdefault |
Destructor.
|
default |
|
delete |
|
inlinestatic |
Computes Point Covariance P, with lambda parameter.
Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix G = F' * F - F' * E * P * E' * F g = F' * (b - E * P * E' * b) Fixed size version
Definition at line 343 of file CameraSet.h.
|
inline |
equals
Definition at line 91 of file CameraSet.h.
|
inlinestaticprotected |
Make a vector of re-projection errors.
Definition at line 51 of file CameraSet.h.
|
inlinestatic |
Computes Point Covariance P, with lambda parameter, dynamic version.
Definition at line 359 of file CameraSet.h.
|
inlinevirtual |
s | optional string naming the factor |
keyFormatter | optional formatter useful for printing Symbols |
Reimplemented in gtsam::PinholeSet< CAMERA >.
Definition at line 85 of file CameraSet.h.
|
inline |
Project a point (possibly Unit3 at infinity), with derivatives Note that F is a sparse block-diagonal matrix, so instead of a large dense matrix this function returns the diagonal blocks. throws CheiralityException
Definition at line 108 of file CameraSet.h.
|
inline |
An overload of the project2 function to accept full matrices and vectors and pass it to the pointer version of the function.
Use SFINAE to resolve overload ambiguity.
Definition at line 141 of file CameraSet.h.
|
inline |
Calculate vector [project2(point)-z] of re-projection errors.
Definition at line 149 of file CameraSet.h.
|
inline |
An overload o the reprojectionError function to accept full matrices and vectors and pass it to the pointer version of the function
Definition at line 160 of file CameraSet.h.
|
inlinestatic |
Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix Dynamic version
Definition at line 376 of file CameraSet.h.
|
inlinestatic |
Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix G = F' * F - F' * E * P * E' * F g = F' * (b - E * P * E' * b) Fixed size version
Definition at line 174 of file CameraSet.h.
|
inlinestatic |
Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix G = F' * F - F' * E * P * E' * F g = F' * (b - E * P * E' * b) In this version, we allow for the case where the keys in the Jacobian are organized differently from the keys in the output SymmetricBlockMatrix In particular: each diagonal block of the Jacobian F captures 2 poses (useful for rolling shutter and extrinsic calibration) such that F keeps the block structure that makes the Schur complement trick fast.
N = 2 or 3 (point dimension), ND is the Jacobian block dimension, NDD is the Hessian block dimension
Definition at line 242 of file CameraSet.h.
|
inlinestatic |
Applies Schur complement (exploiting block structure) to get a smart factor on cameras, and adds the contribution of the smart factor to a pre-allocated augmented Hessian.
Definition at line 397 of file CameraSet.h.
|
staticprotected |
Camera dimension.
Definition at line 47 of file CameraSet.h.
|
staticprotected |
Measurement dimension.
Definition at line 48 of file CameraSet.h.