gtsam  4.1.0
gtsam
EssentialMatrix.h
1 /*
2  * @file EssentialMatrix.h
3  * @brief EssentialMatrix class
4  * @author Frank Dellaert
5  * @date December 17, 2013
6  */
7 
8 #pragma once
9 
10 #include <gtsam/geometry/Pose3.h>
11 #include <gtsam/geometry/Unit3.h>
12 #include <gtsam/geometry/Point2.h>
13 #include <gtsam/base/Manifold.h>
14 
15 #include <iosfwd>
16 #include <string>
17 
18 namespace gtsam {
19 
27  private:
28  Rot3 R_;
29  Unit3 t_;
30  Matrix3 E_;
31 
32  public:
34  static Vector3 Homogeneous(const Point2& p) {
35  return Vector3(p.x(), p.y(), 1);
36  }
37 
40 
42  EssentialMatrix() :E_(t_.skew()) {
43  }
44 
46  EssentialMatrix(const Rot3& aRb, const Unit3& aTb) :
47  R_(aRb), t_(aTb), E_(t_.skew() * R_.matrix()) {
48  }
49 
51  GTSAM_EXPORT static EssentialMatrix FromRotationAndDirection(const Rot3& aRb, const Unit3& aTb,
52  OptionalJacobian<5, 3> H1 = boost::none,
53  OptionalJacobian<5, 2> H2 = boost::none);
54 
56  GTSAM_EXPORT static EssentialMatrix FromPose3(const Pose3& _1P2_,
57  OptionalJacobian<5, 6> H = boost::none);
58 
60  template<typename Engine>
61  static EssentialMatrix Random(Engine & rng) {
62  return EssentialMatrix(Rot3::Random(rng), Unit3::Random(rng));
63  }
64 
65  virtual ~EssentialMatrix() {}
66 
68 
71 
73  GTSAM_EXPORT void print(const std::string& s = "") const;
74 
76  bool equals(const EssentialMatrix& other, double tol = 1e-8) const {
77  return R_.equals(other.R_, tol)
78  && t_.equals(other.t_, tol);
79  }
80 
82 
85  enum { dimension = 5 };
86  inline static size_t Dim() { return dimension;}
87  inline size_t dim() const { return dimension;}
88 
89  typedef OptionalJacobian<dimension, dimension> ChartJacobian;
90 
92  EssentialMatrix retract(const Vector5& xi) const {
93  return EssentialMatrix(R_.retract(xi.head<3>()), t_.retract(xi.tail<2>()));
94  }
95 
97  Vector5 localCoordinates(const EssentialMatrix& other) const {
98  auto v1 = R_.localCoordinates(other.R_);
99  auto v2 = t_.localCoordinates(other.t_);
100  Vector5 v;
101  v << v1, v2;
102  return v;
103  }
105 
108 
110  inline const Rot3& rotation() const {
111  return R_;
112  }
113 
115  inline const Unit3& direction() const {
116  return t_;
117  }
118 
120  inline const Matrix3& matrix() const {
121  return E_;
122  }
123 
125  inline const Unit3& epipole_a() const {
126  return t_;
127  }
128 
130  inline Unit3 epipole_b() const {
131  return R_.unrotate(t_);
132  }
133 
141  GTSAM_EXPORT Point3 transformTo(const Point3& p,
142  OptionalJacobian<3, 5> DE = boost::none,
143  OptionalJacobian<3, 3> Dpoint = boost::none) const;
144 
150  GTSAM_EXPORT EssentialMatrix rotate(const Rot3& cRb, OptionalJacobian<5, 5> HE =
151  boost::none, OptionalJacobian<5, 3> HR = boost::none) const;
152 
158  friend EssentialMatrix operator*(const Rot3& cRb, const EssentialMatrix& E) {
159  return E.rotate(cRb);
160  }
161 
163  GTSAM_EXPORT double error(const Vector3& vA, const Vector3& vB,
164  OptionalJacobian<1, 5> H = boost::none) const;
165 
167 
170 
172  GTSAM_EXPORT friend std::ostream& operator <<(std::ostream& os, const EssentialMatrix& E);
173 
175  GTSAM_EXPORT friend std::istream& operator >>(std::istream& is, EssentialMatrix& E);
176 
178 
179  private:
182 
185  template<class ARCHIVE>
186  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
187  ar & BOOST_SERIALIZATION_NVP(R_);
188  ar & BOOST_SERIALIZATION_NVP(t_);
189 
190  ar & boost::serialization::make_nvp("E11", E_(0, 0));
191  ar & boost::serialization::make_nvp("E12", E_(0, 1));
192  ar & boost::serialization::make_nvp("E13", E_(0, 2));
193  ar & boost::serialization::make_nvp("E21", E_(1, 0));
194  ar & boost::serialization::make_nvp("E22", E_(1, 1));
195  ar & boost::serialization::make_nvp("E23", E_(1, 2));
196  ar & boost::serialization::make_nvp("E31", E_(2, 0));
197  ar & boost::serialization::make_nvp("E32", E_(2, 1));
198  ar & boost::serialization::make_nvp("E33", E_(2, 2));
199  }
200 
202 
203  public:
205 };
206 
207 template<>
208 struct traits<EssentialMatrix> : public internal::Manifold<EssentialMatrix> {};
209 
210 template<>
211 struct traits<const EssentialMatrix> : public internal::Manifold<EssentialMatrix> {};
212 
213 } // namespace gtsam
214 
gtsam::LieGroup::retract
Class retract(const TangentVector &v) const
retract as required by manifold concept: applies v at *this
Definition: Lie.h:130
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::EssentialMatrix::matrix
const Matrix3 & matrix() const
Return 3*3 matrix representation.
Definition: EssentialMatrix.h:120
gtsam::Unit3::retract
GTSAM_EXPORT Unit3 retract(const Vector2 &v, OptionalJacobian< 2, 2 > H=boost::none) const
The retract function.
Definition: Unit3.cpp:247
gtsam::EssentialMatrix::operator>>
GTSAM_EXPORT friend std::istream & operator>>(std::istream &is, EssentialMatrix &E)
stream from stream
Definition: EssentialMatrix.cpp:126
gtsam::EssentialMatrix::access
friend class boost::serialization::access
Serialization function.
Definition: EssentialMatrix.h:184
gtsam::EssentialMatrix::print
GTSAM_EXPORT void print(const std::string &s="") const
print with optional string
Definition: EssentialMatrix.cpp:48
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition: OptionalJacobian.h:39
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::EssentialMatrix::Random
static EssentialMatrix Random(Engine &rng)
Random, using Rot3::Random and Unit3::Random.
Definition: EssentialMatrix.h:61
gtsam::EssentialMatrix::operator<<
GTSAM_EXPORT friend std::ostream & operator<<(std::ostream &os, const EssentialMatrix &E)
stream to stream
Definition: EssentialMatrix.cpp:117
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::Rot3::Random
static Rot3 Random(std::mt19937 &rng)
Random, generates a random axis, then random angle \in [-p,pi] Example: std::mt19937 engine(42); Unit...
Definition: Rot3.cpp:40
Pose3.h
3D Pose
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
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::Unit3::Random
static GTSAM_EXPORT Unit3 Random(std::mt19937 &rng)
Random direction, using boost::uniform_on_sphere Example: std::mt19937 engine(42); Unit3 unit = Unit3...
Definition: Unit3.cpp:47
gtsam::LieGroup::localCoordinates
TangentVector localCoordinates(const Class &g) const
localCoordinates as required by manifold concept: finds tangent vector between *this and g
Definition: Lie.h:135
gtsam::EssentialMatrix::Homogeneous
static Vector3 Homogeneous(const Point2 &p)
Static function to convert Point2 to homogeneous coordinates.
Definition: EssentialMatrix.h:34
gtsam::EssentialMatrix::epipole_b
Unit3 epipole_b() const
Return epipole in image_b, as Unit3 to allow for infinity.
Definition: EssentialMatrix.h:130
gtsam::Rot3::unrotate
Point3 unrotate(const Point3 &p, OptionalJacobian< 3, 3 > H1=boost::none, OptionalJacobian< 3, 3 > H2=boost::none) const
rotate point from world to rotated frame
Definition: Rot3.cpp:136
gtsam::EssentialMatrix::EssentialMatrix
EssentialMatrix()
Default constructor.
Definition: EssentialMatrix.h:42
gtsam::EssentialMatrix::localCoordinates
Vector5 localCoordinates(const EssentialMatrix &other) const
Compute the coordinates in the tangent space.
Definition: EssentialMatrix.h:97
gtsam::Unit3::localCoordinates
GTSAM_EXPORT Vector2 localCoordinates(const Unit3 &s) const
The local coordinates function.
Definition: Unit3.cpp:277
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::EssentialMatrix::rotation
const Rot3 & rotation() const
Rotation.
Definition: EssentialMatrix.h:110
gtsam::Rot3
Definition: Rot3.h:59
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_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition: types.h:269
gtsam::internal::Manifold
Both ManifoldTraits and Testable.
Definition: Manifold.h:120
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::Unit3
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42
gtsam::EssentialMatrix::equals
bool equals(const EssentialMatrix &other, double tol=1e-8) const
assert equality up to a tolerance
Definition: EssentialMatrix.h:76
gtsam::Pose3
Definition: Pose3.h:37
gtsam::Rot3::equals
bool equals(const Rot3 &p, double tol=1e-9) const
equals with an tolerance
Definition: Rot3.cpp:100
Point2.h
2D Point
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::Unit3::equals
bool equals(const Unit3 &s, double tol=1e-9) const
The equals function with tolerance.
Definition: Unit3.h:121
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::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:35
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
Manifold.h
Base class and basic functions for Manifold types.
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::direction
const Unit3 & direction() const
Direction.
Definition: EssentialMatrix.h:115