testEssentialMatrix.cpp
Go to the documentation of this file.
1 /*
2  * @file testEssentialMatrix.cpp
3  * @brief Test EssentialMatrix class
4  * @author Frank Dellaert
5  * @date December 17, 2013
6  */
7 
9 #include <gtsam/base/Testable.h>
13 
14 #include <sstream>
15 
16 using namespace std::placeholders;
17 using namespace std;
18 using namespace gtsam;
19 
22 
23 //*************************************************************************
24 // Create two cameras and corresponding essential matrix E
26 Point3 trueTranslation(0.1, 0, 0);
29 
30 //*************************************************************************
33  EXPECT(assert_equal(expected, actual));
34 }
35 
36 //*************************************************************************
37 TEST(EssentialMatrix, FromRotationAndDirection) {
38  Matrix actualH1, actualH2;
40  trueE, EssentialMatrix::FromRotationAndDirection(trueRotation, trueDirection, actualH1, actualH2),
41  1e-8));
42 
43  {
44  std::function<EssentialMatrix(const Rot3&)> fn = [](const Rot3& R) {
45  return EssentialMatrix::FromRotationAndDirection(R, trueDirection);
46  };
47  Matrix expectedH1 = numericalDerivative11<EssentialMatrix, Rot3>(fn, trueRotation);
48  EXPECT(assert_equal(expectedH1, actualH1, 1e-7));
49  }
50 
51  {
52  std::function<EssentialMatrix(const Unit3&)> fn = [](const Unit3& t) {
53  return EssentialMatrix::FromRotationAndDirection(trueRotation, t);
54  };
55  Matrix expectedH2 = numericalDerivative11<EssentialMatrix, Unit3>(fn, trueDirection);
56  EXPECT(assert_equal(expectedH2, actualH2, 1e-7));
57  }
58 }
59 
60 //*************************************************************************
61 TEST (EssentialMatrix, FromPose3) {
64  EssentialMatrix actual = EssentialMatrix::FromPose3(pose);
65  EXPECT(assert_equal(expected, actual));
66 }
67 
68 //*******************************************************************************
69 TEST(EssentialMatrix, localCoordinates0) {
71  Vector expected = Z_5x1;
72  Vector actual = E.localCoordinates(E);
73  EXPECT(assert_equal(expected, actual, 1e-8));
74 }
75 
76 //*************************************************************************
77 TEST (EssentialMatrix, localCoordinates) {
78 
79  // Pose between two cameras
81  EssentialMatrix hx = EssentialMatrix::FromPose3(pose);
82  Vector actual = hx.localCoordinates(EssentialMatrix::FromPose3(pose));
83  EXPECT(assert_equal(Z_5x1, actual, 1e-8));
84 
85  Vector6 d;
86  d << 0.1, 0.2, 0.3, 0, 0, 0;
87  Vector actual2 = hx.localCoordinates(
88  EssentialMatrix::FromPose3(pose.retract(d)));
89  EXPECT(assert_equal(d.head(5), actual2, 1e-8));
90 }
91 
92 //*************************************************************************
93 TEST (EssentialMatrix, retract0) {
94  EssentialMatrix actual = trueE.retract(Z_5x1);
95  EXPECT(assert_equal(trueE, actual));
96 }
97 
98 //*************************************************************************
99 TEST (EssentialMatrix, retract1) {
101  EssentialMatrix actual = trueE.retract((Vector(5) << 0.1, 0, 0, 0, 0).finished());
102  EXPECT(assert_equal(expected, actual));
103 }
104 
105 //*************************************************************************
106 TEST (EssentialMatrix, retract2) {
108  trueDirection.retract(Vector2(0.1, 0)));
109  EssentialMatrix actual = trueE.retract((Vector(5) << 0, 0, 0, 0.1, 0).finished());
110  EXPECT(assert_equal(expected, actual));
111 }
112 
113 //*************************************************************************
114 TEST (EssentialMatrix, RoundTrip) {
115  Vector5 d;
116  d << 0.1, 0.2, 0.3, 0.4, 0.5;
117  EssentialMatrix e, hx = e.retract(d);
118  Vector actual = e.localCoordinates(hx);
119  EXPECT(assert_equal(d, actual, 1e-8));
120 }
121 
122 //*************************************************************************
124  return E.transformTo(point);
125 }
127  // test with a more complicated EssentialMatrix
128  Rot3 aRb2 = Rot3::Yaw(M_PI / 3.0) * Rot3::Pitch(M_PI_4)
129  * Rot3::Roll(M_PI / 6.0);
130  Point3 aTb2(19.2, 3.7, 5.9);
131  EssentialMatrix E(aRb2, Unit3(aTb2));
132  //EssentialMatrix E(aRb, Unit3(aTb).retract(Vector2(0.1, 0)));
133  static Point3 P(0.2, 0.7, -2);
134  Matrix actH1, actH2;
135  E.transformTo(P, actH1, actH2);
138  EXPECT(assert_equal(expH1, actH1, 1e-8));
139  EXPECT(assert_equal(expH2, actH2, 1e-8));
140 }
141 
142 //*************************************************************************
144  return E.rotate(cRb);
145 }
147  // Suppose the essential matrix is specified in a body coordinate frame B
148  // which is rotated with respect to the camera frame C, via rotation bRc.
149  // The rotation between body and camera is:
150  Point3 bX(1, 0, 0), bY(0, 1, 0), bZ(0, 0, 1);
151  Rot3 bRc(bX, bZ, -bY), cRb = bRc.inverse();
152 
153  // Let's compute the ground truth E in body frame:
154  Rot3 b1Rb2 = bRc * trueRotation * cRb;
155  Point3 b1Tb2 = bRc * trueTranslation;
156  EssentialMatrix bodyE(b1Rb2, Unit3(b1Tb2));
157  EXPECT(assert_equal(bodyE, bRc * trueE, 1e-8));
158  EXPECT(assert_equal(bodyE, trueE.rotate(bRc), 1e-8));
159 
160  // Let's go back to camera frame:
161  EXPECT(assert_equal(trueE, cRb * bodyE, 1e-8));
163 
164  // Derivatives
165  Matrix actH1, actH2;
166  try {
167  bodyE.rotate(cRb, actH1, actH2);
168  } catch (exception& e) {
169  } // avoid exception
172  EXPECT(assert_equal(expH1, actH1, 1e-7));
173  // Does not work yet EXPECT(assert_equal(expH2, actH2, 1e-8));
174 }
175 
176 //*************************************************************************
177 TEST (EssentialMatrix, FromPose3_a) {
178  Matrix actualH;
179  Pose3 pose(trueRotation, trueTranslation); // Pose between two cameras
180  EXPECT(assert_equal(trueE, EssentialMatrix::FromPose3(pose, actualH), 1e-8));
181  std::function<EssentialMatrix(const Pose3&)> fn = [](const Pose3& pose) {
182  return EssentialMatrix::FromPose3(pose);
183  };
184  Matrix expectedH = numericalDerivative11<EssentialMatrix, Pose3>(fn, pose);
185  EXPECT(assert_equal(expectedH, actualH, 1e-7));
186 }
187 
188 //*************************************************************************
189 TEST (EssentialMatrix, FromPose3_b) {
190  Matrix actualH;
191  Rot3 c1Rc2 = Rot3::Ypr(0.1, -0.2, 0.3);
192  Point3 c1Tc2(0.4, 0.5, 0.6);
194  Pose3 pose(c1Rc2, c1Tc2); // Pose between two cameras
195  EXPECT(assert_equal(E, EssentialMatrix::FromPose3(pose, actualH), 1e-8));
196  std::function<EssentialMatrix(const Pose3&)> fn = [](const Pose3& pose) {
197  return EssentialMatrix::FromPose3(pose);
198  };
199  Matrix expectedH = numericalDerivative11<EssentialMatrix, Pose3>(fn, pose);
200  EXPECT(assert_equal(expectedH, actualH, 1e-5));
201 }
202 
203 //*************************************************************************
204 TEST (EssentialMatrix, streaming) {
206  stringstream ss;
207  ss << expected;
208  ss >> actual;
209  EXPECT(assert_equal(expected, actual));
210 }
211 
212 //*************************************************************************
213 TEST (EssentialMatrix, epipoles) {
214  // Create an E
215  Rot3 c1Rc2 = Rot3::Ypr(0.1, -0.2, 0.3);
216  Point3 c1Tc2(0.4, 0.5, 0.6);
218 
219  // Calculate expected values through SVD
220  Matrix U, V;
221  Vector S;
222  gtsam::svd(E.matrix(), U, S, V);
223 
224  // take care of SVD sign ambiguity
225  if (U(0, 2) > 0) {
226  U = -U;
227  V = -V;
228  }
229 
230  // check rank 2 constraint
231  CHECK(std::abs(S(2))<1e-10);
232 
233  // check epipoles not at infinity
234  CHECK(std::abs(U(2,2))>1e-10 && std::abs(V(2,2))>1e-10);
235 
236  // Check epipoles
237 
238  // Epipole in image 1 is just E.direction()
239  Unit3 e1(-U(0, 2), -U(1, 2), -U(2, 2));
240  Unit3 actual = E.epipole_a();
241  EXPECT(assert_equal(e1, actual));
242 
243  // take care of SVD sign ambiguity
244  if (V(0, 2) < 0) {
245  U = -U;
246  V = -V;
247  }
248 
249  // Epipole in image 2 is E.rotation().unrotate(E.direction())
250  Unit3 e2(V(0, 2), V(1, 2), V(2, 2));
251  EXPECT(assert_equal(e2, E.epipole_b()));
252 }
253 
254 /* ************************************************************************* */
255 int main() {
256  TestResult tr;
257  return TestRegistry::runAllTests(tr);
258 }
259 /* ************************************************************************* */
260 
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
EssentialMatrix.h
pose
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
trueRotation
Rot3 trueRotation
Definition: testEssentialMatrix.cpp:25
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
d
static const double d[K][N]
Definition: igam.h:11
rotate_
EssentialMatrix rotate_(const EssentialMatrix &E, const Rot3 &cRb)
Definition: testEssentialMatrix.cpp:143
GTSAM_CONCEPT_TESTABLE_INST
#define GTSAM_CONCEPT_TESTABLE_INST(T)
Definition: Testable.h:176
Testable.h
Concept check for values that can be used in unit tests.
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
TestHarness.h
transform_to_
Point3 transform_to_(const EssentialMatrix &E, const Point3 &point)
Definition: testEssentialMatrix.cpp:123
fn
static double fn[10]
Definition: fresnl.c:103
gtsam::numericalDerivative22
internal::FixedSizeMatrix< Y, X2 >::type numericalDerivative22(std::function< Y(const X1 &, const X2 &)> h, const X1 &x1, const X2 &x2, double delta=1e-5)
Definition: numericalDerivative.h:195
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
trueDirection
Unit3 trueDirection(trueTranslation)
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
CalibratedCamera.h
Calibrated camera for which only pose is unknown.
gtsam::EssentialMatrix::retract
EssentialMatrix retract(const Vector5 &xi) const
Retract delta to manifold.
Definition: EssentialMatrix.h:92
example1::c1Tc2
Point3 c1Tc2
Definition: testEssentialMatrixFactor.cpp:40
ss
static std::stringstream ss
Definition: testBTree.cpp:31
bX
gtsam::Point3 bX(1, 0, 0)
gtsam_unstable.tests.test_ProjectionFactorRollingShutter.point
point
Definition: test_ProjectionFactorRollingShutter.py:25
numericalDerivative.h
Some functions to compute numerical derivatives.
bY
gtsam::Point3 bY(0, 1, 0)
gtsam::transformTo
Line3 transformTo(const Pose3 &wTc, const Line3 &wL, OptionalJacobian< 4, 6 > Dpose, OptionalJacobian< 4, 4 > Dline)
Definition: Line3.cpp:94
gtsam::EssentialMatrix::localCoordinates
Vector5 localCoordinates(const EssentialMatrix &other) const
Compute the coordinates in the tangent space.
Definition: EssentialMatrix.h:97
gtsam::EssentialMatrix
Definition: EssentialMatrix.h:26
gtsam::EssentialMatrix::rotate
GTSAM_EXPORT EssentialMatrix rotate(const Rot3 &cRb, OptionalJacobian< 5, 5 > HE={}, OptionalJacobian< 5, 3 > HR={}) const
Definition: EssentialMatrix.cpp:73
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
gtsam::Pose3
Definition: Pose3.h:37
M_PI_4
#define M_PI_4
Definition: mconf.h:119
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
trueTranslation
Point3 trueTranslation(0.1, 0, 0)
Vector2
Definition: test_operator_overloading.cpp:18
TEST
TEST(EssentialMatrix, equality)
Definition: testEssentialMatrix.cpp:31
gtsam::Unit3::retract
Unit3 retract(const Vector2 &v, OptionalJacobian< 2, 2 > H={}) const
The retract function.
Definition: Unit3.cpp:255
gtsam::Rot3::inverse
Rot3 inverse() const
inverse of a rotation
Definition: Rot3.h:308
TestResult
Definition: TestResult.h:26
gtsam::svd
void svd(const Matrix &A, Matrix &U, Vector &S, Matrix &V)
Definition: Matrix.cpp:558
M_PI_2
#define M_PI_2
Definition: mconf.h:118
E
DiscreteKey E(5, 2)
gtsam::equality
bool equality(const Errors &actual, const Errors &expected, double tol)
Definition: Errors.cpp:52
gtsam
traits
Definition: SFMdata.h:40
CHECK
#define CHECK(condition)
Definition: Test.h:108
gtsam::numericalDerivative21
internal::FixedSizeMatrix< Y, X1 >::type numericalDerivative21(const std::function< Y(const X1 &, const X2 &)> &h, const X1 &x1, const X2 &x2, double delta=1e-5)
Definition: numericalDerivative.h:166
std
Definition: BFloat16.h:88
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
P
static double P[]
Definition: ellpe.c:68
GTSAM_CONCEPT_MANIFOLD_INST
#define GTSAM_CONCEPT_MANIFOLD_INST(T)
‍**
Definition: Manifold.h:177
bZ
gtsam::Point3 bZ(0, 0, 1)
example1::c1Rc2
Rot3 c1Rc2
Definition: testEssentialMatrixFactor.cpp:39
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
U
@ U
Definition: testDecisionTree.cpp:349
gtsam::LieGroup::retract
Class retract(const TangentVector &v) const
retract as required by manifold concept: applies v at *this
Definition: Lie.h:131
V
MatrixXcd V
Definition: EigenSolver_EigenSolver_MatrixType.cpp:15
abs
#define abs(x)
Definition: datatypes.h:17
trueE
EssentialMatrix trueE(trueRotation, trueDirection)
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42
M_PI
#define M_PI
Definition: mconf.h:117
main
int main()
Definition: testEssentialMatrix.cpp:255
align_3::t
Point2 t(10, 10)
cRb
gtsam::Rot3 cRb
Definition: testEssentialMatrixFactor.cpp:33
ceres::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: gtsam/3rdparty/ceres/eigen.h:38
gtsam::testing::rotate
P rotate(const T &r, const P &pt)
Definition: lieProxies.h:47
R
Rot2 R(Rot2::fromAngle(0.1))
example1::bodyE
EssentialMatrix bodyE
Definition: testEssentialMatrixFactor.cpp:267
S
DiscreteKey S(1, 2)


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:39:51