1 //M*////////////////////////////////////////////////////////////////////////////////////// 2 // 3 // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. 4 // 5 // By downloading, copying, installing or using the software you agree to this license. 6 // If you do not agree to this license, do not download, install, 7 // copy or use the software. 8 // 9 // 10 // License Agreement 11 // For Open Source Computer Vision Library 12 // 13 // Copyright (C) 2000, Intel Corporation, all rights reserved. 14 // Copyright (C) 2013, OpenCV Foundation, all rights reserved. 15 // Third party copyrights are property of their respective owners. 16 // 17 // Redistribution and use in source and binary forms, with or without modification, 18 // are permitted provided that the following conditions are met: 19 // 20 // * Redistribution's of source code must retain the above copyright notice, 21 // this list of conditions and the following disclaimer. 22 // 23 // * Redistribution's in binary form must reproduce the above copyright notice, 24 // this list of conditions and the following disclaimer in the documentation 25 // and/or other materials provided with the distribution. 26 // 27 // * The name of the copyright holders may not be used to endorse or promote products 28 // derived from this software without specific prior written permission. 29 // 30 // This software is provided by the copyright holders and contributors "as is" and 31 // any express or implied warranties, including, but not limited to, the implied 32 // warranties of merchantability and fitness for a particular purpose are disclaimed. 33 // In no event shall the Intel Corporation or contributors be liable for any direct, 34 // indirect, incidental, special, exemplary, or consequential damages 35 // (including, but not limited to, procurement of substitute goods or services; 36 // loss of use, data, or profits; or business interruption) however caused 37 // and on any theory of liability, whether in contract, strict liability, 38 // or tort (including negligence or otherwise) arising in any way out of 39 // the use of this software, even if advised of the possibility of such damage. 40 // 41 //M*/ 42 43 /****************************************************************************************\ 44 * Exhaustive Linearization for Robust Camera Pose and Focal Length Estimation. 45 * Contributed by Edgar Riba 46 \****************************************************************************************/ 47 48 #include "precomp.hpp" 49 #include "upnp.h" 50 #include <limits> 51 52 using namespace std; 53 using namespace cv; 54 55 upnp::upnp(const Mat& cameraMatrix, const Mat& opoints, const Mat& ipoints) 56 { 57 if (cameraMatrix.depth() == CV_32F) 58 init_camera_parameters<float>(cameraMatrix); 59 else 60 init_camera_parameters<double>(cameraMatrix); 61 62 number_of_correspondences = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F)); 63 64 pws.resize(3 * number_of_correspondences); 65 us.resize(2 * number_of_correspondences); 66 67 if (opoints.depth() == ipoints.depth()) 68 { 69 if (opoints.depth() == CV_32F) 70 init_points<Point3f,Point2f>(opoints, ipoints); 71 else 72 init_points<Point3d,Point2d>(opoints, ipoints); 73 } 74 else if (opoints.depth() == CV_32F) 75 init_points<Point3f,Point2d>(opoints, ipoints); 76 else 77 init_points<Point3d,Point2f>(opoints, ipoints); 78 79 alphas.resize(4 * number_of_correspondences); 80 pcs.resize(3 * number_of_correspondences); 81 82 max_nr = 0; 83 A1 = NULL; 84 A2 = NULL; 85 } 86 87 upnp::~upnp() 88 { 89 if (A1) 90 delete[] A1; 91 if (A2) 92 delete[] A2; 93 } 94 95 double upnp::compute_pose(Mat& R, Mat& t) 96 { 97 choose_control_points(); 98 compute_alphas(); 99 100 Mat * M = new Mat(2 * number_of_correspondences, 12, CV_64F); 101 102 for(int i = 0; i < number_of_correspondences; i++) 103 { 104 fill_M(M, 2 * i, &alphas[0] + 4 * i, us[2 * i], us[2 * i + 1]); 105 } 106 107 double mtm[12 * 12], d[12], ut[12 * 12], vt[12 * 12]; 108 Mat MtM = Mat(12, 12, CV_64F, mtm); 109 Mat D = Mat(12, 1, CV_64F, d); 110 Mat Ut = Mat(12, 12, CV_64F, ut); 111 Mat Vt = Mat(12, 12, CV_64F, vt); 112 113 MtM = M->t() * (*M); 114 SVD::compute(MtM, D, Ut, Vt, SVD::MODIFY_A | SVD::FULL_UV); 115 Mat(Ut.t()).copyTo(Ut); 116 M->release(); 117 118 double l_6x12[6 * 12], rho[6]; 119 Mat L_6x12 = Mat(6, 12, CV_64F, l_6x12); 120 Mat Rho = Mat(6, 1, CV_64F, rho); 121 122 compute_L_6x12(ut, l_6x12); 123 compute_rho(rho); 124 125 double Betas[3][4], Efs[3][1], rep_errors[3]; 126 double Rs[3][3][3], ts[3][3]; 127 128 find_betas_and_focal_approx_1(&Ut, &Rho, Betas[1], Efs[1]); 129 gauss_newton(&L_6x12, &Rho, Betas[1], Efs[1]); 130 rep_errors[1] = compute_R_and_t(ut, Betas[1], Rs[1], ts[1]); 131 132 find_betas_and_focal_approx_2(&Ut, &Rho, Betas[2], Efs[2]); 133 gauss_newton(&L_6x12, &Rho, Betas[2], Efs[2]); 134 rep_errors[2] = compute_R_and_t(ut, Betas[2], Rs[2], ts[2]); 135 136 int N = 1; 137 if (rep_errors[2] < rep_errors[1]) N = 2; 138 139 Mat(3, 1, CV_64F, ts[N]).copyTo(t); 140 Mat(3, 3, CV_64F, Rs[N]).copyTo(R); 141 fu = fv = Efs[N][0]; 142 143 return fu; 144 } 145 146 void upnp::copy_R_and_t(const double R_src[3][3], const double t_src[3], 147 double R_dst[3][3], double t_dst[3]) 148 { 149 for(int i = 0; i < 3; i++) { 150 for(int j = 0; j < 3; j++) 151 R_dst[i][j] = R_src[i][j]; 152 t_dst[i] = t_src[i]; 153 } 154 } 155 156 void upnp::estimate_R_and_t(double R[3][3], double t[3]) 157 { 158 double pc0[3], pw0[3]; 159 160 pc0[0] = pc0[1] = pc0[2] = 0.0; 161 pw0[0] = pw0[1] = pw0[2] = 0.0; 162 163 for(int i = 0; i < number_of_correspondences; i++) { 164 const double * pc = &pcs[3 * i]; 165 const double * pw = &pws[3 * i]; 166 167 for(int j = 0; j < 3; j++) { 168 pc0[j] += pc[j]; 169 pw0[j] += pw[j]; 170 } 171 } 172 for(int j = 0; j < 3; j++) { 173 pc0[j] /= number_of_correspondences; 174 pw0[j] /= number_of_correspondences; 175 } 176 177 double abt[3 * 3], abt_d[3], abt_u[3 * 3], abt_v[3 * 3]; 178 Mat ABt = Mat(3, 3, CV_64F, abt); 179 Mat ABt_D = Mat(3, 1, CV_64F, abt_d); 180 Mat ABt_U = Mat(3, 3, CV_64F, abt_u); 181 Mat ABt_V = Mat(3, 3, CV_64F, abt_v); 182 183 ABt.setTo(0.0); 184 for(int i = 0; i < number_of_correspondences; i++) { 185 double * pc = &pcs[3 * i]; 186 double * pw = &pws[3 * i]; 187 188 for(int j = 0; j < 3; j++) { 189 abt[3 * j ] += (pc[j] - pc0[j]) * (pw[0] - pw0[0]); 190 abt[3 * j + 1] += (pc[j] - pc0[j]) * (pw[1] - pw0[1]); 191 abt[3 * j + 2] += (pc[j] - pc0[j]) * (pw[2] - pw0[2]); 192 } 193 } 194 195 SVD::compute(ABt, ABt_D, ABt_U, ABt_V, SVD::MODIFY_A); 196 Mat(ABt_V.t()).copyTo(ABt_V); 197 198 for(int i = 0; i < 3; i++) 199 for(int j = 0; j < 3; j++) 200 R[i][j] = dot(abt_u + 3 * i, abt_v + 3 * j); 201 202 const double det = 203 R[0][0] * R[1][1] * R[2][2] + R[0][1] * R[1][2] * R[2][0] + R[0][2] * R[1][0] * R[2][1] - 204 R[0][2] * R[1][1] * R[2][0] - R[0][1] * R[1][0] * R[2][2] - R[0][0] * R[1][2] * R[2][1]; 205 206 if (det < 0) { 207 R[2][0] = -R[2][0]; 208 R[2][1] = -R[2][1]; 209 R[2][2] = -R[2][2]; 210 } 211 212 t[0] = pc0[0] - dot(R[0], pw0); 213 t[1] = pc0[1] - dot(R[1], pw0); 214 t[2] = pc0[2] - dot(R[2], pw0); 215 } 216 217 void upnp::solve_for_sign(void) 218 { 219 if (pcs[2] < 0.0) { 220 for(int i = 0; i < 4; i++) 221 for(int j = 0; j < 3; j++) 222 ccs[i][j] = -ccs[i][j]; 223 224 for(int i = 0; i < number_of_correspondences; i++) { 225 pcs[3 * i ] = -pcs[3 * i]; 226 pcs[3 * i + 1] = -pcs[3 * i + 1]; 227 pcs[3 * i + 2] = -pcs[3 * i + 2]; 228 } 229 } 230 } 231 232 double upnp::compute_R_and_t(const double * ut, const double * betas, 233 double R[3][3], double t[3]) 234 { 235 compute_ccs(betas, ut); 236 compute_pcs(); 237 238 solve_for_sign(); 239 240 estimate_R_and_t(R, t); 241 242 return reprojection_error(R, t); 243 } 244 245 double upnp::reprojection_error(const double R[3][3], const double t[3]) 246 { 247 double sum2 = 0.0; 248 249 for(int i = 0; i < number_of_correspondences; i++) { 250 double * pw = &pws[3 * i]; 251 double Xc = dot(R[0], pw) + t[0]; 252 double Yc = dot(R[1], pw) + t[1]; 253 double inv_Zc = 1.0 / (dot(R[2], pw) + t[2]); 254 double ue = uc + fu * Xc * inv_Zc; 255 double ve = vc + fv * Yc * inv_Zc; 256 double u = us[2 * i], v = us[2 * i + 1]; 257 258 sum2 += sqrt( (u - ue) * (u - ue) + (v - ve) * (v - ve) ); 259 } 260 261 return sum2 / number_of_correspondences; 262 } 263 264 void upnp::choose_control_points() 265 { 266 for (int i = 0; i < 4; ++i) 267 cws[i][0] = cws[i][1] = cws[i][2] = 0.0; 268 cws[0][0] = cws[1][1] = cws[2][2] = 1.0; 269 } 270 271 void upnp::compute_alphas() 272 { 273 Mat CC = Mat(4, 3, CV_64F, &cws); 274 Mat PC = Mat(number_of_correspondences, 3, CV_64F, &pws[0]); 275 Mat ALPHAS = Mat(number_of_correspondences, 4, CV_64F, &alphas[0]); 276 277 Mat CC_ = CC.clone().t(); 278 Mat PC_ = PC.clone().t(); 279 280 Mat row14 = Mat::ones(1, 4, CV_64F); 281 Mat row1n = Mat::ones(1, number_of_correspondences, CV_64F); 282 283 CC_.push_back(row14); 284 PC_.push_back(row1n); 285 286 ALPHAS = Mat( CC_.inv() * PC_ ).t(); 287 } 288 289 void upnp::fill_M(Mat * M, const int row, const double * as, const double u, const double v) 290 { 291 double * M1 = M->ptr<double>(row); 292 double * M2 = M1 + 12; 293 294 for(int i = 0; i < 4; i++) { 295 M1[3 * i ] = as[i] * fu; 296 M1[3 * i + 1] = 0.0; 297 M1[3 * i + 2] = as[i] * (uc - u); 298 299 M2[3 * i ] = 0.0; 300 M2[3 * i + 1] = as[i] * fv; 301 M2[3 * i + 2] = as[i] * (vc - v); 302 } 303 } 304 305 void upnp::compute_ccs(const double * betas, const double * ut) 306 { 307 for(int i = 0; i < 4; ++i) 308 ccs[i][0] = ccs[i][1] = ccs[i][2] = 0.0; 309 310 int N = 4; 311 for(int i = 0; i < N; ++i) { 312 const double * v = ut + 12 * (9 + i); 313 for(int j = 0; j < 4; ++j) 314 for(int k = 0; k < 3; ++k) 315 ccs[j][k] += betas[i] * v[3 * j + k]; 316 } 317 318 for (int i = 0; i < 4; ++i) ccs[i][2] *= fu; 319 } 320 321 void upnp::compute_pcs(void) 322 { 323 for(int i = 0; i < number_of_correspondences; i++) { 324 double * a = &alphas[0] + 4 * i; 325 double * pc = &pcs[0] + 3 * i; 326 327 for(int j = 0; j < 3; j++) 328 pc[j] = a[0] * ccs[0][j] + a[1] * ccs[1][j] + a[2] * ccs[2][j] + a[3] * ccs[3][j]; 329 } 330 } 331 332 void upnp::find_betas_and_focal_approx_1(Mat * Ut, Mat * Rho, double * betas, double * efs) 333 { 334 Mat Kmf1 = Mat(12, 1, CV_64F, Ut->ptr<double>(11)); 335 Mat dsq = Mat(6, 1, CV_64F, Rho->ptr<double>(0)); 336 337 Mat D = compute_constraint_distance_2param_6eq_2unk_f_unk( Kmf1 ); 338 Mat Dt = D.t(); 339 340 Mat A = Dt * D; 341 Mat b = Dt * dsq; 342 343 Mat x = Mat(2, 1, CV_64F); 344 solve(A, b, x); 345 346 betas[0] = sqrt( abs( x.at<double>(0) ) ); 347 betas[1] = betas[2] = betas[3] = 0.0; 348 349 efs[0] = sqrt( abs( x.at<double>(1) ) ) / betas[0]; 350 } 351 352 void upnp::find_betas_and_focal_approx_2(Mat * Ut, Mat * Rho, double * betas, double * efs) 353 { 354 double u[12*12]; 355 Mat U = Mat(12, 12, CV_64F, u); 356 Ut->copyTo(U); 357 358 Mat Kmf1 = Mat(12, 1, CV_64F, Ut->ptr<double>(10)); 359 Mat Kmf2 = Mat(12, 1, CV_64F, Ut->ptr<double>(11)); 360 Mat dsq = Mat(6, 1, CV_64F, Rho->ptr<double>(0)); 361 362 Mat D = compute_constraint_distance_3param_6eq_6unk_f_unk( Kmf1, Kmf2 ); 363 364 Mat A = D; 365 Mat b = dsq; 366 367 double x[6]; 368 Mat X = Mat(6, 1, CV_64F, x); 369 370 solve(A, b, X, DECOMP_QR); 371 372 double solutions[18][3]; 373 generate_all_possible_solutions_for_f_unk(x, solutions); 374 375 // find solution with minimum reprojection error 376 double min_error = std::numeric_limits<double>::max(); 377 int min_sol = 0; 378 for (int i = 0; i < 18; ++i) { 379 380 betas[3] = solutions[i][0]; 381 betas[2] = solutions[i][1]; 382 betas[1] = betas[0] = 0.0; 383 fu = fv = solutions[i][2]; 384 385 double Rs[3][3], ts[3]; 386 double error_i = compute_R_and_t( u, betas, Rs, ts); 387 388 if( error_i < min_error) 389 { 390 min_error = error_i; 391 min_sol = i; 392 } 393 } 394 395 betas[0] = solutions[min_sol][0]; 396 betas[1] = solutions[min_sol][1]; 397 betas[2] = betas[3] = 0.0; 398 399 efs[0] = solutions[min_sol][2]; 400 } 401 402 Mat upnp::compute_constraint_distance_2param_6eq_2unk_f_unk(const Mat& M1) 403 { 404 Mat P = Mat(6, 2, CV_64F); 405 406 double m[13]; 407 for (int i = 1; i < 13; ++i) m[i] = *M1.ptr<double>(i-1); 408 409 double t1 = pow( m[4], 2 ); 410 double t4 = pow( m[1], 2 ); 411 double t5 = pow( m[5], 2 ); 412 double t8 = pow( m[2], 2 ); 413 double t10 = pow( m[6], 2 ); 414 double t13 = pow( m[3], 2 ); 415 double t15 = pow( m[7], 2 ); 416 double t18 = pow( m[8], 2 ); 417 double t22 = pow( m[9], 2 ); 418 double t26 = pow( m[10], 2 ); 419 double t29 = pow( m[11], 2 ); 420 double t33 = pow( m[12], 2 ); 421 422 *P.ptr<double>(0,0) = t1 - 2 * m[4] * m[1] + t4 + t5 - 2 * m[5] * m[2] + t8; 423 *P.ptr<double>(0,1) = t10 - 2 * m[6] * m[3] + t13; 424 *P.ptr<double>(1,0) = t15 - 2 * m[7] * m[1] + t4 + t18 - 2 * m[8] * m[2] + t8; 425 *P.ptr<double>(1,1) = t22 - 2 * m[9] * m[3] + t13; 426 *P.ptr<double>(2,0) = t26 - 2 * m[10] * m[1] + t4 + t29 - 2 * m[11] * m[2] + t8; 427 *P.ptr<double>(2,1) = t33 - 2 * m[12] * m[3] + t13; 428 *P.ptr<double>(3,0) = t15 - 2 * m[7] * m[4] + t1 + t18 - 2 * m[8] * m[5] + t5; 429 *P.ptr<double>(3,1) = t22 - 2 * m[9] * m[6] + t10; 430 *P.ptr<double>(4,0) = t26 - 2 * m[10] * m[4] + t1 + t29 - 2 * m[11] * m[5] + t5; 431 *P.ptr<double>(4,1) = t33 - 2 * m[12] * m[6] + t10; 432 *P.ptr<double>(5,0) = t26 - 2 * m[10] * m[7] + t15 + t29 - 2 * m[11] * m[8] + t18; 433 *P.ptr<double>(5,1) = t33 - 2 * m[12] * m[9] + t22; 434 435 return P; 436 } 437 438 Mat upnp::compute_constraint_distance_3param_6eq_6unk_f_unk(const Mat& M1, const Mat& M2) 439 { 440 Mat P = Mat(6, 6, CV_64F); 441 442 double m[3][13]; 443 for (int i = 1; i < 13; ++i) 444 { 445 m[1][i] = *M1.ptr<double>(i-1); 446 m[2][i] = *M2.ptr<double>(i-1); 447 } 448 449 double t1 = pow( m[1][4], 2 ); 450 double t2 = pow( m[1][1], 2 ); 451 double t7 = pow( m[1][5], 2 ); 452 double t8 = pow( m[1][2], 2 ); 453 double t11 = m[1][1] * m[2][1]; 454 double t12 = m[1][5] * m[2][5]; 455 double t15 = m[1][2] * m[2][2]; 456 double t16 = m[1][4] * m[2][4]; 457 double t19 = pow( m[2][4], 2 ); 458 double t22 = pow( m[2][2], 2 ); 459 double t23 = pow( m[2][1], 2 ); 460 double t24 = pow( m[2][5], 2 ); 461 double t28 = pow( m[1][6], 2 ); 462 double t29 = pow( m[1][3], 2 ); 463 double t34 = pow( m[1][3], 2 ); 464 double t36 = m[1][6] * m[2][6]; 465 double t40 = pow( m[2][6], 2 ); 466 double t41 = pow( m[2][3], 2 ); 467 double t47 = pow( m[1][7], 2 ); 468 double t48 = pow( m[1][8], 2 ); 469 double t52 = m[1][7] * m[2][7]; 470 double t55 = m[1][8] * m[2][8]; 471 double t59 = pow( m[2][8], 2 ); 472 double t62 = pow( m[2][7], 2 ); 473 double t64 = pow( m[1][9], 2 ); 474 double t68 = m[1][9] * m[2][9]; 475 double t74 = pow( m[2][9], 2 ); 476 double t78 = pow( m[1][10], 2 ); 477 double t79 = pow( m[1][11], 2 ); 478 double t84 = m[1][10] * m[2][10]; 479 double t87 = m[1][11] * m[2][11]; 480 double t90 = pow( m[2][10], 2 ); 481 double t95 = pow( m[2][11], 2 ); 482 double t99 = pow( m[1][12], 2 ); 483 double t101 = m[1][12] * m[2][12]; 484 double t105 = pow( m[2][12], 2 ); 485 486 *P.ptr<double>(0,0) = t1 + t2 - 2 * m[1][4] * m[1][1] - 2 * m[1][5] * m[1][2] + t7 + t8; 487 *P.ptr<double>(0,1) = -2 * m[2][4] * m[1][1] + 2 * t11 + 2 * t12 - 2 * m[1][4] * m[2][1] - 2 * m[2][5] * m[1][2] + 2 * t15 + 2 * t16 - 2 * m[1][5] * m[2][2]; 488 *P.ptr<double>(0,2) = t19 - 2 * m[2][4] * m[2][1] + t22 + t23 + t24 - 2 * m[2][5] * m[2][2]; 489 *P.ptr<double>(0,3) = t28 + t29 - 2 * m[1][6] * m[1][3]; 490 *P.ptr<double>(0,4) = -2 * m[2][6] * m[1][3] + 2 * t34 - 2 * m[1][6] * m[2][3] + 2 * t36; 491 *P.ptr<double>(0,5) = -2 * m[2][6] * m[2][3] + t40 + t41; 492 493 *P.ptr<double>(1,0) = t8 - 2 * m[1][8] * m[1][2] - 2 * m[1][7] * m[1][1] + t47 + t48 + t2; 494 *P.ptr<double>(1,1) = 2 * t15 - 2 * m[1][8] * m[2][2] - 2 * m[2][8] * m[1][2] + 2 * t52 - 2 * m[1][7] * m[2][1] - 2 * m[2][7] * m[1][1] + 2 * t55 + 2 * t11; 495 *P.ptr<double>(1,2) = -2 * m[2][8] * m[2][2] + t22 + t23 + t59 - 2 * m[2][7] * m[2][1] + t62; 496 *P.ptr<double>(1,3) = t29 + t64 - 2 * m[1][9] * m[1][3]; 497 *P.ptr<double>(1,4) = 2 * t34 + 2 * t68 - 2 * m[2][9] * m[1][3] - 2 * m[1][9] * m[2][3]; 498 *P.ptr<double>(1,5) = -2 * m[2][9] * m[2][3] + t74 + t41; 499 500 *P.ptr<double>(2,0) = -2 * m[1][11] * m[1][2] + t2 + t8 + t78 + t79 - 2 * m[1][10] * m[1][1]; 501 *P.ptr<double>(2,1) = 2 * t15 - 2 * m[1][11] * m[2][2] + 2 * t84 - 2 * m[1][10] * m[2][1] - 2 * m[2][10] * m[1][1] + 2 * t87 - 2 * m[2][11] * m[1][2]+ 2 * t11; 502 *P.ptr<double>(2,2) = t90 + t22 - 2 * m[2][10] * m[2][1] + t23 - 2 * m[2][11] * m[2][2] + t95; 503 *P.ptr<double>(2,3) = -2 * m[1][12] * m[1][3] + t99 + t29; 504 *P.ptr<double>(2,4) = 2 * t34 + 2 * t101 - 2 * m[2][12] * m[1][3] - 2 * m[1][12] * m[2][3]; 505 *P.ptr<double>(2,5) = t41 + t105 - 2 * m[2][12] * m[2][3]; 506 507 *P.ptr<double>(3,0) = t48 + t1 - 2 * m[1][8] * m[1][5] + t7 - 2 * m[1][7] * m[1][4] + t47; 508 *P.ptr<double>(3,1) = 2 * t16 - 2 * m[1][7] * m[2][4] + 2 * t55 + 2 * t52 - 2 * m[1][8] * m[2][5] - 2 * m[2][8] * m[1][5] - 2 * m[2][7] * m[1][4] + 2 * t12; 509 *P.ptr<double>(3,2) = t24 - 2 * m[2][8] * m[2][5] + t19 - 2 * m[2][7] * m[2][4] + t62 + t59; 510 *P.ptr<double>(3,3) = -2 * m[1][9] * m[1][6] + t64 + t28; 511 *P.ptr<double>(3,4) = 2 * t68 + 2 * t36 - 2 * m[2][9] * m[1][6] - 2 * m[1][9] * m[2][6]; 512 *P.ptr<double>(3,5) = t40 + t74 - 2 * m[2][9] * m[2][6]; 513 514 *P.ptr<double>(4,0) = t1 - 2 * m[1][10] * m[1][4] + t7 + t78 + t79 - 2 * m[1][11] * m[1][5]; 515 *P.ptr<double>(4,1) = 2 * t84 - 2 * m[1][11] * m[2][5] - 2 * m[1][10] * m[2][4] + 2 * t16 - 2 * m[2][11] * m[1][5] + 2 * t87 - 2 * m[2][10] * m[1][4] + 2 * t12; 516 *P.ptr<double>(4,2) = t19 + t24 - 2 * m[2][10] * m[2][4] - 2 * m[2][11] * m[2][5] + t95 + t90; 517 *P.ptr<double>(4,3) = t28 - 2 * m[1][12] * m[1][6] + t99; 518 *P.ptr<double>(4,4) = 2 * t101 + 2 * t36 - 2 * m[2][12] * m[1][6] - 2 * m[1][12] * m[2][6]; 519 *P.ptr<double>(4,5) = t105 - 2 * m[2][12] * m[2][6] + t40; 520 521 *P.ptr<double>(5,0) = -2 * m[1][10] * m[1][7] + t47 + t48 + t78 + t79 - 2 * m[1][11] * m[1][8]; 522 *P.ptr<double>(5,1) = 2 * t84 + 2 * t87 - 2 * m[2][11] * m[1][8] - 2 * m[1][10] * m[2][7] - 2 * m[2][10] * m[1][7] + 2 * t55 + 2 * t52 - 2 * m[1][11] * m[2][8]; 523 *P.ptr<double>(5,2) = -2 * m[2][10] * m[2][7] - 2 * m[2][11] * m[2][8] + t62 + t59 + t90 + t95; 524 *P.ptr<double>(5,3) = t64 - 2 * m[1][12] * m[1][9] + t99; 525 *P.ptr<double>(5,4) = 2 * t68 - 2 * m[2][12] * m[1][9] - 2 * m[1][12] * m[2][9] + 2 * t101; 526 *P.ptr<double>(5,5) = t105 - 2 * m[2][12] * m[2][9] + t74; 527 528 return P; 529 } 530 531 void upnp::generate_all_possible_solutions_for_f_unk(const double betas[5], double solutions[18][3]) 532 { 533 int matrix_to_resolve[18][9] = { 534 { 2, 0, 0, 1, 1, 0, 2, 0, 2 }, { 2, 0, 0, 1, 1, 0, 1, 1, 2 }, 535 { 2, 0, 0, 1, 1, 0, 0, 2, 2 }, { 2, 0, 0, 0, 2, 0, 2, 0, 2 }, 536 { 2, 0, 0, 0, 2, 0, 1, 1, 2 }, { 2, 0, 0, 0, 2, 0, 0, 2, 2 }, 537 { 2, 0, 0, 2, 0, 2, 1, 1, 2 }, { 2, 0, 0, 2, 0, 2, 0, 2, 2 }, 538 { 2, 0, 0, 1, 1, 2, 0, 2, 2 }, { 1, 1, 0, 0, 2, 0, 2, 0, 2 }, 539 { 1, 1, 0, 0, 2, 0, 1, 1, 2 }, { 1, 1, 0, 2, 0, 2, 0, 2, 2 }, 540 { 1, 1, 0, 2, 0, 2, 1, 1, 2 }, { 1, 1, 0, 2, 0, 2, 0, 2, 2 }, 541 { 1, 1, 0, 1, 1, 2, 0, 2, 2 }, { 0, 2, 0, 2, 0, 2, 1, 1, 2 }, 542 { 0, 2, 0, 2, 0, 2, 0, 2, 2 }, { 0, 2, 0, 1, 1, 2, 0, 2, 2 } 543 }; 544 545 int combination[18][3] = { 546 { 1, 2, 4 }, { 1, 2, 5 }, { 1, 2, 6 }, { 1, 3, 4 }, 547 { 1, 3, 5 }, { 1, 3, 6 }, { 1, 4, 5 }, { 1, 4, 6 }, 548 { 1, 5, 6 }, { 2, 3, 4 }, { 2, 3, 5 }, { 2, 3, 6 }, 549 { 2, 4, 5 }, { 2, 4, 6 }, { 2, 5, 6 }, { 3, 4, 5 }, 550 { 3, 4, 6 }, { 3, 5, 6 } 551 }; 552 553 for (int i = 0; i < 18; ++i) { 554 double matrix[9], independent_term[3]; 555 Mat M = Mat(3, 3, CV_64F, matrix); 556 Mat I = Mat(3, 1, CV_64F, independent_term); 557 Mat S = Mat(1, 3, CV_64F); 558 559 for (int j = 0; j < 9; ++j) matrix[j] = (double)matrix_to_resolve[i][j]; 560 561 independent_term[0] = log( abs( betas[ combination[i][0]-1 ] ) ); 562 independent_term[1] = log( abs( betas[ combination[i][1]-1 ] ) ); 563 independent_term[2] = log( abs( betas[ combination[i][2]-1 ] ) ); 564 565 exp( Mat(M.inv() * I), S); 566 567 solutions[i][0] = S.at<double>(0); 568 solutions[i][1] = S.at<double>(1) * sign( betas[1] ); 569 solutions[i][2] = abs( S.at<double>(2) ); 570 } 571 } 572 573 void upnp::gauss_newton(const Mat * L_6x12, const Mat * Rho, double betas[4], double * f) 574 { 575 const int iterations_number = 50; 576 577 double a[6*4], b[6], x[4]; 578 Mat * A = new Mat(6, 4, CV_64F, a); 579 Mat * B = new Mat(6, 1, CV_64F, b); 580 Mat * X = new Mat(4, 1, CV_64F, x); 581 582 for(int k = 0; k < iterations_number; k++) 583 { 584 compute_A_and_b_gauss_newton(L_6x12->ptr<double>(0), Rho->ptr<double>(0), betas, A, B, f[0]); 585 qr_solve(A, B, X); 586 for(int i = 0; i < 3; i++) 587 betas[i] += x[i]; 588 f[0] += x[3]; 589 } 590 591 if (f[0] < 0) f[0] = -f[0]; 592 fu = fv = f[0]; 593 594 } 595 596 void upnp::compute_A_and_b_gauss_newton(const double * l_6x12, const double * rho, 597 const double betas[4], Mat * A, Mat * b, double const f) 598 { 599 600 for(int i = 0; i < 6; i++) { 601 const double * rowL = l_6x12 + i * 12; 602 double * rowA = A->ptr<double>(i); 603 604 rowA[0] = 2 * rowL[0] * betas[0] + rowL[1] * betas[1] + rowL[2] * betas[2] + f*f * ( 2 * rowL[6]*betas[0] + rowL[7]*betas[1] + rowL[8]*betas[2] ); 605 rowA[1] = rowL[1] * betas[0] + 2 * rowL[3] * betas[1] + rowL[4] * betas[2] + f*f * ( rowL[7]*betas[0] + 2 * rowL[9]*betas[1] + rowL[10]*betas[2] ); 606 rowA[2] = rowL[2] * betas[0] + rowL[4] * betas[1] + 2 * rowL[5] * betas[2] + f*f * ( rowL[8]*betas[0] + rowL[10]*betas[1] + 2 * rowL[11]*betas[2] ); 607 rowA[3] = 2*f * ( rowL[6]*betas[0]*betas[0] + rowL[7]*betas[0]*betas[1] + rowL[8]*betas[0]*betas[2] + rowL[9]*betas[1]*betas[1] + rowL[10]*betas[1]*betas[2] + rowL[11]*betas[2]*betas[2] ) ; 608 609 *b->ptr<double>(i) = rho[i] - 610 ( 611 rowL[0] * betas[0] * betas[0] + 612 rowL[1] * betas[0] * betas[1] + 613 rowL[2] * betas[0] * betas[2] + 614 rowL[3] * betas[1] * betas[1] + 615 rowL[4] * betas[1] * betas[2] + 616 rowL[5] * betas[2] * betas[2] + 617 f*f * rowL[6] * betas[0] * betas[0] + 618 f*f * rowL[7] * betas[0] * betas[1] + 619 f*f * rowL[8] * betas[0] * betas[2] + 620 f*f * rowL[9] * betas[1] * betas[1] + 621 f*f * rowL[10] * betas[1] * betas[2] + 622 f*f * rowL[11] * betas[2] * betas[2] 623 ); 624 } 625 } 626 627 void upnp::compute_L_6x12(const double * ut, double * l_6x12) 628 { 629 const double * v[3]; 630 631 v[0] = ut + 12 * 9; 632 v[1] = ut + 12 * 10; 633 v[2] = ut + 12 * 11; 634 635 double dv[3][6][3]; 636 637 for(int i = 0; i < 3; i++) { 638 int a = 0, b = 1; 639 for(int j = 0; j < 6; j++) { 640 dv[i][j][0] = v[i][3 * a ] - v[i][3 * b]; 641 dv[i][j][1] = v[i][3 * a + 1] - v[i][3 * b + 1]; 642 dv[i][j][2] = v[i][3 * a + 2] - v[i][3 * b + 2]; 643 644 b++; 645 if (b > 3) { 646 a++; 647 b = a + 1; 648 } 649 } 650 } 651 652 for(int i = 0; i < 6; i++) { 653 double * row = l_6x12 + 12 * i; 654 655 row[0] = dotXY(dv[0][i], dv[0][i]); 656 row[1] = 2.0f * dotXY(dv[0][i], dv[1][i]); 657 row[2] = dotXY(dv[1][i], dv[1][i]); 658 row[3] = 2.0f * dotXY(dv[0][i], dv[2][i]); 659 row[4] = 2.0f * dotXY(dv[1][i], dv[2][i]); 660 row[5] = dotXY(dv[2][i], dv[2][i]); 661 662 row[6] = dotZ(dv[0][i], dv[0][i]); 663 row[7] = 2.0f * dotZ(dv[0][i], dv[1][i]); 664 row[8] = 2.0f * dotZ(dv[0][i], dv[2][i]); 665 row[9] = dotZ(dv[1][i], dv[1][i]); 666 row[10] = 2.0f * dotZ(dv[1][i], dv[2][i]); 667 row[11] = dotZ(dv[2][i], dv[2][i]); 668 } 669 } 670 671 void upnp::compute_rho(double * rho) 672 { 673 rho[0] = dist2(cws[0], cws[1]); 674 rho[1] = dist2(cws[0], cws[2]); 675 rho[2] = dist2(cws[0], cws[3]); 676 rho[3] = dist2(cws[1], cws[2]); 677 rho[4] = dist2(cws[1], cws[3]); 678 rho[5] = dist2(cws[2], cws[3]); 679 } 680 681 double upnp::dist2(const double * p1, const double * p2) 682 { 683 return 684 (p1[0] - p2[0]) * (p1[0] - p2[0]) + 685 (p1[1] - p2[1]) * (p1[1] - p2[1]) + 686 (p1[2] - p2[2]) * (p1[2] - p2[2]); 687 } 688 689 double upnp::dot(const double * v1, const double * v2) 690 { 691 return v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2]; 692 } 693 694 double upnp::dotXY(const double * v1, const double * v2) 695 { 696 return v1[0] * v2[0] + v1[1] * v2[1]; 697 } 698 699 double upnp::dotZ(const double * v1, const double * v2) 700 { 701 return v1[2] * v2[2]; 702 } 703 704 double upnp::sign(const double v) 705 { 706 return ( v < 0.0 ) ? -1.0 : ( v > 0.0 ) ? 1.0 : 0.0; 707 } 708 709 void upnp::qr_solve(Mat * A, Mat * b, Mat * X) 710 { 711 const int nr = A->rows; 712 const int nc = A->cols; 713 714 if (max_nr != 0 && max_nr < nr) 715 { 716 delete [] A1; 717 delete [] A2; 718 } 719 if (max_nr < nr) 720 { 721 max_nr = nr; 722 A1 = new double[nr]; 723 A2 = new double[nr]; 724 } 725 726 double * pA = A->ptr<double>(0), * ppAkk = pA; 727 for(int k = 0; k < nc; k++) 728 { 729 double * ppAik1 = ppAkk, eta = fabs(*ppAik1); 730 for(int i = k + 1; i < nr; i++) 731 { 732 double elt = fabs(*ppAik1); 733 if (eta < elt) eta = elt; 734 ppAik1 += nc; 735 } 736 if (eta == 0) 737 { 738 A1[k] = A2[k] = 0.0; 739 //cerr << "God damnit, A is singular, this shouldn't happen." << endl; 740 return; 741 } 742 else 743 { 744 double * ppAik2 = ppAkk, sum2 = 0.0, inv_eta = 1. / eta; 745 for(int i = k; i < nr; i++) 746 { 747 *ppAik2 *= inv_eta; 748 sum2 += *ppAik2 * *ppAik2; 749 ppAik2 += nc; 750 } 751 double sigma = sqrt(sum2); 752 if (*ppAkk < 0) 753 sigma = -sigma; 754 *ppAkk += sigma; 755 A1[k] = sigma * *ppAkk; 756 A2[k] = -eta * sigma; 757 for(int j = k + 1; j < nc; j++) 758 { 759 double * ppAik = ppAkk, sum = 0; 760 for(int i = k; i < nr; i++) 761 { 762 sum += *ppAik * ppAik[j - k]; 763 ppAik += nc; 764 } 765 double tau = sum / A1[k]; 766 ppAik = ppAkk; 767 for(int i = k; i < nr; i++) 768 { 769 ppAik[j - k] -= tau * *ppAik; 770 ppAik += nc; 771 } 772 } 773 } 774 ppAkk += nc + 1; 775 } 776 777 // b <- Qt b 778 double * ppAjj = pA, * pb = b->ptr<double>(0); 779 for(int j = 0; j < nc; j++) 780 { 781 double * ppAij = ppAjj, tau = 0; 782 for(int i = j; i < nr; i++) 783 { 784 tau += *ppAij * pb[i]; 785 ppAij += nc; 786 } 787 tau /= A1[j]; 788 ppAij = ppAjj; 789 for(int i = j; i < nr; i++) 790 { 791 pb[i] -= tau * *ppAij; 792 ppAij += nc; 793 } 794 ppAjj += nc + 1; 795 } 796 797 // X = R-1 b 798 double * pX = X->ptr<double>(0); 799 pX[nc - 1] = pb[nc - 1] / A2[nc - 1]; 800 for(int i = nc - 2; i >= 0; i--) 801 { 802 double * ppAij = pA + i * nc + (i + 1), sum = 0; 803 804 for(int j = i + 1; j < nc; j++) 805 { 806 sum += *ppAij * pX[j]; 807 ppAij++; 808 } 809 pX[i] = (pb[i] - sum) / A2[i]; 810 } 811 } 812
