Loading...
Searching...
No Matches
EssentialMatrix.h
/*
* @file EssentialMatrix.h
* @brief EssentialMatrix class
* @author Frank Dellaert
* @date December 17, 2013
*/
#pragma once
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Unit3.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/base/Manifold.h>
#include <iosfwd>
#include <string>
namespace gtsam {
class EssentialMatrix {
private:
Rot3 R_;
Unit3 t_;
Matrix3 E_;
public:
static Vector3 Homogeneous(const Point2& p) {
return Vector3(p.x(), p.y(), 1);
}
EssentialMatrix() :E_(t_.skew()) {
}
EssentialMatrix(const Rot3& aRb, const Unit3& aTb) :
R_(aRb), t_(aTb), E_(t_.skew() * R_.matrix()) {
}
GTSAM_EXPORT static EssentialMatrix FromRotationAndDirection(const Rot3& aRb, const Unit3& aTb,
OptionalJacobian<5, 3> H1 = boost::none,
OptionalJacobian<5, 2> H2 = boost::none);
GTSAM_EXPORT static EssentialMatrix FromPose3(const Pose3& _1P2_,
OptionalJacobian<5, 6> H = boost::none);
template<typename Engine>
static EssentialMatrix Random(Engine & rng) {
return EssentialMatrix(Rot3::Random(rng), Unit3::Random(rng));
}
virtual ~EssentialMatrix() {}
GTSAM_EXPORT void print(const std::string& s = "") const;
bool equals(const EssentialMatrix& other, double tol = 1e-8) const {
return R_.equals(other.R_, tol)
&& t_.equals(other.t_, tol);
}
enum { dimension = 5 };
inline static size_t Dim() { return dimension;}
inline size_t dim() const { return dimension;}
typedef OptionalJacobian<dimension, dimension> ChartJacobian;
EssentialMatrix retract(const Vector5& xi) const {
return EssentialMatrix(R_.retract(xi.head<3>()), t_.retract(xi.tail<2>()));
}
Vector5 localCoordinates(const EssentialMatrix& other) const {
auto v1 = R_.localCoordinates(other.R_);
auto v2 = t_.localCoordinates(other.t_);
Vector5 v;
v << v1, v2;
return v;
}
inline const Rot3& rotation() const {
return R_;
}
inline const Unit3& direction() const {
return t_;
}
inline const Matrix3& matrix() const {
return E_;
}
inline const Unit3& epipole_a() const {
return t_;
}
inline Unit3 epipole_b() const {
return R_.unrotate(t_);
}
GTSAM_EXPORT Point3 transformTo(const Point3& p,
OptionalJacobian<3, 5> DE = boost::none,
OptionalJacobian<3, 3> Dpoint = boost::none) const;
GTSAM_EXPORT EssentialMatrix rotate(const Rot3& cRb, OptionalJacobian<5, 5> HE =
boost::none, OptionalJacobian<5, 3> HR = boost::none) const;
friend EssentialMatrix operator*(const Rot3& cRb, const EssentialMatrix& E) {
return E.rotate(cRb);
}
GTSAM_EXPORT double error(const Vector3& vA, const Vector3& vB,
OptionalJacobian<1, 5> H = boost::none) const;
GTSAM_EXPORT friend std::ostream& operator <<(std::ostream& os, const EssentialMatrix& E);
GTSAM_EXPORT friend std::istream& operator >>(std::istream& is, EssentialMatrix& E);
private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_NVP(R_);
ar & BOOST_SERIALIZATION_NVP(t_);
ar & boost::serialization::make_nvp("E11", E_(0, 0));
ar & boost::serialization::make_nvp("E12", E_(0, 1));
ar & boost::serialization::make_nvp("E13", E_(0, 2));
ar & boost::serialization::make_nvp("E21", E_(1, 0));
ar & boost::serialization::make_nvp("E22", E_(1, 1));
ar & boost::serialization::make_nvp("E23", E_(1, 2));
ar & boost::serialization::make_nvp("E31", E_(2, 0));
ar & boost::serialization::make_nvp("E32", E_(2, 1));
ar & boost::serialization::make_nvp("E33", E_(2, 2));
}
public:
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
template<>
struct traits<EssentialMatrix> : public internal::Manifold<EssentialMatrix> {};
template<>
struct traits<const EssentialMatrix> : public internal::Manifold<EssentialMatrix> {};
} // namespace gtsam
Manifold.h
Base class and basic functions for Manifold types.
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
Point2.h
2D Point
Pose3.h
3D Pose
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::Point2
Vector2 Point2
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2...
Definition Point2.h:27
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition Point3.h:36
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
gtsam::internal::Manifold
Both ManifoldTraits and Testable.
Definition Manifold.h:120
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
gtsam::EssentialMatrix
An essential matrix is like a Pose3, except with translation up to scale It is named after the 3*3 ma...
Definition EssentialMatrix.h:26
gtsam::EssentialMatrix::operator*
friend EssentialMatrix operator*(const Rot3 &cRb, const EssentialMatrix &E)
Given essential matrix E in camera frame B, convert to body frame C.
Definition EssentialMatrix.h:158
gtsam::EssentialMatrix::rotation
const Rot3 & rotation() const
Rotation.
Definition EssentialMatrix.h:110
gtsam::EssentialMatrix::localCoordinates
Vector5 localCoordinates(const EssentialMatrix &other) const
Compute the coordinates in the tangent space.
Definition EssentialMatrix.h:97
gtsam::EssentialMatrix::equals
bool equals(const EssentialMatrix &other, double tol=1e-8) const
assert equality up to a tolerance
Definition EssentialMatrix.h:76
gtsam::EssentialMatrix::EssentialMatrix
EssentialMatrix()
Default constructor.
Definition EssentialMatrix.h:42
gtsam::EssentialMatrix::FromPose3
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).
Definition EssentialMatrix.cpp:27
gtsam::EssentialMatrix::operator>>
GTSAM_EXPORT friend std::istream & operator>>(std::istream &is, EssentialMatrix &E)
stream from stream
Definition EssentialMatrix.cpp:126
gtsam::EssentialMatrix::Random
static EssentialMatrix Random(Engine &rng)
Random, using Rot3::Random and Unit3::Random.
Definition EssentialMatrix.h:61
gtsam::EssentialMatrix::FromRotationAndDirection
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.
Definition EssentialMatrix.cpp:16
gtsam::EssentialMatrix::error
GTSAM_EXPORT double error(const Vector3 &vA, const Vector3 &vB, OptionalJacobian< 1, 5 > H=boost::none) const
epipolar error, algebraic
Definition EssentialMatrix.cpp:104
gtsam::EssentialMatrix::retract
EssentialMatrix retract(const Vector5 &xi) const
Retract delta to manifold.
Definition EssentialMatrix.h:92
gtsam::EssentialMatrix::EssentialMatrix
EssentialMatrix(const Rot3 &aRb, const Unit3 &aTb)
Construct from rotation and translation.
Definition EssentialMatrix.h:46
gtsam::EssentialMatrix::print
GTSAM_EXPORT void print(const std::string &s="") const
print with optional string
Definition EssentialMatrix.cpp:48
gtsam::EssentialMatrix::Homogeneous
static Vector3 Homogeneous(const Point2 &p)
Static function to convert Point2 to homogeneous coordinates.
Definition EssentialMatrix.h:34
gtsam::EssentialMatrix::epipole_a
const Unit3 & epipole_a() const
Return epipole in image_a , as Unit3 to allow for infinity.
Definition EssentialMatrix.h:125
gtsam::EssentialMatrix::direction
const Unit3 & direction() const
Direction.
Definition EssentialMatrix.h:115
gtsam::EssentialMatrix::access
friend class boost::serialization::access
Serialization function.
Definition EssentialMatrix.h:184
gtsam::EssentialMatrix::transformTo
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
Definition EssentialMatrix.cpp:55
gtsam::EssentialMatrix::epipole_b
Unit3 epipole_b() const
Return epipole in image_b, as Unit3 to allow for infinity.
Definition EssentialMatrix.h:130
gtsam::EssentialMatrix::operator<<
GTSAM_EXPORT friend std::ostream & operator<<(std::ostream &os, const EssentialMatrix &E)
stream to stream
Definition EssentialMatrix.cpp:117
gtsam::EssentialMatrix::matrix
const Matrix3 & matrix() const
Return 3*3 matrix representation.
Definition EssentialMatrix.h:120
gtsam::EssentialMatrix::rotate
GTSAM_EXPORT 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.
Definition EssentialMatrix.cpp:73
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3).
Definition Pose3.h:37
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition Rot3.h:58
gtsam::Rot3::Random
static Rot3 Random(std::mt19937 &rng)
Random, generates a random axis, then random angle [-pi,pi] Example: std::mt19937 engine(42); Unit3 ...
Definition Rot3.cpp:40
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition Unit3.h:43
gtsam::Unit3::Random
static Unit3 Random(std::mt19937 &rng)
Random direction, using boost::uniform_on_sphere Example: std::mt19937 engine(42); Unit3 unit = Unit3...
Definition Unit3.cpp:55