1 // This file is part of OpenMVG, an Open Multiple View Geometry C++ library.
2
3 // Copyright (c) 2017, Romain JANVIER & Pierre MOULON
4 // Copyright (c) 2020 Pierre MOULON.
5
6 // This Source Code Form is subject to the terms of the Mozilla Public
7 // License, v. 2.0. If a copy of the MPL was not distributed with this
8 // file, You can obtain one at http://mozilla.org/MPL/2.0/.
9
10 #include <cmath>
11 #include "openMVG/multiview/solver_essential_three_point.hpp"
12
13 namespace openMVG {
14
15 //
16 //
17 // Three point orthographic essential matrix estimation
18 // This solver is based on the following paper:
19 // Magnus Oskarsson, "Two-View Orthographic Epipolar Geometry: Minimal and Optimal Solvers",
20 // Journal of Mathematical Imaging and Vision, 2017
21 //
22 // Original matlab implementation can be found online
23 // https://github.com/hamburgerlady/ortho-gem
24 /**
25 * @brief Computes the relative pose of two orthographic cameras from 3 correspondences.
26 *
27 * \param x1 Points in the first image. One per column.
28 * \param x2 Corresponding points in the second image. One per column.
29 * \param E A list of at most 2 candidates orthographic essential matrix solutions.
30 */
ThreePointsRelativePose(const Mat2X & x1,const Mat2X & x2,std::vector<Mat3> * E)31 void ThreePointsRelativePose
32 (
33 const Mat2X &x1,
34 const Mat2X &x2,
35 std::vector<Mat3> *E
36 )
37 {
38 const Vec2
39 xd1 = x1.col(1) - x1.col(0),
40 yd1 = x1.col(2) - x1.col(0),
41 xd2 = x2.col(1) - x2.col(0),
42 yd2 = x2.col(2) - x2.col(0);
43
44 const double
45 denom = xd1.x() * yd1.y() - xd1.y() * yd1.x(),
46 aac = ( xd1.y() * yd2.x() - xd2.x() * yd1.y() ) / denom,
47 aad = ( xd1.y() * yd2.y() - xd2.y() * yd1.y() ) / denom,
48 bbc = ( xd2.x() * yd1.x() - xd1.x() * yd2.x() ) / denom,
49 bbd = ( xd2.y() * yd1.x() - xd1.x() * yd2.y() ) / denom;
50
51 // cache aac * aac
52 const double aac_sq = aac * aac;
53
54 const double
55 dd_2 = - aac_sq + aad * aad - bbc * bbc + bbd * bbd,
56 dd_1c = 2.0 * aac * aad + 2.0 * bbc * bbd,
57 dd_0 = aac_sq + bbc * bbc - 1.0,
58 d4_4 = dd_1c * dd_1c + dd_2 * dd_2,
59 d4_2 = -dd_1c * dd_1c + 2.0 * dd_0 * dd_2,
60 d4_0 = dd_0 * dd_0,
61 tmp = sqrt( ( d4_2 * d4_2 - 4.0 * d4_4 * d4_0 ) ),
62 tmp_csol = - aac_sq + aad * aad - bbc * bbc + bbd * bbd;
63
64 const auto ComputeEssentialMatrix = [&](const double root) -> Mat3 {
65 const double
66 dsol = sqrt( - root / d4_4 / 2.0 ),
67 csol = - ( tmp_csol * dsol * dsol + aac_sq + bbc * bbc - 1.0 ) /
68 ( 2.0 * aac * aad * dsol + 2.0 * bbc * bbd * dsol ),
69 asol = aac * csol + aad * dsol,
70 bsol = bbc * csol + bbd * dsol,
71 esol = - asol * x1(0,0) - bsol * x1(1,0) - csol * x2(0,0)
72 - dsol * x2(1,0);
73
74 return (Mat3() << 0.0, 0.0, asol,
75 0.0, 0.0, bsol,
76 csol, dsol, esol).finished();
77 };
78
79 E->emplace_back(ComputeEssentialMatrix(d4_2 + tmp));
80 E->emplace_back(ComputeEssentialMatrix(d4_2 - tmp));
81 }
82
83 namespace essential {
84 namespace kernel {
85
Solve(const Mat3X & bearing_a,const Mat3X & bearing_b,std::vector<Mat3> * pvec_E)86 void ThreePointUprightRelativePoseSolver::Solve
87 (
88 const Mat3X & bearing_a,
89 const Mat3X & bearing_b,
90 std::vector<Mat3> * pvec_E
91 )
92 {
93 // Build the action matrix -> see (6,7) in the paper
94 Eigen::Matrix<double, 3, 4> A;
95 for (const int i : {0,1,2})
96 {
97 const auto & bearing_a_i = bearing_a.col(i);
98 const auto & bearing_b_i = bearing_b.col(i);
99 A.row(i) <<
100 bearing_a_i.x() * bearing_b_i.y(), -bearing_a_i.z() * bearing_b_i.y(),
101 -bearing_b_i.x() * bearing_a_i.y(), -bearing_b_i.z() * bearing_a_i.y();
102 }
103
104 Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, 4, 4>> solver(A.transpose() * A);
105 const Vec4 nullspace = solver.eigenvectors().leftCols<1>();
106
107 Mat3 E = Mat3::Zero(); // see (3) in the paper
108 E(0, 1) = nullspace(2);
109 E(1, 0) = - nullspace(0);
110 E(1, 2) = nullspace(1);
111 E(2, 1) = nullspace(3);
112
113 pvec_E->emplace_back(E);
114 }
115
116 } // namespace kernel
117 } // namespace essential
118 } // namespace openMVG
119