Static Public Member Functions | Private Attributes | List of all members
gtsam::EssentialMatrix Class Reference

#include <EssentialMatrix.h>

Public Member Functions

Testable
GTSAM_EXPORT void print (const std::string &s="") const
 print with optional string More...
 
bool equals (const EssentialMatrix &other, double tol=1e-8) const
 assert equality up to a tolerance More...
 

Static Public Member Functions

static Vector3 Homogeneous (const Point2 &p)
 Static function to convert Point2 to homogeneous coordinates. More...
 

Private Attributes

Matrix3 E_
 Essential matrix. More...
 
Rot3 R_
 Rotation. More...
 
Unit3 t_
 Translation. More...
 

Friends

Streaming operators
GTSAM_EXPORT friend std::ostream & operator<< (std::ostream &os, const EssentialMatrix &E)
 stream to stream More...
 
GTSAM_EXPORT friend std::istream & operator>> (std::istream &is, EssentialMatrix &E)
 stream from stream More...
 

Constructors and named constructors

static GTSAM_EXPORT EssentialMatrix FromRotationAndDirection (const Rot3 &aRb, const Unit3 &aTb, OptionalJacobian< 5, 3 > H1=boost::none, OptionalJacobian< 5, 2 > H2=boost::none)
 Named constructor with derivatives. More...
 
static GTSAM_EXPORT EssentialMatrix FromPose3 (const Pose3 &_1P2_, OptionalJacobian< 5, 6 > H=boost::none)
 Named constructor converting a Pose3 with scale to EssentialMatrix (no scale) More...
 
template<typename Engine >
static EssentialMatrix Random (Engine &rng)
 Random, using Rot3::Random and Unit3::Random. More...
 
 EssentialMatrix ()
 Default constructor. More...
 
 EssentialMatrix (const Rot3 &aRb, const Unit3 &aTb)
 Construct from rotation and translation. More...
 
virtual ~EssentialMatrix ()
 

Manifold

enum  { dimension = 5 }
 
typedef OptionalJacobian< dimension, dimensionChartJacobian
 
static size_t Dim ()
 
size_t dim () const
 
EssentialMatrix retract (const Vector5 &xi) const
 Retract delta to manifold. More...
 
Vector5 localCoordinates (const EssentialMatrix &other) const
 Compute the coordinates in the tangent space. More...
 

Essential matrix methods

EssentialMatrix operator* (const Rot3 &cRb, const EssentialMatrix &E)
 
const Rot3rotation () const
 Rotation. More...
 
const Unit3direction () const
 Direction. More...
 
const Matrix3 & matrix () const
 Return 3*3 matrix representation. More...
 
const Unit3epipole_a () const
 Return epipole in image_a , as Unit3 to allow for infinity. More...
 
Unit3 epipole_b () const
 Return epipole in image_b, as Unit3 to allow for infinity. More...
 
GTSAM_EXPORT Point3 transformTo (const Point3 &p, OptionalJacobian< 3, 5 > DE=boost::none, OptionalJacobian< 3, 3 > Dpoint=boost::none) const
 takes point in world coordinates and transforms it to pose with |t|==1 More...
 
GTSAM_EXPORT EssentialMatrix rotate (const Rot3 &cRb, OptionalJacobian< 5, 5 > HE=boost::none, OptionalJacobian< 5, 3 > HR=boost::none) const
 
GTSAM_EXPORT double error (const Vector3 &vA, const Vector3 &vB, OptionalJacobian< 1, 5 > H=boost::none) const
 epipolar error, algebraic More...
 

Advanced Interface

class boost::serialization::access
 
template<class ARCHIVE >
void serialize (ARCHIVE &ar, const unsigned int)
 

Detailed Description

An essential matrix is like a Pose3, except with translation up to scale It is named after the 3*3 matrix aEb = [aTb]x aRb from computer vision, but here we choose instead to parameterize it as a (Rot3,Unit3) pair. We can then non-linearly optimize immediately on this 5-dimensional manifold.

Definition at line 26 of file EssentialMatrix.h.

Member Typedef Documentation

Definition at line 89 of file EssentialMatrix.h.

Member Enumeration Documentation

anonymous enum
Enumerator
dimension 

Definition at line 85 of file EssentialMatrix.h.

Constructor & Destructor Documentation

gtsam::EssentialMatrix::EssentialMatrix ( )
inline

Default constructor.

Definition at line 42 of file EssentialMatrix.h.

gtsam::EssentialMatrix::EssentialMatrix ( const Rot3 aRb,
const Unit3 aTb 
)
inline

Construct from rotation and translation.

Definition at line 46 of file EssentialMatrix.h.

virtual gtsam::EssentialMatrix::~EssentialMatrix ( )
inlinevirtual

Definition at line 65 of file EssentialMatrix.h.

Member Function Documentation

static size_t gtsam::EssentialMatrix::Dim ( )
inlinestatic

Definition at line 86 of file EssentialMatrix.h.

size_t gtsam::EssentialMatrix::dim ( ) const
inline

Definition at line 87 of file EssentialMatrix.h.

const Unit3& gtsam::EssentialMatrix::direction ( void  ) const
inline

Direction.

Definition at line 115 of file EssentialMatrix.h.

const Unit3& gtsam::EssentialMatrix::epipole_a ( ) const
inline

