Transfers points between views using essential matrices with a shared calibration. More...
#include <TransferFactor.h>
Public Types | |
using | Base = TransferFactor< EM > |
using | shared_ptr = std::shared_ptr< This > |
using | This = EssentialTransferFactor< K > |
![]() | |
using | Base = NoiseModelFactorN< EssentialMatrix, EssentialMatrix > |
using | Triplet = std::tuple< Point2, Point2, Point2 > |
![]() | |
using | ValueType = typename std::tuple_element< I - 1, std::tuple< ValueTypes... > >::type |
![]() | |
typedef std::shared_ptr< This > | shared_ptr |
![]() | |
typedef std::shared_ptr< This > | shared_ptr |
![]() | |
typedef KeyVector::const_iterator | const_iterator |
Const iterator over keys. More... | |
typedef KeyVector::iterator | iterator |
Iterator over keys. More... | |
Public Member Functions | |
EssentialTransferFactor (EdgeKey edge1, EdgeKey edge2, const std::vector< Triplet > &triplets, const std::shared_ptr< K > &calibration, const SharedNoiseModel &model=nullptr) | |
Constructor that accepts a vector of point triplets and a shared calibration. More... | |
Vector | evaluateError (const EM &E1, const EM &E2, OptionalMatrixType H1=nullptr, OptionalMatrixType H2=nullptr) const override |
Evaluate error function. More... | |
Vector2 | TransferError (const Matrix3 &Eca, const Point2 &pa, const Matrix3 &Ecb, const Point2 &pb, const Point2 &pc) const |
Transfer points pa and pb to view c and evaluate error. More... | |
![]() | |
Vector | evaluateError (const EssentialMatrix &F1, const EssentialMatrix &F2, OptionalMatrixType H1=nullptr, OptionalMatrixType H2=nullptr) const override |
Vector of errors returns 2*N vector. More... | |
TransferFactor (EdgeKey edge1, EdgeKey edge2, const std::vector< Triplet > &triplets, const SharedNoiseModel &model=nullptr) | |
Constructor that accepts a vector of point triplets. More... | |
![]() | |
Key | key () const |
virtual Vector | unwhitenedError (const Values &x, OptionalMatrixVecType H=nullptr) const =0 |
Vector | unwhitenedError (const Values &x, std::vector< Matrix > &H) const |
~NoiseModelFactorN () override | |
NoiseModelFactorN () | |
Default Constructor for I/O. More... | |
NoiseModelFactorN (const SharedNoiseModel &noiseModel, KeyType< ValueTypes >... keys) | |
NoiseModelFactorN (const SharedNoiseModel &noiseModel, CONTAINER keys) | |
Vector | unwhitenedError (const Values &x, OptionalMatrixVecType H=nullptr) const override |
virtual Vector | evaluateError (const ValueTypes &... x, OptionalMatrixTypeT< ValueTypes >... H) const=0 |
Vector | evaluateError (const ValueTypes &... x, MatrixTypeT< ValueTypes > &... H) const |
Vector | evaluateError (const ValueTypes &... x) const |
AreAllMatrixRefs< Vector, OptionalJacArgs... > | evaluateError (const ValueTypes &... x, OptionalJacArgs &&... H) const |
AreAllMatrixPtrs< Vector, OptionalJacArgs... > | evaluateError (const ValueTypes &... x, OptionalJacArgs &&... H) const |
Key | key1 () const |
Key | key2 () const |
Key | key3 () const |
Key | key4 () const |
Key | key5 () const |
Key | key6 () const |
![]() | |
shared_ptr | cloneWithNewNoiseModel (const SharedNoiseModel newNoise) const |
size_t | dim () const override |
bool | equals (const NonlinearFactor &f, double tol=1e-9) const override |
double | error (const HybridValues &c) const override |
virtual double | error (const Values &c) const |
double | error (const Values &c) const override |
std::shared_ptr< GaussianFactor > | linearize (const Values &x) const override |
const SharedNoiseModel & | noiseModel () const |
access to the noise model More... | |
NoiseModelFactor () | |
template<typename CONTAINER > | |
NoiseModelFactor (const SharedNoiseModel &noiseModel, const CONTAINER &keys) | |
void | print (const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override |
Vector | unweightedWhitenedError (const Values &c) const |
Vector | unwhitenedError (const Values &x, std::vector< Matrix > &H) const |
double | weight (const Values &c) const |
Vector | whitenedError (const Values &c) const |
~NoiseModelFactor () override | |
![]() | |
NonlinearFactor () | |
template<typename CONTAINER > | |
NonlinearFactor (const CONTAINER &keys) | |
double | error (const HybridValues &c) const override |
virtual bool | active (const Values &) const |
virtual shared_ptr | clone () const |
virtual shared_ptr | rekey (const std::map< Key, Key > &rekey_mapping) const |
virtual shared_ptr | rekey (const KeyVector &new_keys) const |
virtual bool | sendable () const |
![]() | |
virtual | ~Factor ()=default |
Default destructor. More... | |
bool | empty () const |
Whether the factor is empty (involves zero variables). More... | |
Key | front () const |
First key. More... | |
Key | back () const |
Last key. More... | |
const_iterator | find (Key key) const |
find More... | |
const KeyVector & | keys () const |
Access the factor's involved variable keys. More... | |
const_iterator | begin () const |
const_iterator | end () const |
size_t | size () const |
virtual void | printKeys (const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const |
print only keys More... | |
bool | equals (const This &other, double tol=1e-9) const |
check equality More... | |
KeyVector & | keys () |
iterator | begin () |
iterator | end () |
![]() | |
std::pair< Matrix3, Matrix3 > | getMatrices (const EssentialMatrix &F1, const EssentialMatrix &F2) const |
Create Matrix3 objects based on EdgeKey configurations. More... | |
TransferEdges (EdgeKey edge1, EdgeKey edge2) | |
Private Types | |
using | EM = EssentialMatrix |
using | Triplet = std::tuple< Point2, Point2, Point2 > |
Private Attributes | |
std::shared_ptr< K > | calibration_ |
Shared pointer to calibration object. More... | |
Additional Inherited Members | |
![]() | |
static size_t | ViewA (const EdgeKey &edge1, const EdgeKey &edge2) |
Returns the view A index based on the EdgeKeys. More... | |
static size_t | ViewB (const EdgeKey &edge1, const EdgeKey &edge2) |
Returns the view B index based on the EdgeKeys. More... | |
static size_t | ViewC (const EdgeKey &edge1, const EdgeKey &edge2) |
Returns the view C index based on the EdgeKeys. More... | |
![]() | |
constexpr static auto | N |
N is the number of variables (N-way factor) More... | |
![]() | |
using | Base = NoiseModelFactor |
using | KeyType = Key |
using | MatrixTypeT = Matrix |
using | OptionalMatrixTypeT = Matrix * |
using | This = NoiseModelFactorN< ValueTypes... > |
using | IsConvertible = typename std::enable_if< std::is_convertible< From, To >::value, void >::type |
using | IndexIsValid = typename std::enable_if<(I >=1) &&(I<=N), void >::type |
using | ContainerElementType = typename std::decay< decltype(*std::declval< Container >().begin())>::type |
using | IsContainerOfKeys = IsConvertible< ContainerElementType< Container >, Key > |
using | AreAllMatrixRefs = std::enable_if_t<(... &&std::is_convertible< Args, Matrix & >::value), Ret > |
using | IsMatrixPointer = std::is_same< typename std::decay_t< Arg >, Matrix * > |
using | IsNullpointer = std::is_same< typename std::decay_t< Arg >, std::nullptr_t > |
using | AreAllMatrixPtrs = std::enable_if_t<(... &&(IsMatrixPointer< Args >::value||IsNullpointer< Args >::value)), Ret > |
![]() | |
typedef NonlinearFactor | Base |
typedef NoiseModelFactor | This |
![]() | |
typedef Factor | Base |
typedef NonlinearFactor | This |
![]() | |
NoiseModelFactor (const SharedNoiseModel &noiseModel) | |
![]() | |
Factor () | |
template<typename CONTAINER > | |
Factor (const CONTAINER &keys) | |
template<typename ITERATOR > | |
Factor (ITERATOR first, ITERATOR last) | |
![]() | |
template<typename CONTAINER > | |
static Factor | FromKeys (const CONTAINER &keys) |
template<typename ITERATOR > | |
static Factor | FromIterators (ITERATOR first, ITERATOR last) |
![]() | |
std::vector< Triplet > | triplets_ |
Point triplets. More... | |
![]() | |
SharedNoiseModel | noiseModel_ |
![]() | |
KeyVector | keys_ |
The keys involved in this factor. More... | |
![]() | |
uint32_t | c_ |
The transfer target. More... | |
EdgeKey | edge1_ |
EdgeKey | edge2_ |
The two EdgeKeys. More... | |
Transfers points between views using essential matrices with a shared calibration.
This factor is templated on the calibration class K and extends the TransferFactor for EssentialMatrices. It involves two essential matrices and a shared calibration object (K). The evaluateError function calibrates the image points, calls the base class's transfer method, and computes the error using bulk numerical differentiation.
Definition at line 146 of file TransferFactor.h.
using gtsam::EssentialTransferFactor< K >::Base = TransferFactor<EM> |
Definition at line 152 of file TransferFactor.h.
|
private |
Definition at line 147 of file TransferFactor.h.
using gtsam::EssentialTransferFactor< K >::shared_ptr = std::shared_ptr<This> |
Definition at line 154 of file TransferFactor.h.
using gtsam::EssentialTransferFactor< K >::This = EssentialTransferFactor<K> |
Definition at line 153 of file TransferFactor.h.
|
private |
Definition at line 148 of file TransferFactor.h.
|
inline |
Constructor that accepts a vector of point triplets and a shared calibration.
edge1 | First EdgeKey specifying E1: (a, c) or (c, a) |
edge2 | Second EdgeKey specifying E2: (b, c) or (c, b) |
triplets | A vector of triplets containing (pa, pb, pc) |
calibration | Shared pointer to calibration object |
model | An optional SharedNoiseModel |
Definition at line 166 of file TransferFactor.h.
|
inlineoverride |
Evaluate error function.
Definition at line 183 of file TransferFactor.h.
|
inline |
Transfer points pa and pb to view c and evaluate error.
Definition at line 173 of file TransferFactor.h.
|
private |
Shared pointer to calibration object.
Definition at line 149 of file TransferFactor.h.