ACloudViewer  3.9.4
A Modern Library for 3D Data Processing
pose_test.cc
Go to the documentation of this file.
1 // Copyright (c) 2018, ETH Zurich and UNC Chapel Hill.
2 // All rights reserved.
3 //
4 // Redistribution and use in source and binary forms, with or without
5 // modification, are permitted provided that the following conditions are met:
6 //
7 // * Redistributions of source code must retain the above copyright
8 // notice, this list of conditions and the following disclaimer.
9 //
10 // * Redistributions in binary form must reproduce the above copyright
11 // notice, this list of conditions and the following disclaimer in the
12 // documentation and/or other materials provided with the distribution.
13 //
14 // * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
15 // its contributors may be used to endorse or promote products derived
16 // from this software without specific prior written permission.
17 //
18 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
19 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
21 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
22 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
23 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
24 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
25 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
26 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
27 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
28 // POSSIBILITY OF SUCH DAMAGE.
29 //
30 // Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de)
31 
32 #define TEST_NAME "base/pose"
33 #include "util/testing.h"
34 
35 #include <Eigen/Core>
36 
37 #include "base/pose.h"
38 #include "base/projection.h"
39 #include "util/math.h"
40 
41 using namespace colmap;
42 
43 BOOST_AUTO_TEST_CASE(TestCrossProductMatrix) {
44  BOOST_CHECK_EQUAL(CrossProductMatrix(Eigen::Vector3d(0, 0, 0)),
45  Eigen::Matrix3d::Zero());
46  Eigen::Matrix3d ref_matrix;
47  ref_matrix << 0, -3, 2, 3, 0, -1, -2, 1, 0;
48  BOOST_CHECK_EQUAL(CrossProductMatrix(Eigen::Vector3d(1, 2, 3)), ref_matrix);
49 }
50 
51 BOOST_AUTO_TEST_CASE(TestEulerAnglesX) {
52  const double rx = 0.3;
53  const double ry = 0;
54  const double rz = 0;
55  double rxx, ryy, rzz;
56 
57  RotationMatrixToEulerAngles(EulerAnglesToRotationMatrix(rx, ry, rz), &rxx,
58  &ryy, &rzz);
59 
60  BOOST_CHECK_CLOSE(rx, rxx, 1e-6);
61  BOOST_CHECK_CLOSE(ry, ryy, 1e-6);
62  BOOST_CHECK_CLOSE(rz, rzz, 1e-6);
63 }
64 
65 BOOST_AUTO_TEST_CASE(TestEulerAnglesY) {
66  const double rx = 0;
67  const double ry = 0.3;
68  const double rz = 0;
69  double rxx, ryy, rzz;
70 
71  RotationMatrixToEulerAngles(EulerAnglesToRotationMatrix(rx, ry, rz), &rxx,
72  &ryy, &rzz);
73 
74  BOOST_CHECK_CLOSE(rx, rxx, 1e-6);
75  BOOST_CHECK_CLOSE(ry, ryy, 1e-6);
76  BOOST_CHECK_CLOSE(rz, rzz, 1e-6);
77 }
78 
79 BOOST_AUTO_TEST_CASE(TestEulerAnglesZ) {
80  const double rx = 0;
81  const double ry = 0;
82  const double rz = 0.3;
83  double rxx, ryy, rzz;
84 
85  RotationMatrixToEulerAngles(EulerAnglesToRotationMatrix(rx, ry, rz), &rxx,
86  &ryy, &rzz);
87 
88  BOOST_CHECK_CLOSE(rx, rxx, 1e-6);
89  BOOST_CHECK_CLOSE(ry, ryy, 1e-6);
90  BOOST_CHECK_CLOSE(rz, rzz, 1e-6);
91 }
92 
93 BOOST_AUTO_TEST_CASE(TestEulerAnglesXYZ) {
94  const double rx = 0.1;
95  const double ry = 0.2;
96  const double rz = 0.3;
97  double rxx, ryy, rzz;
98 
99  RotationMatrixToEulerAngles(EulerAnglesToRotationMatrix(rx, ry, rz), &rxx,
100  &ryy, &rzz);
101 
102  BOOST_CHECK_CLOSE(rx, rxx, 1e-6);
103  BOOST_CHECK_CLOSE(ry, ryy, 1e-6);
104  BOOST_CHECK_CLOSE(rz, rzz, 1e-6);
105 }
106 
107 BOOST_AUTO_TEST_CASE(TestQuaternionToRotationMatrix) {
108  const double rx = 0;
109  const double ry = 0;
110  const double rz = 0.3;
111  const Eigen::Matrix3d rot_mat0 = EulerAnglesToRotationMatrix(rx, ry, rz);
112  const Eigen::Matrix3d rot_mat1 =
113  QuaternionToRotationMatrix(RotationMatrixToQuaternion(rot_mat0));
114  BOOST_CHECK(rot_mat0.isApprox(rot_mat1));
115 }
116 
117 BOOST_AUTO_TEST_CASE(TestComposeIdentityQuaternion) {
118  BOOST_CHECK_EQUAL(ComposeIdentityQuaternion(), Eigen::Vector4d(1, 0, 0, 0));
119 }
120 
121 BOOST_AUTO_TEST_CASE(TestNormalizeQuaternion) {
122  BOOST_CHECK_EQUAL(NormalizeQuaternion(ComposeIdentityQuaternion()),
123  ComposeIdentityQuaternion());
124  BOOST_CHECK_EQUAL(NormalizeQuaternion(Eigen::Vector4d(2, 0, 0, 0)),
125  ComposeIdentityQuaternion());
126  BOOST_CHECK_EQUAL(NormalizeQuaternion(Eigen::Vector4d(0.5, 0, 0, 0)),
127  ComposeIdentityQuaternion());
128  BOOST_CHECK_EQUAL(NormalizeQuaternion(Eigen::Vector4d(0, 0, 0, 0)),
129  ComposeIdentityQuaternion());
130  BOOST_CHECK(
131  NormalizeQuaternion(Eigen::Vector4d(1, 1, 0, 0))
132  .isApprox(Eigen::Vector4d(std::sqrt(2) / 2, std::sqrt(2) / 2, 0, 0)));
133  BOOST_CHECK(
134  NormalizeQuaternion(Eigen::Vector4d(0.5, 0.5, 0, 0))
135  .isApprox(Eigen::Vector4d(std::sqrt(2) / 2, std::sqrt(2) / 2, 0, 0)));
136 }
137 
138 BOOST_AUTO_TEST_CASE(TestInvertQuaternion) {
139  BOOST_CHECK_EQUAL(InvertQuaternion(ComposeIdentityQuaternion()),
140  Eigen::Vector4d(1, -0, -0, -0));
141  BOOST_CHECK_EQUAL(InvertQuaternion(Eigen::Vector4d(2, 0, 0, 0)),
142  Eigen::Vector4d(2, -0, -0, -0));
143  BOOST_CHECK_EQUAL(
144  InvertQuaternion(InvertQuaternion(Eigen::Vector4d(0.1, 0.2, 0.3, 0.4))),
145  Eigen::Vector4d(0.1, 0.2, 0.3, 0.4));
146 }
147 
148 BOOST_AUTO_TEST_CASE(TestConcatenateQuaternions) {
149  BOOST_CHECK_EQUAL(ConcatenateQuaternions(ComposeIdentityQuaternion(),
150  ComposeIdentityQuaternion()),
151  ComposeIdentityQuaternion());
152  BOOST_CHECK_EQUAL(ConcatenateQuaternions(Eigen::Vector4d(2, 0, 0, 0),
153  ComposeIdentityQuaternion()),
154  ComposeIdentityQuaternion());
155  BOOST_CHECK_EQUAL(ConcatenateQuaternions(ComposeIdentityQuaternion(),
156  Eigen::Vector4d(2, 0, 0, 0)),
157  ComposeIdentityQuaternion());
158  BOOST_CHECK(ConcatenateQuaternions(
159  Eigen::Vector4d(0.1, 0.2, 0.3, 0.4),
160  InvertQuaternion(Eigen::Vector4d(0.1, 0.2, 0.3, 0.4)))
161  .isApprox(ComposeIdentityQuaternion()));
162  BOOST_CHECK(ConcatenateQuaternions(
163  InvertQuaternion(Eigen::Vector4d(0.1, 0.2, 0.3, 0.4)),
164  Eigen::Vector4d(0.1, 0.2, 0.3, 0.4))
165  .isApprox(ComposeIdentityQuaternion()));
166 }
167 
168 BOOST_AUTO_TEST_CASE(TestQuaternionRotatePoint) {
169  BOOST_CHECK_EQUAL(QuaternionRotatePoint(ComposeIdentityQuaternion(),
170  Eigen::Vector3d(0, 0, 0)),
171  Eigen::Vector3d(0, 0, 0));
172  BOOST_CHECK_EQUAL(QuaternionRotatePoint(Eigen::Vector4d(0.1, 0, 0, 0),
173  Eigen::Vector3d(0, 0, 0)),
174  Eigen::Vector3d(0, 0, 0));
175  BOOST_CHECK_EQUAL(QuaternionRotatePoint(ComposeIdentityQuaternion(),
176  Eigen::Vector3d(1, 1, 0)),
177  Eigen::Vector3d(1, 1, 0));
178  BOOST_CHECK_EQUAL(QuaternionRotatePoint(Eigen::Vector4d(0.1, 0, 0, 0),
179  Eigen::Vector3d(1, 1, 0)),
180  Eigen::Vector3d(1, 1, 0));
181  BOOST_CHECK(
182  QuaternionRotatePoint(
183  RotationMatrixToQuaternion(EulerAnglesToRotationMatrix(M_PI, 0, 0)),
184  Eigen::Vector3d(1, 1, 0))
185  .isApprox(Eigen::Vector3d(1, -1, 0)));
186 }
187 
188 BOOST_AUTO_TEST_CASE(TestAverageQuaternions) {
189  std::vector<Eigen::Vector4d> qvecs;
190  std::vector<double> weights;
191 
192  qvecs = {{ComposeIdentityQuaternion()}};
193  weights = {1.0};
194  BOOST_CHECK_EQUAL(AverageQuaternions(qvecs, weights),
195  ComposeIdentityQuaternion());
196 
197  qvecs = {ComposeIdentityQuaternion()};
198  weights = {2.0};
199  BOOST_CHECK_EQUAL(AverageQuaternions(qvecs, weights),
200  ComposeIdentityQuaternion());
201 
202  qvecs = {ComposeIdentityQuaternion(), ComposeIdentityQuaternion()};
203  weights = {1.0, 1.0};
204  BOOST_CHECK_EQUAL(AverageQuaternions(qvecs, weights),
205  ComposeIdentityQuaternion());
206 
207  qvecs = {ComposeIdentityQuaternion(), ComposeIdentityQuaternion()};
208  weights = {1.0, 2.0};
209  BOOST_CHECK_EQUAL(AverageQuaternions(qvecs, weights),
210  ComposeIdentityQuaternion());
211 
212  qvecs = {ComposeIdentityQuaternion(), Eigen::Vector4d(2, 0, 0, 0)};
213  weights = {1.0, 2.0};
214  BOOST_CHECK_EQUAL(AverageQuaternions(qvecs, weights),
215  ComposeIdentityQuaternion());
216 
217  qvecs = {ComposeIdentityQuaternion(), Eigen::Vector4d(1, 1, 0, 0)};
218  weights = {1.0, 1.0};
219  BOOST_CHECK(AverageQuaternions(qvecs, weights)
220  .isApprox(Eigen::Vector4d(0.92388, 0.382683, 0, 0), 1e-6));
221 
222  qvecs = {ComposeIdentityQuaternion(), Eigen::Vector4d(1, 1, 0, 0)};
223  weights = {1.0, 2.0};
224  BOOST_CHECK(AverageQuaternions(qvecs, weights)
225  .isApprox(Eigen::Vector4d(0.850651, 0.525731, 0, 0), 1e-6));
226 }
227 
228 BOOST_AUTO_TEST_CASE(TestRotationFromUnitVectors) {
229  BOOST_CHECK_EQUAL(RotationFromUnitVectors(Eigen::Vector3d(0, 0, 1),
230  Eigen::Vector3d(0, 0, 1)),
231  Eigen::Matrix3d::Identity());
232  BOOST_CHECK_EQUAL(RotationFromUnitVectors(Eigen::Vector3d(0, 0, 2),
233  Eigen::Vector3d(0, 0, 2)),
234  Eigen::Matrix3d::Identity());
235 
236  Eigen::Matrix3d ref_matrix1;
237  ref_matrix1 << 1, 0, 0, 0, 0, 1, 0, -1, 0;
238  BOOST_CHECK_EQUAL(RotationFromUnitVectors(Eigen::Vector3d(0, 0, 1),
239  Eigen::Vector3d(0, 1, 0)),
240  ref_matrix1);
241  BOOST_CHECK_EQUAL(ref_matrix1 * Eigen::Vector3d(0, 0, 1),
242  Eigen::Vector3d(0, 1, 0));
243  BOOST_CHECK_EQUAL(RotationFromUnitVectors(Eigen::Vector3d(0, 0, 2),
244  Eigen::Vector3d(0, 2, 0)),
245  ref_matrix1);
246  BOOST_CHECK_EQUAL(ref_matrix1 * Eigen::Vector3d(0, 0, 2),
247  Eigen::Vector3d(0, 2, 0));
248 
249  BOOST_CHECK_EQUAL(RotationFromUnitVectors(Eigen::Vector3d(0, 0, 1),
250  Eigen::Vector3d(0, 0, -1)),
251  Eigen::Matrix3d::Identity());
252 }
253 
254 BOOST_AUTO_TEST_CASE(TestPoseFromProjectionMatrix) {
255  const Eigen::Vector4d qvec = Eigen::Vector4d::Random().normalized();
256  const Eigen::Vector3d tvec(3, 4, 5);
257  const Eigen::Matrix3x4d proj_matrix = ComposeProjectionMatrix(qvec, tvec);
258  const Eigen::Matrix3x4d inv_proj_matrix = InvertProjectionMatrix(proj_matrix);
259  const Eigen::Vector3d pose = ProjectionCenterFromMatrix(proj_matrix);
260  BOOST_CHECK_CLOSE((inv_proj_matrix.rightCols<1>() - pose).norm(), 0, 1e-6);
261 }
262 
263 BOOST_AUTO_TEST_CASE(TestPoseFromProjectionParameters) {
264  const Eigen::Vector4d qvec = Eigen::Vector4d::Random().normalized();
265  const Eigen::Vector3d tvec(3, 4, 5);
266  const Eigen::Matrix3x4d proj_matrix = ComposeProjectionMatrix(qvec, tvec);
267  const Eigen::Matrix3x4d inv_proj_matrix = InvertProjectionMatrix(proj_matrix);
268  const Eigen::Vector3d pose = ProjectionCenterFromPose(qvec, tvec);
269  BOOST_CHECK((inv_proj_matrix.rightCols<1>() - pose).norm() < 1e-6);
270 }
271 
272 BOOST_AUTO_TEST_CASE(TestComputeRelativePose) {
273  Eigen::Vector4d qvec12;
274  Eigen::Vector3d tvec12;
275 
276  ComputeRelativePose(ComposeIdentityQuaternion(), Eigen::Vector3d(0, 0, 0),
277  ComposeIdentityQuaternion(), Eigen::Vector3d(0, 0, 0),
278  &qvec12, &tvec12);
279  BOOST_CHECK_EQUAL(qvec12, ComposeIdentityQuaternion());
280  BOOST_CHECK_EQUAL(tvec12, Eigen::Vector3d(0, 0, 0));
281 
282  ComputeRelativePose(ComposeIdentityQuaternion(), Eigen::Vector3d(0, 0, 0),
283  ComposeIdentityQuaternion(), Eigen::Vector3d(1, 0, 0),
284  &qvec12, &tvec12);
285  BOOST_CHECK_EQUAL(qvec12, ComposeIdentityQuaternion());
286  BOOST_CHECK_EQUAL(tvec12, Eigen::Vector3d(1, 0, 0));
287 
288  ComputeRelativePose(ComposeIdentityQuaternion(), Eigen::Vector3d(0, 0, 0),
289  Eigen::Vector4d(1, 1, 0, 0), Eigen::Vector3d(0, 0, 0),
290  &qvec12, &tvec12);
291  BOOST_CHECK(qvec12.isApprox(Eigen::Vector4d(0.707107, 0.707107, 0, 0), 1e-6));
292  BOOST_CHECK_EQUAL(tvec12, Eigen::Vector3d(0, 0, 0));
293 
294  ComputeRelativePose(ComposeIdentityQuaternion(), Eigen::Vector3d(0, 0, 0),
295  Eigen::Vector4d(1, 1, 0, 0), Eigen::Vector3d(1, 0, 0),
296  &qvec12, &tvec12);
297  BOOST_CHECK(qvec12.isApprox(Eigen::Vector4d(0.707107, 0.707107, 0, 0), 1e-6));
298  BOOST_CHECK_EQUAL(tvec12, Eigen::Vector3d(1, 0, 0));
299 
300  ComputeRelativePose(Eigen::Vector4d(1, 1, 0, 0), Eigen::Vector3d(0, 0, 0),
301  Eigen::Vector4d(1, 1, 0, 0), Eigen::Vector3d(1, 0, 0),
302  &qvec12, &tvec12);
303  BOOST_CHECK(qvec12.isApprox(ComposeIdentityQuaternion()));
304  BOOST_CHECK_EQUAL(tvec12, Eigen::Vector3d(1, 0, 0));
305 
306  ComputeRelativePose(ComposeIdentityQuaternion(), Eigen::Vector3d(0, 0, 1),
307  Eigen::Vector4d(1, 1, 0, 0), Eigen::Vector3d(0, 0, 0),
308  &qvec12, &tvec12);
309  BOOST_CHECK(qvec12.isApprox(Eigen::Vector4d(0.707107, 0.707107, 0, 0), 1e-6));
310  BOOST_CHECK(tvec12.isApprox(Eigen::Vector3d(0, 1, 0)));
311 }
312 
313 BOOST_AUTO_TEST_CASE(TestConcatenatePoses) {
314  Eigen::Vector4d qvec12;
315  Eigen::Vector3d tvec12;
316 
317  ConcatenatePoses(ComposeIdentityQuaternion(), Eigen::Vector3d(0, 0, 0),
318  ComposeIdentityQuaternion(), Eigen::Vector3d(0, 0, 0),
319  &qvec12, &tvec12);
320  BOOST_CHECK_EQUAL(qvec12, ComposeIdentityQuaternion());
321  BOOST_CHECK_EQUAL(tvec12, Eigen::Vector3d(0, 0, 0));
322 
323  ConcatenatePoses(ComposeIdentityQuaternion(), Eigen::Vector3d(0, 0, 0),
324  ComposeIdentityQuaternion(), Eigen::Vector3d(0, 1, 2),
325  &qvec12, &tvec12);
326  BOOST_CHECK_EQUAL(qvec12, ComposeIdentityQuaternion());
327  BOOST_CHECK_EQUAL(tvec12, Eigen::Vector3d(0, 1, 2));
328 
329  ConcatenatePoses(ComposeIdentityQuaternion(), Eigen::Vector3d(0, 1, 2),
330  ComposeIdentityQuaternion(), Eigen::Vector3d(3, 4, 5),
331  &qvec12, &tvec12);
332  BOOST_CHECK_EQUAL(qvec12, ComposeIdentityQuaternion());
333  BOOST_CHECK_EQUAL(tvec12, Eigen::Vector3d(3, 5, 7));
334 
335  Eigen::Vector4d rel_qvec12;
336  Eigen::Vector3d rel_tvec12;
337  ComputeRelativePose(Eigen::Vector4d(1, 1, 0, 0), Eigen::Vector3d(0, 1, 2),
338  Eigen::Vector4d(1, 3, 0, 0), Eigen::Vector3d(3, 4, 5),
339  &rel_qvec12, &rel_tvec12);
340  ConcatenatePoses(Eigen::Vector4d(1, 1, 0, 0), Eigen::Vector3d(0, 1, 2),
341  rel_qvec12, rel_tvec12, &qvec12, &tvec12);
342  BOOST_CHECK(
343  qvec12.isApprox(NormalizeQuaternion(Eigen::Vector4d(1, 3, 0, 0))));
344  BOOST_CHECK(tvec12.isApprox(Eigen::Vector3d(3, 4, 5)));
345 }
346 
347 BOOST_AUTO_TEST_CASE(TestInvertPose) {
348  Eigen::Vector4d inv_qvec;
349  Eigen::Vector3d inv_tvec;
350  InvertPose(ComposeIdentityQuaternion(), Eigen::Vector3d(0, 0, 0), &inv_qvec,
351  &inv_tvec);
352  BOOST_CHECK_EQUAL(inv_qvec, ComposeIdentityQuaternion());
353  BOOST_CHECK_EQUAL(inv_tvec, Eigen::Vector3d(0, 0, 0));
354  InvertPose(Eigen::Vector4d(0, 1, 2, 3), Eigen::Vector3d(0, 1, 2), &inv_qvec,
355  &inv_tvec);
356  Eigen::Vector4d inv_inv_qvec;
357  Eigen::Vector3d inv_inv_tvec;
358  InvertPose(inv_qvec, inv_tvec, &inv_inv_qvec, &inv_inv_tvec);
359  BOOST_CHECK_EQUAL(inv_inv_qvec, Eigen::Vector4d(0, 1, 2, 3));
360  BOOST_CHECK(inv_inv_tvec.isApprox(Eigen::Vector3d(0, 1, 2)));
361 }
362 
363 BOOST_AUTO_TEST_CASE(TestInterpolatePose) {
364  const Eigen::Vector4d qvec1 = Eigen::Vector4d::Random().normalized();
365  const Eigen::Vector3d tvec1 = Eigen::Vector3d::Random();
366  const Eigen::Vector4d qvec2 = Eigen::Vector4d::Random().normalized();
367  const Eigen::Vector3d tvec2 = Eigen::Vector3d::Random();
368 
369  Eigen::Vector4d qveci;
370  Eigen::Vector3d tveci;
371 
372  InterpolatePose(qvec1, tvec1, qvec2, tvec2, 0, &qveci, &tveci);
373  BOOST_CHECK(tvec1.isApprox(tveci));
374 
375  InterpolatePose(qvec1, tvec1, qvec2, tvec2, 1, &qveci, &tveci);
376  BOOST_CHECK(tvec2.isApprox(tveci));
377 
378  InterpolatePose(qvec1, tvec1, qvec2, tvec2, 0.5, &qveci, &tveci);
379  BOOST_CHECK(((tvec1 + tvec2) / 2).isApprox(tveci));
380 }
381 
382 BOOST_AUTO_TEST_CASE(TestCalculateBaseline) {
383  Eigen::Vector4d qvec1(1, 0, 0, 0);
384  Eigen::Vector4d qvec2(1, 0, 0, 0);
385 
386  Eigen::Vector3d tvec1(0, 0, 0);
387  Eigen::Vector3d tvec2(0, 0, 1);
388 
389  const double baseline1 = CalculateBaseline(qvec1, tvec1, qvec2, tvec2).norm();
390  BOOST_CHECK_CLOSE(baseline1, 1, 1e-10);
391 
392  tvec2(2) = 2;
393 
394  const double baseline2 = CalculateBaseline(qvec1, tvec1, qvec2, tvec2).norm();
395  BOOST_CHECK_CLOSE(baseline2, 2, 1e-10);
396 }
397 
398 BOOST_AUTO_TEST_CASE(TestCheckCheirality) {
399  const Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
400  const Eigen::Vector3d t(1, 0, 0);
401 
402  std::vector<Eigen::Vector2d> points1;
403  std::vector<Eigen::Vector2d> points2;
404  std::vector<Eigen::Vector3d> points3D;
405 
406  points1.emplace_back(0, 0);
407  points2.emplace_back(0.1, 0);
408  BOOST_CHECK(CheckCheirality(R, t, points1, points2, &points3D));
409  BOOST_CHECK_EQUAL(points3D.size(), 1);
410 
411  points1.emplace_back(0, 0);
412  points2.emplace_back(-0.1, 0);
413  BOOST_CHECK(CheckCheirality(R, t, points1, points2, &points3D));
414  BOOST_CHECK_EQUAL(points3D.size(), 1);
415 
416  points2[1][0] = 0.2;
417  BOOST_CHECK(CheckCheirality(R, t, points1, points2, &points3D));
418  BOOST_CHECK_EQUAL(points3D.size(), 2);
419 
420  points2[0][0] = -0.2;
421  points2[1][0] = -0.2;
422  BOOST_CHECK(!CheckCheirality(R, t, points1, points2, &points3D));
423  BOOST_CHECK_EQUAL(points3D.size(), 0);
424 }
M_PI
constexpr double M_PI
Pi.
Definition: CVConst.h:19
e
const double * e
Definition: essential_matrix_poly.h:10
Eigen::Matrix3x4d
Matrix< double, 3, 4 > Matrix3x4d
Definition: types.h:39
colmap::ProjectionCenterFromMatrix
Eigen::Vector3d ProjectionCenterFromMatrix(const Eigen::Matrix3x4d &proj_matrix)
Definition: pose.cc:159
colmap::RotationFromUnitVectors
Eigen::Matrix3d RotationFromUnitVectors(const Eigen::Vector3d &vector1, const Eigen::Vector3d &vector2)
Definition: pose.cc:145
colmap::ComputeRelativePose
void ComputeRelativePose(const Eigen::Vector4d &qvec1, const Eigen::Vector3d &tvec1, const Eigen::Vector4d &qvec2, const Eigen::Vector3d &tvec2, Eigen::Vector4d *qvec12, Eigen::Vector3d *tvec12)
Definition: pose.cc:173
colmap::EulerAnglesToRotationMatrix
Eigen::Matrix3d EulerAnglesToRotationMatrix(const double rx, const double ry, const double rz)
Definition: pose.cc:59
colmap::ComposeIdentityQuaternion
Eigen::Vector4d ComposeIdentityQuaternion()
Definition: pose.h:209
colmap::InvertPose
void InvertPose(const Eigen::Vector4d &qvec, const Eigen::Vector3d &tvec, Eigen::Vector4d *inv_qvec, Eigen::Vector3d *inv_tvec)
Definition: pose.cc:192
colmap::CrossProductMatrix
Eigen::Matrix3d CrossProductMatrix(const Eigen::Vector3d &vector)
Definition: pose.cc:41
colmap::NormalizeQuaternion
Eigen::Vector4d NormalizeQuaternion(const Eigen::Vector4d &qvec)
Definition: pose.cc:82
colmap::AverageQuaternions
Eigen::Vector4d AverageQuaternions(const std::vector< Eigen::Vector4d > &qvecs, const std::vector< double > &weights)
Definition: pose.cc:118
colmap::ConcatenatePoses
void ConcatenatePoses(const Eigen::Vector4d &qvec1, const Eigen::Vector3d &tvec1, const Eigen::Vector4d &qvec2, const Eigen::Vector3d &tvec2, Eigen::Vector4d *qvec12, Eigen::Vector3d *tvec12)
Definition: pose.cc:183
colmap::InvertProjectionMatrix
Eigen::Matrix3x4d InvertProjectionMatrix(const Eigen::Matrix3x4d &proj_matrix)
Definition: projection.cc:55
colmap::RotationMatrixToQuaternion
Eigen::Vector4d RotationMatrixToQuaternion(const Eigen::Matrix3d &rot_mat)
Definition: pose.cc:70
colmap::ConcatenateQuaternions
Eigen::Vector4d ConcatenateQuaternions(const Eigen::Vector4d &qvec1, const Eigen::Vector4d &qvec2)
Definition: pose.cc:97
colmap::InvertQuaternion
Eigen::Vector4d InvertQuaternion(const Eigen::Vector4d &qvec)
Definition: pose.cc:93
colmap::QuaternionRotatePoint
Eigen::Vector3d QuaternionRotatePoint(const Eigen::Vector4d &qvec, const Eigen::Vector3d &point)
Definition: pose.cc:110
colmap::ProjectionCenterFromPose
Eigen::Vector3d ProjectionCenterFromPose(const Eigen::Vector4d &qvec, const Eigen::Vector3d &tvec)
Definition: pose.cc:164
colmap::CalculateBaseline
Eigen::Vector3d CalculateBaseline(const Eigen::Vector4d &qvec1, const Eigen::Vector3d &tvec1, const Eigen::Vector4d &qvec2, const Eigen::Vector3d &tvec2)
Definition: pose.cc:216
colmap::RotationMatrixToEulerAngles
void RotationMatrixToEulerAngles(const Eigen::Matrix3d &R, double *rx, double *ry, double *rz)
Definition: pose.cc:48
colmap::ComposeProjectionMatrix
Eigen::Matrix3x4d ComposeProjectionMatrix(const Eigen::Vector4d &qvec, const Eigen::Vector3d &tvec)
Definition: projection.cc:39
colmap::InterpolatePose
void InterpolatePose(const Eigen::Vector4d &qvec1, const Eigen::Vector3d &tvec1, const Eigen::Vector4d &qvec2, const Eigen::Vector3d &tvec2, const double t, Eigen::Vector4d *qveci, Eigen::Vector3d *tveci)
Definition: pose.cc:198
colmap::CheckCheirality
bool CheckCheirality(const Eigen::Matrix3d &R, const Eigen::Vector3d &t, const std::vector< Eigen::Vector2d > &points1, const std::vector< Eigen::Vector2d > &points2, std::vector< Eigen::Vector3d > *points3D)
Definition: pose.cc:225
colmap::QuaternionToRotationMatrix
Eigen::Matrix3d QuaternionToRotationMatrix(const Eigen::Vector4d &qvec)
Definition: pose.cc:75
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(TestCrossProductMatrix)
Definition: pose_test.cc:43