1 #include <cstring> 2 #include <cmath> 3 #include <iostream> 4 5 #include "polynom_solver.h" 6 #include "p3p.h" 7 8 void p3p::init_inverse_parameters() 9 { 10 inv_fx = 1. / fx; 11 inv_fy = 1. / fy; 12 cx_fx = cx / fx; 13 cy_fy = cy / fy; 14 } 15 16 p3p::p3p(cv::Mat cameraMatrix) 17 { 18 if (cameraMatrix.depth() == CV_32F) 19 init_camera_parameters<float>(cameraMatrix); 20 else 21 init_camera_parameters<double>(cameraMatrix); 22 init_inverse_parameters(); 23 } 24 25 p3p::p3p(double _fx, double _fy, double _cx, double _cy) 26 { 27 fx = _fx; 28 fy = _fy; 29 cx = _cx; 30 cy = _cy; 31 init_inverse_parameters(); 32 } 33 34 bool p3p::solve(cv::Mat& R, cv::Mat& tvec, const cv::Mat& opoints, const cv::Mat& ipoints) 35 { 36 double rotation_matrix[3][3], translation[3]; 37 std::vector<double> points; 38 if (opoints.depth() == ipoints.depth()) 39 { 40 if (opoints.depth() == CV_32F) 41 extract_points<cv::Point3f,cv::Point2f>(opoints, ipoints, points); 42 else 43 extract_points<cv::Point3d,cv::Point2d>(opoints, ipoints, points); 44 } 45 else if (opoints.depth() == CV_32F) 46 extract_points<cv::Point3f,cv::Point2d>(opoints, ipoints, points); 47 else 48 extract_points<cv::Point3d,cv::Point2f>(opoints, ipoints, points); 49 50 bool result = solve(rotation_matrix, translation, points[0], points[1], points[2], points[3], points[4], points[5], 51 points[6], points[7], points[8], points[9], points[10], points[11], points[12], points[13], points[14], 52 points[15], points[16], points[17], points[18], points[19]); 53 cv::Mat(3, 1, CV_64F, translation).copyTo(tvec); 54 cv::Mat(3, 3, CV_64F, rotation_matrix).copyTo(R); 55 return result; 56 } 57 58 bool p3p::solve(double R[3][3], double t[3], 59 double mu0, double mv0, double X0, double Y0, double Z0, 60 double mu1, double mv1, double X1, double Y1, double Z1, 61 double mu2, double mv2, double X2, double Y2, double Z2, 62 double mu3, double mv3, double X3, double Y3, double Z3) 63 { 64 double Rs[4][3][3], ts[4][3]; 65 66 int n = solve(Rs, ts, mu0, mv0, X0, Y0, Z0, mu1, mv1, X1, Y1, Z1, mu2, mv2, X2, Y2, Z2); 67 68 if (n == 0) 69 return false; 70 71 int ns = 0; 72 double min_reproj = 0; 73 for(int i = 0; i < n; i++) { 74 double X3p = Rs[i][0][0] * X3 + Rs[i][0][1] * Y3 + Rs[i][0][2] * Z3 + ts[i][0]; 75 double Y3p = Rs[i][1][0] * X3 + Rs[i][1][1] * Y3 + Rs[i][1][2] * Z3 + ts[i][1]; 76 double Z3p = Rs[i][2][0] * X3 + Rs[i][2][1] * Y3 + Rs[i][2][2] * Z3 + ts[i][2]; 77 double mu3p = cx + fx * X3p / Z3p; 78 double mv3p = cy + fy * Y3p / Z3p; 79 double reproj = (mu3p - mu3) * (mu3p - mu3) + (mv3p - mv3) * (mv3p - mv3); 80 if (i == 0 || min_reproj > reproj) { 81 ns = i; 82 min_reproj = reproj; 83 } 84 } 85 86 for(int i = 0; i < 3; i++) { 87 for(int j = 0; j < 3; j++) 88 R[i][j] = Rs[ns][i][j]; 89 t[i] = ts[ns][i]; 90 } 91 92 return true; 93 } 94 95 int p3p::solve(double R[4][3][3], double t[4][3], 96 double mu0, double mv0, double X0, double Y0, double Z0, 97 double mu1, double mv1, double X1, double Y1, double Z1, 98 double mu2, double mv2, double X2, double Y2, double Z2) 99 { 100 double mk0, mk1, mk2; 101 double norm; 102 103 mu0 = inv_fx * mu0 - cx_fx; 104 mv0 = inv_fy * mv0 - cy_fy; 105 norm = sqrt(mu0 * mu0 + mv0 * mv0 + 1); 106 mk0 = 1. / norm; mu0 *= mk0; mv0 *= mk0; 107 108 mu1 = inv_fx * mu1 - cx_fx; 109 mv1 = inv_fy * mv1 - cy_fy; 110 norm = sqrt(mu1 * mu1 + mv1 * mv1 + 1); 111 mk1 = 1. / norm; mu1 *= mk1; mv1 *= mk1; 112 113 mu2 = inv_fx * mu2 - cx_fx; 114 mv2 = inv_fy * mv2 - cy_fy; 115 norm = sqrt(mu2 * mu2 + mv2 * mv2 + 1); 116 mk2 = 1. / norm; mu2 *= mk2; mv2 *= mk2; 117 118 double distances[3]; 119 distances[0] = sqrt( (X1 - X2) * (X1 - X2) + (Y1 - Y2) * (Y1 - Y2) + (Z1 - Z2) * (Z1 - Z2) ); 120 distances[1] = sqrt( (X0 - X2) * (X0 - X2) + (Y0 - Y2) * (Y0 - Y2) + (Z0 - Z2) * (Z0 - Z2) ); 121 distances[2] = sqrt( (X0 - X1) * (X0 - X1) + (Y0 - Y1) * (Y0 - Y1) + (Z0 - Z1) * (Z0 - Z1) ); 122 123 // Calculate angles 124 double cosines[3]; 125 cosines[0] = mu1 * mu2 + mv1 * mv2 + mk1 * mk2; 126 cosines[1] = mu0 * mu2 + mv0 * mv2 + mk0 * mk2; 127 cosines[2] = mu0 * mu1 + mv0 * mv1 + mk0 * mk1; 128 129 double lengths[4][3]; 130 int n = solve_for_lengths(lengths, distances, cosines); 131 132 int nb_solutions = 0; 133 for(int i = 0; i < n; i++) { 134 double M_orig[3][3]; 135 136 M_orig[0][0] = lengths[i][0] * mu0; 137 M_orig[0][1] = lengths[i][0] * mv0; 138 M_orig[0][2] = lengths[i][0] * mk0; 139 140 M_orig[1][0] = lengths[i][1] * mu1; 141 M_orig[1][1] = lengths[i][1] * mv1; 142 M_orig[1][2] = lengths[i][1] * mk1; 143 144 M_orig[2][0] = lengths[i][2] * mu2; 145 M_orig[2][1] = lengths[i][2] * mv2; 146 M_orig[2][2] = lengths[i][2] * mk2; 147 148 if (!align(M_orig, X0, Y0, Z0, X1, Y1, Z1, X2, Y2, Z2, R[nb_solutions], t[nb_solutions])) 149 continue; 150 151 nb_solutions++; 152 } 153 154 return nb_solutions; 155 } 156 157 /// Given 3D distances between three points and cosines of 3 angles at the apex, calculates 158 /// the lentghs of the line segments connecting projection center (P) and the three 3D points (A, B, C). 159 /// Returned distances are for |PA|, |PB|, |PC| respectively. 160 /// Only the solution to the main branch. 161 /// Reference : X.S. Gao, X.-R. Hou, J. Tang, H.-F. Chang; "Complete Solution Classification for the Perspective-Three-Point Problem" 162 /// IEEE Trans. on PAMI, vol. 25, No. 8, August 2003 163 /// \param lengths3D Lengths of line segments up to four solutions. 164 /// \param dist3D Distance between 3D points in pairs |BC|, |AC|, |AB|. 165 /// \param cosines Cosine of the angles /_BPC, /_APC, /_APB. 166 /// \returns Number of solutions. 167 /// WARNING: NOT ALL THE DEGENERATE CASES ARE IMPLEMENTED 168 169 int p3p::solve_for_lengths(double lengths[4][3], double distances[3], double cosines[3]) 170 { 171 double p = cosines[0] * 2; 172 double q = cosines[1] * 2; 173 double r = cosines[2] * 2; 174 175 double inv_d22 = 1. / (distances[2] * distances[2]); 176 double a = inv_d22 * (distances[0] * distances[0]); 177 double b = inv_d22 * (distances[1] * distances[1]); 178 179 double a2 = a * a, b2 = b * b, p2 = p * p, q2 = q * q, r2 = r * r; 180 double pr = p * r, pqr = q * pr; 181 182 // Check reality condition (the four points should not be coplanar) 183 if (p2 + q2 + r2 - pqr - 1 == 0) 184 return 0; 185 186 double ab = a * b, a_2 = 2*a; 187 188 double A = -2 * b + b2 + a2 + 1 + ab*(2 - r2) - a_2; 189 190 // Check reality condition 191 if (A == 0) return 0; 192 193 double a_4 = 4*a; 194 195 double B = q*(-2*(ab + a2 + 1 - b) + r2*ab + a_4) + pr*(b - b2 + ab); 196 double C = q2 + b2*(r2 + p2 - 2) - b*(p2 + pqr) - ab*(r2 + pqr) + (a2 - a_2)*(2 + q2) + 2; 197 double D = pr*(ab-b2+b) + q*((p2-2)*b + 2 * (ab - a2) + a_4 - 2); 198 double E = 1 + 2*(b - a - ab) + b2 - b*p2 + a2; 199 200 double temp = (p2*(a-1+b) + r2*(a-1-b) + pqr - a*pqr); 201 double b0 = b * temp * temp; 202 // Check reality condition 203 if (b0 == 0) 204 return 0; 205 206 double real_roots[4]; 207 int n = solve_deg4(A, B, C, D, E, real_roots[0], real_roots[1], real_roots[2], real_roots[3]); 208 209 if (n == 0) 210 return 0; 211 212 int nb_solutions = 0; 213 double r3 = r2*r, pr2 = p*r2, r3q = r3 * q; 214 double inv_b0 = 1. / b0; 215 216 // For each solution of x 217 for(int i = 0; i < n; i++) { 218 double x = real_roots[i]; 219 220 // Check reality condition 221 if (x <= 0) 222 continue; 223 224 double x2 = x*x; 225 226 double b1 = 227 ((1-a-b)*x2 + (q*a-q)*x + 1 - a + b) * 228 (((r3*(a2 + ab*(2 - r2) - a_2 + b2 - 2*b + 1)) * x + 229 230 (r3q*(2*(b-a2) + a_4 + ab*(r2 - 2) - 2) + pr2*(1 + a2 + 2*(ab-a-b) + r2*(b - b2) + b2))) * x2 + 231 232 (r3*(q2*(1-2*a+a2) + r2*(b2-ab) - a_4 + 2*(a2 - b2) + 2) + r*p2*(b2 + 2*(ab - b - a) + 1 + a2) + pr2*q*(a_4 + 2*(b - ab - a2) - 2 - r2*b)) * x + 233 234 2*r3q*(a_2 - b - a2 + ab - 1) + pr2*(q2 - a_4 + 2*(a2 - b2) + r2*b + q2*(a2 - a_2) + 2) + 235 p2*(p*(2*(ab - a - b) + a2 + b2 + 1) + 2*q*r*(b + a_2 - a2 - ab - 1))); 236 237 // Check reality condition 238 if (b1 <= 0) 239 continue; 240 241 double y = inv_b0 * b1; 242 double v = x2 + y*y - x*y*r; 243 244 if (v <= 0) 245 continue; 246 247 double Z = distances[2] / sqrt(v); 248 double X = x * Z; 249 double Y = y * Z; 250 251 lengths[nb_solutions][0] = X; 252 lengths[nb_solutions][1] = Y; 253 lengths[nb_solutions][2] = Z; 254 255 nb_solutions++; 256 } 257 258 return nb_solutions; 259 } 260 261 bool p3p::align(double M_end[3][3], 262 double X0, double Y0, double Z0, 263 double X1, double Y1, double Z1, 264 double X2, double Y2, double Z2, 265 double R[3][3], double T[3]) 266 { 267 // Centroids: 268 double C_start[3], C_end[3]; 269 for(int i = 0; i < 3; i++) C_end[i] = (M_end[0][i] + M_end[1][i] + M_end[2][i]) / 3; 270 C_start[0] = (X0 + X1 + X2) / 3; 271 C_start[1] = (Y0 + Y1 + Y2) / 3; 272 C_start[2] = (Z0 + Z1 + Z2) / 3; 273 274 // Covariance matrix s: 275 double s[3 * 3]; 276 for(int j = 0; j < 3; j++) { 277 s[0 * 3 + j] = (X0 * M_end[0][j] + X1 * M_end[1][j] + X2 * M_end[2][j]) / 3 - C_end[j] * C_start[0]; 278 s[1 * 3 + j] = (Y0 * M_end[0][j] + Y1 * M_end[1][j] + Y2 * M_end[2][j]) / 3 - C_end[j] * C_start[1]; 279 s[2 * 3 + j] = (Z0 * M_end[0][j] + Z1 * M_end[1][j] + Z2 * M_end[2][j]) / 3 - C_end[j] * C_start[2]; 280 } 281 282 double Qs[16], evs[4], U[16]; 283 284 Qs[0 * 4 + 0] = s[0 * 3 + 0] + s[1 * 3 + 1] + s[2 * 3 + 2]; 285 Qs[1 * 4 + 1] = s[0 * 3 + 0] - s[1 * 3 + 1] - s[2 * 3 + 2]; 286 Qs[2 * 4 + 2] = s[1 * 3 + 1] - s[2 * 3 + 2] - s[0 * 3 + 0]; 287 Qs[3 * 4 + 3] = s[2 * 3 + 2] - s[0 * 3 + 0] - s[1 * 3 + 1]; 288 289 Qs[1 * 4 + 0] = Qs[0 * 4 + 1] = s[1 * 3 + 2] - s[2 * 3 + 1]; 290 Qs[2 * 4 + 0] = Qs[0 * 4 + 2] = s[2 * 3 + 0] - s[0 * 3 + 2]; 291 Qs[3 * 4 + 0] = Qs[0 * 4 + 3] = s[0 * 3 + 1] - s[1 * 3 + 0]; 292 Qs[2 * 4 + 1] = Qs[1 * 4 + 2] = s[1 * 3 + 0] + s[0 * 3 + 1]; 293 Qs[3 * 4 + 1] = Qs[1 * 4 + 3] = s[2 * 3 + 0] + s[0 * 3 + 2]; 294 Qs[3 * 4 + 2] = Qs[2 * 4 + 3] = s[2 * 3 + 1] + s[1 * 3 + 2]; 295 296 jacobi_4x4(Qs, evs, U); 297 298 // Looking for the largest eigen value: 299 int i_ev = 0; 300 double ev_max = evs[i_ev]; 301 for(int i = 1; i < 4; i++) 302 if (evs[i] > ev_max) 303 ev_max = evs[i_ev = i]; 304 305 // Quaternion: 306 double q[4]; 307 for(int i = 0; i < 4; i++) 308 q[i] = U[i * 4 + i_ev]; 309 310 double q02 = q[0] * q[0], q12 = q[1] * q[1], q22 = q[2] * q[2], q32 = q[3] * q[3]; 311 double q0_1 = q[0] * q[1], q0_2 = q[0] * q[2], q0_3 = q[0] * q[3]; 312 double q1_2 = q[1] * q[2], q1_3 = q[1] * q[3]; 313 double q2_3 = q[2] * q[3]; 314 315 R[0][0] = q02 + q12 - q22 - q32; 316 R[0][1] = 2. * (q1_2 - q0_3); 317 R[0][2] = 2. * (q1_3 + q0_2); 318 319 R[1][0] = 2. * (q1_2 + q0_3); 320 R[1][1] = q02 + q22 - q12 - q32; 321 R[1][2] = 2. * (q2_3 - q0_1); 322 323 R[2][0] = 2. * (q1_3 - q0_2); 324 R[2][1] = 2. * (q2_3 + q0_1); 325 R[2][2] = q02 + q32 - q12 - q22; 326 327 for(int i = 0; i < 3; i++) 328 T[i] = C_end[i] - (R[i][0] * C_start[0] + R[i][1] * C_start[1] + R[i][2] * C_start[2]); 329 330 return true; 331 } 332 333 bool p3p::jacobi_4x4(double * A, double * D, double * U) 334 { 335 double B[4], Z[4]; 336 double Id[16] = {1., 0., 0., 0., 337 0., 1., 0., 0., 338 0., 0., 1., 0., 339 0., 0., 0., 1.}; 340 341 memcpy(U, Id, 16 * sizeof(double)); 342 343 B[0] = A[0]; B[1] = A[5]; B[2] = A[10]; B[3] = A[15]; 344 memcpy(D, B, 4 * sizeof(double)); 345 memset(Z, 0, 4 * sizeof(double)); 346 347 for(int iter = 0; iter < 50; iter++) { 348 double sum = fabs(A[1]) + fabs(A[2]) + fabs(A[3]) + fabs(A[6]) + fabs(A[7]) + fabs(A[11]); 349 350 if (sum == 0.0) 351 return true; 352 353 double tresh = (iter < 3) ? 0.2 * sum / 16. : 0.0; 354 for(int i = 0; i < 3; i++) { 355 double * pAij = A + 5 * i + 1; 356 for(int j = i + 1 ; j < 4; j++) { 357 double Aij = *pAij; 358 double eps_machine = 100.0 * fabs(Aij); 359 360 if ( iter > 3 && fabs(D[i]) + eps_machine == fabs(D[i]) && fabs(D[j]) + eps_machine == fabs(D[j]) ) 361 *pAij = 0.0; 362 else if (fabs(Aij) > tresh) { 363 double hh = D[j] - D[i], t; 364 if (fabs(hh) + eps_machine == fabs(hh)) 365 t = Aij / hh; 366 else { 367 double theta = 0.5 * hh / Aij; 368 t = 1.0 / (fabs(theta) + sqrt(1.0 + theta * theta)); 369 if (theta < 0.0) t = -t; 370 } 371 372 hh = t * Aij; 373 Z[i] -= hh; 374 Z[j] += hh; 375 D[i] -= hh; 376 D[j] += hh; 377 *pAij = 0.0; 378 379 double c = 1.0 / sqrt(1 + t * t); 380 double s = t * c; 381 double tau = s / (1.0 + c); 382 for(int k = 0; k <= i - 1; k++) { 383 double g = A[k * 4 + i], h = A[k * 4 + j]; 384 A[k * 4 + i] = g - s * (h + g * tau); 385 A[k * 4 + j] = h + s * (g - h * tau); 386 } 387 for(int k = i + 1; k <= j - 1; k++) { 388 double g = A[i * 4 + k], h = A[k * 4 + j]; 389 A[i * 4 + k] = g - s * (h + g * tau); 390 A[k * 4 + j] = h + s * (g - h * tau); 391 } 392 for(int k = j + 1; k < 4; k++) { 393 double g = A[i * 4 + k], h = A[j * 4 + k]; 394 A[i * 4 + k] = g - s * (h + g * tau); 395 A[j * 4 + k] = h + s * (g - h * tau); 396 } 397 for(int k = 0; k < 4; k++) { 398 double g = U[k * 4 + i], h = U[k * 4 + j]; 399 U[k * 4 + i] = g - s * (h + g * tau); 400 U[k * 4 + j] = h + s * (g - h * tau); 401 } 402 } 403 pAij++; 404 } 405 } 406 407 for(int i = 0; i < 4; i++) B[i] += Z[i]; 408 memcpy(D, B, 4 * sizeof(double)); 409 memset(Z, 0, 4 * sizeof(double)); 410 } 411 412 return false; 413 } 414
