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];
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;
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;
42 double const alpha2 = alpha * alpha;
43 double const alpha3 = alpha2 * alpha;
44 double const beta2 = beta * beta;
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);
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;
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;
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)));
68 for (
int i = 0; i < 4; ++i)
69 (*real_roots)[i] = complex_roots[i].real();
80 double const colinear_threshold = 1e-10;
81 if ((p2 - p1).cross(p3 - p1).square_norm() < colinear_threshold)
88 double const normalize_epsilon = 1e-10;
108 if (T.
row(2).dot(f3) > 0.0)
135 double d_12 = (p2 - p1).norm();
136 double f_1 = f3[0] / f3[2];
137 double f_2 = f3[1] / f3[2];
141 double cos_beta = f1.
dot(f2);
142 double b = 1.0 / (1.0 -
MATH_POW2(cos_beta)) - 1;
153 double p_1_pw3 = p_1_pw2 * p_1;
154 double p_1_pw4 = p_1_pw3 * p_1;
156 double p_2_pw3 = p_2_pw2 * p_2;
157 double p_2_pw4 = p_2_pw3 * p_2;
163 factors[0] = - f_2_pw2 * p_2_pw4 - p_2_pw4 * f_1_pw2 - p_2_pw4;
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;
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
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;
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;
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
190 + f_2_pw2 * p_2_pw2 * p_1_pw2
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;
198 solve_quartic_roots(factors, &real_roots);
202 solutions->resize(4);
203 for (
int i = 0; i < 4; ++i)
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);
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));
214 cos_alpha = -cos_alpha;
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));
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;
234 solutions->at(i) = R.
hstack(C);
Matrix class for arbitrary dimensions and types.
Vector< T, M > row(int index) const
Returns a row of the matrix as vector.
Matrix< T, N, M+O > hstack(Matrix< T, N, O > const &other) const
Stacks this (left) and another matrix (right) horizontally.
Matrix< T, M, N > transposed(void) const
Returns a transposed copy of self by treating rows as columns.
Vector class for arbitrary dimensions and types.
T square_norm(void) const
Computes the squared norm of the vector (much cheaper).
Vector< T, N > cross(Vector< T, N > const &other) const
Cross product between this and another vector.
T dot(Vector< T, N > const &other) const
Dot (or scalar) product between self and another vector.
Vector< T, N > & normalize(void)
Normalizes self and returns reference to self.
#define MATH_EPSILON_EQ(x, v, eps)
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.
void swap(mve::Image< T > &a, mve::Image< T > &b)
Specialization of std::swap for efficient image swapping.
#define SFM_NAMESPACE_END
#define SFM_NAMESPACE_BEGIN