Return epipole in image_a , as Unit3 to allow for infinity.

Definition at line 125 of file EssentialMatrix.h.

Unit3 gtsam::EssentialMatrix::epipole_b ( ) const
inline

Return epipole in image_b, as Unit3 to allow for infinity.

Definition at line 130 of file EssentialMatrix.h.

bool gtsam::EssentialMatrix::equals ( const EssentialMatrix other,
double  tol = 1e-8 
) const
inline

assert equality up to a tolerance

Definition at line 76 of file EssentialMatrix.h.

double gtsam::EssentialMatrix::error ( const Vector3 vA,
const Vector3 vB,
OptionalJacobian< 1, 5 >  H = boost::none 
) const

epipolar error, algebraic

Definition at line 104 of file EssentialMatrix.cpp.

EssentialMatrix gtsam::EssentialMatrix::FromPose3 ( const Pose3 _1P2_,
OptionalJacobian< 5, 6 >  H = boost::none 
)
static

Named constructor converting a Pose3 with scale to EssentialMatrix (no scale)

Definition at line 27 of file EssentialMatrix.cpp.

EssentialMatrix gtsam::EssentialMatrix::FromRotationAndDirection ( const Rot3 aRb,
const Unit3 aTb,
OptionalJacobian< 5, 3 >  H1 = boost::none,
OptionalJacobian< 5, 2 >  H2 = boost::none 
)
static

Named constructor with derivatives.

Definition at line 16 of file EssentialMatrix.cpp.

static Vector3 gtsam::EssentialMatrix::Homogeneous ( const Point2 p)
inlinestatic

Static function to convert Point2 to homogeneous coordinates.

Definition at line 34 of file EssentialMatrix.h.

Vector5 gtsam::EssentialMatrix::localCoordinates ( const EssentialMatrix other) const
inline

Compute the coordinates in the tangent space.

Definition at line 97 of file EssentialMatrix.h.

const Matrix3& gtsam::EssentialMatrix::matrix ( ) const
inline

Return 3*3 matrix representation.

Definition at line 120 of file EssentialMatrix.h.

void gtsam::EssentialMatrix::print ( const std::string &  s = "") const

print with optional string

Definition at line 48 of file EssentialMatrix.cpp.

template<typename Engine >
static EssentialMatrix gtsam::EssentialMatrix::Random ( Engine &  rng)
inlinestatic

Random, using Rot3::Random and Unit3::Random.

Definition at line 61 of file EssentialMatrix.h.

EssentialMatrix gtsam::EssentialMatrix::retract ( const Vector5 &  xi) const
inline

Retract delta to manifold.

Definition at line 92 of file EssentialMatrix.h.

EssentialMatrix gtsam::EssentialMatrix::rotate ( const Rot3 cRb,
OptionalJacobian< 5, 5 >  HE = boost::none,
OptionalJacobian< 5, 3 >  HR = boost::none 
) const

Given essential matrix E in camera frame B, convert to body frame C

Parameters
cRbrotation from body frame to camera frame
Eessential matrix E in camera frame C

Definition at line 73 of file EssentialMatrix.cpp.

const Rot3& gtsam::EssentialMatrix::rotation ( ) const
inline

Rotation.

Definition at line 110 of file EssentialMatrix.h.

template<class ARCHIVE >
void gtsam::EssentialMatrix::serialize ( ARCHIVE &  ar,
const unsigned  int 
)
inlineprivate

Definition at line 186 of file EssentialMatrix.h.

Point3 gtsam::EssentialMatrix::transformTo ( const Point3 p,
OptionalJacobian< 3, 5 >  DE = boost::none,
OptionalJacobian< 3, 3 >  Dpoint = boost::none 
) const

takes point in world coordinates and transforms it to pose with |t|==1

Parameters
ppoint in world coordinates
DEoptional 3*5 Jacobian wrpt to E
Dpointoptional 3*3 Jacobian wrpt point
Returns
point in pose coordinates

Definition at line 55 of file EssentialMatrix.cpp.

Friends And Related Function Documentation

friend class boost::serialization::access
friend

Serialization function

Definition at line 184 of file EssentialMatrix.h.

EssentialMatrix operator* ( const Rot3 cRb,
const EssentialMatrix E 
)
friend

Given essential matrix E in camera frame B, convert to body frame C

Parameters
cRbrotation from body frame to camera frame
Eessential matrix E in camera frame C

Definition at line 158 of file EssentialMatrix.h.

GTSAM_EXPORT friend std::ostream& operator<< ( std::ostream &  os,
const EssentialMatrix E 
)
friend

stream to stream

GTSAM_EXPORT friend std::istream& operator>> ( std::istream &  is,
EssentialMatrix E 
)
friend

stream from stream

Member Data Documentation

Matrix3 gtsam::EssentialMatrix::E_
private

Essential matrix.

Definition at line 30 of file EssentialMatrix.h.

Rot3 gtsam::EssentialMatrix::R_
private

Rotation.

Definition at line 28 of file EssentialMatrix.h.

Unit3 gtsam::EssentialMatrix::t_
private

Translation.

Definition at line 29 of file EssentialMatrix.h.


The documentation for this class was generated from the following files:


gtsam
Author(s):
autogenerated on Sat May 8 2021 02:58:08