MVE - Multi-View Environment mve-devel
Loading...
Searching...
No Matches
pose_p3p.cc
Go to the documentation of this file.
1/*
2 * Copyright (C) 2015, Simon Fuhrmann
3 * TU Darmstadt - Graphics, Capture and Massively Parallel Computing
4 * All rights reserved.
5 *
6 * This software may be modified and distributed under the terms
7 * of the BSD 3-Clause license. See the LICENSE.txt file for details.
8 */
9
10#include <complex>
11#include <algorithm>
12
13#include "math/matrix.h"
14#include "math/vector.h"
15#include "sfm/pose_p3p.h"
16
18
19namespace
20{
21 void
22 solve_quartic_roots (math::Vec5d const& factors, math::Vec4d* real_roots)
23 {
24 double const A = factors[0];
25 double const B = factors[1];
26 double const C = factors[2];
27 double const D = factors[3];
28 double const E = factors[4];
29
30 double const A2 = A * A;
31 double const B2 = B * B;
32 double const A3 = A2 * A;
33 double const B3 = B2 * B;
34 double const A4 = A3 * A;
35 double const B4 = B3 * B;
36
37 double const alpha = -3.0 * B2 / (8.0 * A2) + C / A;
38 double const beta = B3 / (8.0 * A3)- B * C / (2.0 * A2) + D / A;
39 double const gamma = -3.0 * B4 / (256.0 * A4) + B2 * C / (16.0 * A3)
40 - B * D / (4.0 * A2) + E / A;
41
42 double const alpha2 = alpha * alpha;
43 double const alpha3 = alpha2 * alpha;
44 double const beta2 = beta * beta;
45
46 std::complex<double> P(-alpha2 / 12.0 - gamma, 0.0);
47 std::complex<double> Q(-alpha3 / 108.0
48 + alpha * gamma / 3.0 - beta2 / 8.0, 0.0);
49 std::complex<double> R = -Q / 2.0
50 + std::sqrt(Q * Q / 4.0 + P * P * P / 27.0);
51
52 std::complex<double> U = std::pow(R, 1.0 / 3.0);
53 std::complex<double> y = (U.real() == 0.0)
54 ? -5.0 * alpha / 6.0 - std::pow(Q, 1.0 / 3.0)
55 : -5.0 * alpha / 6.0 - P / (3.0 * U) + U;
56
57 std::complex<double> w = std::sqrt(alpha + 2.0 * y);
58 std::complex<double> part1 = -B / (4.0 * A);
59 std::complex<double> part2 = 3.0 * alpha + 2.0 * y;
60 std::complex<double> part3 = 2.0 * beta / w;
61
62 std::complex<double> complex_roots[4];
63 complex_roots[0] = part1 + 0.5 * (w + std::sqrt(-(part2 + part3)));
64 complex_roots[1] = part1 + 0.5 * (w - std::sqrt(-(part2 + part3)));
65 complex_roots[2] = part1 + 0.5 * (-w + std::sqrt(-(part2 - part3)));
66 complex_roots[3] = part1 + 0.5 * (-w - std::sqrt(-(part2 - part3)));
67
68 for (int i = 0; i < 4; ++i)
69 (*real_roots)[i] = complex_roots[i].real();
70 }
71}
72
73void
77 std::vector<math::Matrix<double, 3, 4> >* solutions)
78{
79 /* Check if points are co-linear. In this case return no solution. */
80 double const colinear_threshold = 1e-10;
81 if ((p2 - p1).cross(p3 - p1).square_norm() < colinear_threshold)
82 {
83 solutions->clear();
84 return;
85 }
86
87 /* Normalize directions if necessary. */
88 double const normalize_epsilon = 1e-10;
89 if (!MATH_EPSILON_EQ(f1.square_norm(), 1.0, normalize_epsilon))
90 f1.normalize();
91 if (!MATH_EPSILON_EQ(f2.square_norm(), 1.0, normalize_epsilon))
92 f2.normalize();
93 if (!MATH_EPSILON_EQ(f3.square_norm(), 1.0, normalize_epsilon))
94 f3.normalize();
95
96 /* Create camera frame. */
98 {
99 math::Vec3d e1 = f1;
100 math::Vec3d e3 = f1.cross(f2).normalized();
101 math::Vec3d e2 = e3.cross(e1);
102 std::copy(e1.begin(), e1.end(), T.begin() + 0);
103 std::copy(e2.begin(), e2.end(), T.begin() + 3);
104 std::copy(e3.begin(), e3.end(), T.begin() + 6);
105 }
106
107 /* Change camera frame and point order if new z > 0. */
108 if (T.row(2).dot(f3) > 0.0)
109 {
110 std::swap(p1, p2);
111 std::swap(f1, f2);
112
113 math::Vec3d e1 = f1;
114 math::Vec3d e3 = f1.cross(f2).normalized();
115 math::Vec3d e2 = e3.cross(e1);
116 std::copy(e1.begin(), e1.end(), T.begin() + 0);
117 std::copy(e2.begin(), e2.end(), T.begin() + 3);
118 std::copy(e3.begin(), e3.end(), T.begin() + 6);
119 }
120 f3 = T * f3;
121
122 /* Create world frame. */
124 {
125 math::Vec3d n1 = (p2 - p1).normalized();
126 math::Vec3d n3 = n1.cross(p3 - p1).normalized();
127 math::Vec3d n2 = n3.cross(n1);
128 std::copy(n1.begin(), n1.end(), N.begin() + 0);
129 std::copy(n2.begin(), n2.end(), N.begin() + 3);
130 std::copy(n3.begin(), n3.end(), N.begin() + 6);
131 }
132 p3 = N * (p3 - p1);
133
134 /* Extraction of known parameters. */
135 double d_12 = (p2 - p1).norm();
136 double f_1 = f3[0] / f3[2];
137 double f_2 = f3[1] / f3[2];
138 double p_1 = p3[0];
139 double p_2 = p3[1];
140
141 double cos_beta = f1.dot(f2);
142 double b = 1.0 / (1.0 - MATH_POW2(cos_beta)) - 1;
143
144 if (cos_beta < 0.0)
145 b = -std::sqrt(b);
146 else
147 b = std::sqrt(b);
148
149 /* Temporary pre-computed variables. */
150 double f_1_pw2 = MATH_POW2(f_1);
151 double f_2_pw2 = MATH_POW2(f_2);
152 double p_1_pw2 = MATH_POW2(p_1);
153 double p_1_pw3 = p_1_pw2 * p_1;
154 double p_1_pw4 = p_1_pw3 * p_1;
155 double p_2_pw2 = MATH_POW2(p_2);
156 double p_2_pw3 = p_2_pw2 * p_2;
157 double p_2_pw4 = p_2_pw3 * p_2;
158 double d_12_pw2 = MATH_POW2(d_12);
159 double b_pw2 = MATH_POW2(b);
160
161 /* Factors of the 4th degree polynomial. */
162 math::Vec5d factors;
163 factors[0] = - f_2_pw2 * p_2_pw4 - p_2_pw4 * f_1_pw2 - p_2_pw4;
164
165 factors[1] = 2.0 * p_2_pw3 * d_12 * b
166 + 2.0 * f_2_pw2 * p_2_pw3 * d_12 * b
167 - 2.0 * f_2 * p_2_pw3 * f_1 * d_12;
168
169 factors[2] = - f_2_pw2 * p_2_pw2 * p_1_pw2
170 - f_2_pw2 * p_2_pw2 * d_12_pw2 * b_pw2
171 - f_2_pw2 * p_2_pw2 * d_12_pw2
172 + f_2_pw2 * p_2_pw4
173 + p_2_pw4 * f_1_pw2
174 + 2.0 * p_1 * p_2_pw2 * d_12
175 + 2.0 * f_1 * f_2 * p_1 * p_2_pw2 * d_12 * b
176 - p_2_pw2 * p_1_pw2 * f_1_pw2
177 + 2.0 * p_1 * p_2_pw2 * f_2_pw2 * d_12
178 - p_2_pw2 * d_12_pw2 * b_pw2
179 - 2.0 * p_1_pw2 * p_2_pw2;
180
181 factors[3] = 2.0 * p_1_pw2 * p_2 * d_12 * b
182 + 2.0 * f_2 * p_2_pw3 * f_1 * d_12
183 - 2.0 * f_2_pw2 * p_2_pw3 * d_12 * b
184 - 2.0 * p_1 * p_2 * d_12_pw2 * b;
185
186 factors[4] = -2.0 * f_2 * p_2_pw2 * f_1 * p_1 * d_12 * b
187 + f_2_pw2 * p_2_pw2 * d_12_pw2
188 + 2.0 * p_1_pw3 * d_12
189 - p_1_pw2 * d_12_pw2
190 + f_2_pw2 * p_2_pw2 * p_1_pw2
191 - p_1_pw4
192 - 2.0 * f_2_pw2 * p_2_pw2 * p_1 * d_12
193 + p_2_pw2 * f_1_pw2 * p_1_pw2
194 + f_2_pw2 * p_2_pw2 * d_12_pw2 * b_pw2;
195
196 /* Solve for the roots of the polynomial. */
197 math::Vec4d real_roots;
198 solve_quartic_roots(factors, &real_roots);
199
200 /* Back-substitution of each solution. */
201 solutions->clear();
202 solutions->resize(4);
203 for (int i = 0; i < 4; ++i)
204 {
205 double cot_alpha = (-f_1 * p_1 / f_2 - real_roots[i] * p_2 + d_12 * b)
206 / (-f_1 * real_roots[i] * p_2 / f_2 + p_1 - d_12);
207
208 double cos_theta = real_roots[i];
209 double sin_theta = std::sqrt(1.0 - MATH_POW2(real_roots[i]));
210 double sin_alpha = std::sqrt(1.0 / (MATH_POW2(cot_alpha) + 1));
211 double cos_alpha = std::sqrt(1.0 - MATH_POW2(sin_alpha));
212
213 if (cot_alpha < 0.0)
214 cos_alpha = -cos_alpha;
215
216 math::Vec3d C(
217 d_12 * cos_alpha * (sin_alpha * b + cos_alpha),
218 cos_theta * d_12 * sin_alpha * (sin_alpha * b + cos_alpha),
219 sin_theta * d_12 * sin_alpha * (sin_alpha * b + cos_alpha));
220
221 C = p1 + N.transposed() * C;
222
224 R[0] = -cos_alpha; R[1] = -sin_alpha * cos_theta; R[2] = -sin_alpha * sin_theta;
225 R[3] = sin_alpha; R[4] = -cos_alpha * cos_theta; R[5] = -cos_alpha * sin_theta;
226 R[6] = 0.0; R[7] = -sin_theta; R[8] = cos_theta;
227
228 R = N.transposed().mult(R.transposed()).mult(T);
229
230 /* Convert camera position and cam-to-world rotation to pose. */
231 R = R.transposed();
232 C = -R * C;
233
234 solutions->at(i) = R.hstack(C);
235 }
236}
237
Matrix class for arbitrary dimensions and types.
Definition matrix.h:54
Vector< T, M > row(int index) const
Returns a row of the matrix as vector.
Definition matrix.h:323
Matrix< T, N, M+O > hstack(Matrix< T, N, O > const &other) const
Stacks this (left) and another matrix (right) horizontally.
Definition matrix.h:341
Matrix< T, M, N > transposed(void) const
Returns a transposed copy of self by treating rows as columns.
Definition matrix.h:448
T * begin(void)
Definition matrix.h:506
Vector class for arbitrary dimensions and types.
Definition vector.h:87
T square_norm(void) const
Computes the squared norm of the vector (much cheaper).
Definition vector.h:441
Vector< T, N > cross(Vector< T, N > const &other) const
Cross product between this and another vector.
Definition vector.h:549
T dot(Vector< T, N > const &other) const
Dot (or scalar) product between self and another vector.
Definition vector.h:542
Vector< T, N > & normalize(void)
Normalizes self and returns reference to self.
Definition vector.h:448
T * end(void)
Definition vector.h:597
T * begin(void)
Definition vector.h:583
#define MATH_EPSILON_EQ(x, v, eps)
Definition defines.h:96
#define MATH_POW2(x)
Definition defines.h:68
void pose_p3p_kneip(math::Vec3d p1, math::Vec3d p2, math::Vec3d p3, math::Vec3d f1, math::Vec3d f2, math::Vec3d f3, std::vector< math::Matrix< double, 3, 4 > > *solutions)
Implementation of the perspective three point (P3P) algorithm.
Definition pose_p3p.cc:74
void swap(mve::Image< T > &a, mve::Image< T > &b)
Specialization of std::swap for efficient image swapping.
Definition image.h:478
#define SFM_NAMESPACE_END
Definition defines.h:14
#define SFM_NAMESPACE_BEGIN
Definition defines.h:13