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 37 of file CameraSet.h.
|
protected |
Definition at line 39 of file CameraSet.h.
using gtsam::CameraSet< CAMERA >::FBlocks = std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> > |
Definition at line 79 of file CameraSet.h.
using gtsam::CameraSet< CAMERA >::MatrixZD = Eigen::Matrix<double, ZDim, D> |
Definitions for blocks of F.
Definition at line 78 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 45 of file CameraSet.h.
|
protected |
Definition at line 46 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 344 of file CameraSet.h.
|
inline |
equals
Definition at line 92 of file CameraSet.h.
|
inlinestaticprotected |
Make a vector of re-projection errors.
Definition at line 52 of file CameraSet.h.
|
inlinestatic |
Computes Point Covariance P, with lambda parameter, dynamic version.
Definition at line 360 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 86 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 109 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 142 of file CameraSet.h.
|
inline |
Calculate vector [project2(point)-z] of re-projection errors.
Definition at line 150 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 161 of file CameraSet.h.
|
inlinestatic |
Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix Dynamic version
Definition at line 377 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 175 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 243 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 398 of file CameraSet.h.
|
staticprotected |
Camera dimension.
Definition at line 48 of file CameraSet.h.
|
staticprotected |
Measurement dimension.
Definition at line 49 of file CameraSet.h.