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 // Intel License Agreement 11 // For Open Source Computer Vision Library 12 // 13 // Copyright (C) 2000, Intel Corporation, all rights reserved. 14 // Third party copyrights are property of their respective owners. 15 // 16 // Redistribution and use in source and binary forms, with or without modification, 17 // are permitted provided that the following conditions are met: 18 // 19 // * Redistribution's of source code must retain the above copyright notice, 20 // this list of conditions and the following disclaimer. 21 // 22 // * Redistribution's in binary form must reproduce the above copyright notice, 23 // this list of conditions and the following disclaimer in the documentation 24 // and/or other materials provided with the distribution. 25 // 26 // * The name of Intel Corporation may not be used to endorse or promote products 27 // derived from this software without specific prior written permission. 28 // 29 // This software is provided by the copyright holders and contributors "as is" and 30 // any express or implied warranties, including, but not limited to, the implied 31 // warranties of merchantability and fitness for a particular purpose are disclaimed. 32 // In no event shall the Intel Corporation or contributors be liable for any direct, 33 // indirect, incidental, special, exemplary, or consequential damages 34 // (including, but not limited to, procurement of substitute goods or services; 35 // loss of use, data, or profits; or business interruption) however caused 36 // and on any theory of liability, whether in contract, strict liability, 37 // or tort (including negligence or otherwise) arising in any way out of 38 // the use of this software, even if advised of the possibility of such damage. 39 // 40 //M*/ 41 #include "_cv.h" 42 43 44 CV_IMPL CvRect 45 cvMaxRect( const CvRect* rect1, const CvRect* rect2 ) 46 { 47 if( rect1 && rect2 ) 48 { 49 CvRect max_rect; 50 int a, b; 51 52 max_rect.x = a = rect1->x; 53 b = rect2->x; 54 if( max_rect.x > b ) 55 max_rect.x = b; 56 57 max_rect.width = a += rect1->width; 58 b += rect2->width; 59 60 if( max_rect.width < b ) 61 max_rect.width = b; 62 max_rect.width -= max_rect.x; 63 64 max_rect.y = a = rect1->y; 65 b = rect2->y; 66 if( max_rect.y > b ) 67 max_rect.y = b; 68 69 max_rect.height = a += rect1->height; 70 b += rect2->height; 71 72 if( max_rect.height < b ) 73 max_rect.height = b; 74 max_rect.height -= max_rect.y; 75 return max_rect; 76 } 77 else if( rect1 ) 78 return *rect1; 79 else if( rect2 ) 80 return *rect2; 81 else 82 return cvRect(0,0,0,0); 83 } 84 85 86 CV_IMPL void 87 cvBoxPoints( CvBox2D box, CvPoint2D32f pt[4] ) 88 { 89 CV_FUNCNAME( "cvBoxPoints" ); 90 91 __BEGIN__; 92 93 double angle = box.angle*CV_PI/180.; 94 float a = (float)cos(angle)*0.5f; 95 float b = (float)sin(angle)*0.5f; 96 97 if( !pt ) 98 CV_ERROR( CV_StsNullPtr, "NULL vertex array pointer" ); 99 100 pt[0].x = box.center.x - a*box.size.height - b*box.size.width; 101 pt[0].y = box.center.y + b*box.size.height - a*box.size.width; 102 pt[1].x = box.center.x + a*box.size.height - b*box.size.width; 103 pt[1].y = box.center.y - b*box.size.height - a*box.size.width; 104 pt[2].x = 2*box.center.x - pt[0].x; 105 pt[2].y = 2*box.center.y - pt[0].y; 106 pt[3].x = 2*box.center.x - pt[1].x; 107 pt[3].y = 2*box.center.y - pt[1].y; 108 109 __END__; 110 } 111 112 113 int 114 icvIntersectLines( double x1, double dx1, double y1, double dy1, 115 double x2, double dx2, double y2, double dy2, double *t2 ) 116 { 117 double d = dx1 * dy2 - dx2 * dy1; 118 int result = -1; 119 120 if( d != 0 ) 121 { 122 *t2 = ((x2 - x1) * dy1 - (y2 - y1) * dx1) / d; 123 result = 0; 124 } 125 return result; 126 } 127 128 129 void 130 icvCreateCenterNormalLine( CvSubdiv2DEdge edge, double *_a, double *_b, double *_c ) 131 { 132 CvPoint2D32f org = cvSubdiv2DEdgeOrg( edge )->pt; 133 CvPoint2D32f dst = cvSubdiv2DEdgeDst( edge )->pt; 134 135 double a = dst.x - org.x; 136 double b = dst.y - org.y; 137 double c = -(a * (dst.x + org.x) + b * (dst.y + org.y)); 138 139 *_a = a + a; 140 *_b = b + b; 141 *_c = c; 142 } 143 144 145 void 146 icvIntersectLines3( double *a0, double *b0, double *c0, 147 double *a1, double *b1, double *c1, CvPoint2D32f * point ) 148 { 149 double det = a0[0] * b1[0] - a1[0] * b0[0]; 150 151 if( det != 0 ) 152 { 153 det = 1. / det; 154 point->x = (float) ((b0[0] * c1[0] - b1[0] * c0[0]) * det); 155 point->y = (float) ((a1[0] * c0[0] - a0[0] * c1[0]) * det); 156 } 157 else 158 { 159 point->x = point->y = FLT_MAX; 160 } 161 } 162 163 164 CV_IMPL double 165 cvPointPolygonTest( const CvArr* _contour, CvPoint2D32f pt, int measure_dist ) 166 { 167 double result = 0; 168 CV_FUNCNAME( "cvCheckPointPolygon" ); 169 170 __BEGIN__; 171 172 CvSeqBlock block; 173 CvContour header; 174 CvSeq* contour = (CvSeq*)_contour; 175 CvSeqReader reader; 176 int i, total, counter = 0; 177 int is_float; 178 double min_dist_num = FLT_MAX, min_dist_denom = 1; 179 CvPoint ip = {0,0}; 180 181 if( !CV_IS_SEQ(contour) ) 182 { 183 CV_CALL( contour = cvPointSeqFromMat( CV_SEQ_KIND_CURVE + CV_SEQ_FLAG_CLOSED, 184 _contour, &header, &block )); 185 } 186 else if( CV_IS_SEQ_POLYGON(contour) ) 187 { 188 if( contour->header_size == sizeof(CvContour) && !measure_dist ) 189 { 190 CvRect r = ((CvContour*)contour)->rect; 191 if( pt.x < r.x || pt.y < r.y || 192 pt.x >= r.x + r.width || pt.y >= r.y + r.height ) 193 return -100; 194 } 195 } 196 else if( CV_IS_SEQ_CHAIN(contour) ) 197 { 198 CV_ERROR( CV_StsBadArg, 199 "Chains are not supported. Convert them to polygonal representation using cvApproxChains()" ); 200 } 201 else 202 CV_ERROR( CV_StsBadArg, "Input contour is neither a valid sequence nor a matrix" ); 203 204 total = contour->total; 205 is_float = CV_SEQ_ELTYPE(contour) == CV_32FC2; 206 cvStartReadSeq( contour, &reader, -1 ); 207 208 if( !is_float && !measure_dist && (ip.x = cvRound(pt.x)) == pt.x && (ip.y = cvRound(pt.y)) == pt.y ) 209 { 210 // the fastest "pure integer" branch 211 CvPoint v0, v; 212 CV_READ_SEQ_ELEM( v, reader ); 213 214 for( i = 0; i < total; i++ ) 215 { 216 int dist; 217 v0 = v; 218 CV_READ_SEQ_ELEM( v, reader ); 219 220 if( (v0.y <= ip.y && v.y <= ip.y) || 221 (v0.y > ip.y && v.y > ip.y) || 222 (v0.x < ip.x && v.x < ip.x) ) 223 { 224 if( ip.y == v.y && (ip.x == v.x || (ip.y == v0.y && 225 ((v0.x <= ip.x && ip.x <= v.x) || (v.x <= ip.x && ip.x <= v0.x)))) ) 226 EXIT; 227 continue; 228 } 229 230 dist = (ip.y - v0.y)*(v.x - v0.x) - (ip.x - v0.x)*(v.y - v0.y); 231 if( dist == 0 ) 232 EXIT; 233 if( v.y < v0.y ) 234 dist = -dist; 235 counter += dist > 0; 236 } 237 238 result = counter % 2 == 0 ? -100 : 100; 239 } 240 else 241 { 242 CvPoint2D32f v0, v; 243 CvPoint iv; 244 245 if( is_float ) 246 { 247 CV_READ_SEQ_ELEM( v, reader ); 248 } 249 else 250 { 251 CV_READ_SEQ_ELEM( iv, reader ); 252 v = cvPointTo32f( iv ); 253 } 254 255 if( !measure_dist ) 256 { 257 for( i = 0; i < total; i++ ) 258 { 259 double dist; 260 v0 = v; 261 if( is_float ) 262 { 263 CV_READ_SEQ_ELEM( v, reader ); 264 } 265 else 266 { 267 CV_READ_SEQ_ELEM( iv, reader ); 268 v = cvPointTo32f( iv ); 269 } 270 271 if( (v0.y <= pt.y && v.y <= pt.y) || 272 (v0.y > pt.y && v.y > pt.y) || 273 (v0.x < pt.x && v.x < pt.x) ) 274 { 275 if( pt.y == v.y && (pt.x == v.x || (pt.y == v0.y && 276 ((v0.x <= pt.x && pt.x <= v.x) || (v.x <= pt.x && pt.x <= v0.x)))) ) 277 EXIT; 278 continue; 279 } 280 281 dist = (double)(pt.y - v0.y)*(v.x - v0.x) - (double)(pt.x - v0.x)*(v.y - v0.y); 282 if( dist == 0 ) 283 EXIT; 284 if( v.y < v0.y ) 285 dist = -dist; 286 counter += dist > 0; 287 } 288 289 result = counter % 2 == 0 ? -100 : 100; 290 } 291 else 292 { 293 for( i = 0; i < total; i++ ) 294 { 295 double dx, dy, dx1, dy1, dx2, dy2, dist_num, dist_denom = 1; 296 297 v0 = v; 298 if( is_float ) 299 { 300 CV_READ_SEQ_ELEM( v, reader ); 301 } 302 else 303 { 304 CV_READ_SEQ_ELEM( iv, reader ); 305 v = cvPointTo32f( iv ); 306 } 307 308 dx = v.x - v0.x; dy = v.y - v0.y; 309 dx1 = pt.x - v0.x; dy1 = pt.y - v0.y; 310 dx2 = pt.x - v.x; dy2 = pt.y - v.y; 311 312 if( dx1*dx + dy1*dy <= 0 ) 313 dist_num = dx1*dx1 + dy1*dy1; 314 else if( dx2*dx + dy2*dy >= 0 ) 315 dist_num = dx2*dx2 + dy2*dy2; 316 else 317 { 318 dist_num = (dy1*dx - dx1*dy); 319 dist_num *= dist_num; 320 dist_denom = dx*dx + dy*dy; 321 } 322 323 if( dist_num*min_dist_denom < min_dist_num*dist_denom ) 324 { 325 min_dist_num = dist_num; 326 min_dist_denom = dist_denom; 327 if( min_dist_num == 0 ) 328 break; 329 } 330 331 if( (v0.y <= pt.y && v.y <= pt.y) || 332 (v0.y > pt.y && v.y > pt.y) || 333 (v0.x < pt.x && v.x < pt.x) ) 334 continue; 335 336 dist_num = dy1*dx - dx1*dy; 337 if( dy < 0 ) 338 dist_num = -dist_num; 339 counter += dist_num > 0; 340 } 341 342 result = sqrt(min_dist_num/min_dist_denom); 343 if( counter % 2 == 0 ) 344 result = -result; 345 } 346 } 347 348 __END__; 349 350 return result; 351 } 352 353 354 CV_IMPL void 355 cvRQDecomp3x3( const CvMat *matrixM, CvMat *matrixR, CvMat *matrixQ, 356 CvMat *matrixQx, CvMat *matrixQy, CvMat *matrixQz, 357 CvPoint3D64f *eulerAngles) 358 { 359 CV_FUNCNAME("cvRQDecomp3x3"); 360 __BEGIN__; 361 362 double _M[3][3], _R[3][3], _Q[3][3]; 363 CvMat M = cvMat(3, 3, CV_64F, _M); 364 CvMat R = cvMat(3, 3, CV_64F, _R); 365 CvMat Q = cvMat(3, 3, CV_64F, _Q); 366 double z, c, s; 367 368 /* Validate parameters. */ 369 CV_ASSERT( CV_IS_MAT(matrixM) && CV_IS_MAT(matrixR) && CV_IS_MAT(matrixQ) && 370 matrixM->cols == 3 && matrixM->rows == 3 && 371 CV_ARE_SIZES_EQ(matrixM, matrixR) && CV_ARE_SIZES_EQ(matrixM, matrixQ)); 372 373 cvConvert(matrixM, &M); 374 375 { 376 /* Find Givens rotation Q_x for x axis (left multiplication). */ 377 /* 378 ( 1 0 0 ) 379 Qx = ( 0 c s ), c = m33/sqrt(m32^2 + m33^2), s = m32/sqrt(m32^2 + m33^2) 380 ( 0 -s c ) 381 */ 382 s = _M[2][1]; 383 c = _M[2][2]; 384 z = 1./sqrt(c * c + s * s + DBL_EPSILON); 385 c *= z; 386 s *= z; 387 388 double _Qx[3][3] = { {1, 0, 0}, {0, c, s}, {0, -s, c} }; 389 CvMat Qx = cvMat(3, 3, CV_64F, _Qx); 390 391 cvMatMul(&M, &Qx, &R); 392 assert(fabs(_R[2][1]) < FLT_EPSILON); 393 _R[2][1] = 0; 394 395 /* Find Givens rotation for y axis. */ 396 /* 397 ( c 0 s ) 398 Qy = ( 0 1 0 ), c = m33/sqrt(m31^2 + m33^2), s = m31/sqrt(m31^2 + m33^2) 399 (-s 0 c ) 400 */ 401 s = _R[2][0]; 402 c = _R[2][2]; 403 z = 1./sqrt(c * c + s * s + DBL_EPSILON); 404 c *= z; 405 s *= z; 406 407 double _Qy[3][3] = { {c, 0, s}, {0, 1, 0}, {-s, 0, c} }; 408 CvMat Qy = cvMat(3, 3, CV_64F, _Qy); 409 cvMatMul(&R, &Qy, &M); 410 411 assert(fabs(_M[2][0]) < FLT_EPSILON); 412 _M[2][0] = 0; 413 414 /* Find Givens rotation for z axis. */ 415 /* 416 ( c s 0 ) 417 Qz = (-s c 0 ), c = m22/sqrt(m21^2 + m22^2), s = m21/sqrt(m21^2 + m22^2) 418 ( 0 0 1 ) 419 */ 420 421 s = _M[1][0]; 422 c = _M[1][1]; 423 z = 1./sqrt(c * c + s * s + DBL_EPSILON); 424 c *= z; 425 s *= z; 426 427 double _Qz[3][3] = { {c, s, 0}, {-s, c, 0}, {0, 0, 1} }; 428 CvMat Qz = cvMat(3, 3, CV_64F, _Qz); 429 430 cvMatMul(&M, &Qz, &R); 431 assert(fabs(_R[1][0]) < FLT_EPSILON); 432 _R[1][0] = 0; 433 434 // Solve the decomposition ambiguity. 435 // Diagonal entries of R, except the last one, shall be positive. 436 // Further rotate R by 180 degree if necessary 437 if( _R[0][0] < 0 ) 438 { 439 if( _R[1][1] < 0 ) 440 { 441 // rotate around z for 180 degree, i.e. a rotation matrix of 442 // [-1, 0, 0], 443 // [ 0, -1, 0], 444 // [ 0, 0, 1] 445 _R[0][0] *= -1; 446 _R[0][1] *= -1; 447 _R[1][1] *= -1; 448 449 _Qz[0][0] *= -1; 450 _Qz[0][1] *= -1; 451 _Qz[1][0] *= -1; 452 _Qz[1][1] *= -1; 453 } 454 else 455 { 456 // rotate around y for 180 degree, i.e. a rotation matrix of 457 // [-1, 0, 0], 458 // [ 0, 1, 0], 459 // [ 0, 0, -1] 460 _R[0][0] *= -1; 461 _R[0][2] *= -1; 462 _R[1][2] *= -1; 463 _R[2][2] *= -1; 464 465 cvTranspose( &Qz, &Qz ); 466 467 _Qy[0][0] *= -1; 468 _Qy[0][2] *= -1; 469 _Qy[2][0] *= -1; 470 _Qy[2][2] *= -1; 471 } 472 } 473 else if( _R[1][1] < 0 ) 474 { 475 // ??? for some reason, we never get here ??? 476 477 // rotate around x for 180 degree, i.e. a rotation matrix of 478 // [ 1, 0, 0], 479 // [ 0, -1, 0], 480 // [ 0, 0, -1] 481 _R[0][1] *= -1; 482 _R[0][2] *= -1; 483 _R[1][1] *= -1; 484 _R[1][2] *= -1; 485 _R[2][2] *= -1; 486 487 cvTranspose( &Qz, &Qz ); 488 cvTranspose( &Qy, &Qy ); 489 490 _Qx[1][1] *= -1; 491 _Qx[1][2] *= -1; 492 _Qx[2][1] *= -1; 493 _Qx[2][2] *= -1; 494 } 495 496 // calculate the euler angle 497 if( eulerAngles ) 498 { 499 eulerAngles->x = acos(_Qx[1][1]) * (_Qx[1][2] >= 0 ? 1 : -1) * (180.0 / CV_PI); 500 eulerAngles->y = acos(_Qy[0][0]) * (_Qy[0][2] >= 0 ? 1 : -1) * (180.0 / CV_PI); 501 eulerAngles->z = acos(_Qz[0][0]) * (_Qz[0][1] >= 0 ? 1 : -1) * (180.0 / CV_PI); 502 } 503 504 /* Calulate orthogonal matrix. */ 505 /* 506 Q = QzT * QyT * QxT 507 */ 508 cvGEMM( &Qz, &Qy, 1, 0, 0, &M, CV_GEMM_A_T + CV_GEMM_B_T ); 509 cvGEMM( &M, &Qx, 1, 0, 0, &Q, CV_GEMM_B_T ); 510 511 /* Save R and Q matrices. */ 512 cvConvert( &R, matrixR ); 513 cvConvert( &Q, matrixQ ); 514 515 if( matrixQx ) 516 cvConvert(&Qx, matrixQx); 517 if( matrixQy ) 518 cvConvert(&Qy, matrixQy); 519 if( matrixQz ) 520 cvConvert(&Qz, matrixQz); 521 } 522 523 __END__; 524 } 525 526 527 CV_IMPL void 528 cvDecomposeProjectionMatrix( const CvMat *projMatr, CvMat *calibMatr, 529 CvMat *rotMatr, CvMat *posVect, 530 CvMat *rotMatrX, CvMat *rotMatrY, 531 CvMat *rotMatrZ, CvPoint3D64f *eulerAngles) 532 { 533 CvMat *tmpProjMatr = 0; 534 CvMat *tmpMatrixD = 0; 535 CvMat *tmpMatrixV = 0; 536 CvMat *tmpMatrixM = 0; 537 538 CV_FUNCNAME("cvDecomposeProjectionMatrix"); 539 __BEGIN__; 540 541 /* Validate parameters. */ 542 if(projMatr == 0 || calibMatr == 0 || rotMatr == 0 || posVect == 0) 543 CV_ERROR(CV_StsNullPtr, "Some of parameters is a NULL pointer!"); 544 545 if(!CV_IS_MAT(projMatr) || !CV_IS_MAT(calibMatr) || !CV_IS_MAT(rotMatr) || !CV_IS_MAT(posVect)) 546 CV_ERROR(CV_StsUnsupportedFormat, "Input parameters must be a matrices!"); 547 548 if(projMatr->cols != 4 || projMatr->rows != 3) 549 CV_ERROR(CV_StsUnmatchedSizes, "Size of projection matrix must be 3x4!"); 550 551 if(calibMatr->cols != 3 || calibMatr->rows != 3 || rotMatr->cols != 3 || rotMatr->rows != 3) 552 CV_ERROR(CV_StsUnmatchedSizes, "Size of calibration and rotation matrices must be 3x3!"); 553 554 if(posVect->cols != 1 || posVect->rows != 4) 555 CV_ERROR(CV_StsUnmatchedSizes, "Size of position vector must be 4x1!"); 556 557 CV_CALL(tmpProjMatr = cvCreateMat(4, 4, CV_64F)); 558 CV_CALL(tmpMatrixD = cvCreateMat(4, 4, CV_64F)); 559 CV_CALL(tmpMatrixV = cvCreateMat(4, 4, CV_64F)); 560 CV_CALL(tmpMatrixM = cvCreateMat(3, 3, CV_64F)); 561 562 /* Compute position vector. */ 563 564 cvSetZero(tmpProjMatr); // Add zero row to make matrix square. 565 int i, k; 566 for(i = 0; i < 3; i++) 567 for(k = 0; k < 4; k++) 568 cvmSet(tmpProjMatr, i, k, cvmGet(projMatr, i, k)); 569 570 CV_CALL(cvSVD(tmpProjMatr, tmpMatrixD, NULL, tmpMatrixV, CV_SVD_MODIFY_A + CV_SVD_V_T)); 571 572 /* Save position vector. */ 573 574 for(i = 0; i < 4; i++) 575 cvmSet(posVect, i, 0, cvmGet(tmpMatrixV, 3, i)); // Solution is last row of V. 576 577 /* Compute calibration and rotation matrices via RQ decomposition. */ 578 579 cvGetCols(projMatr, tmpMatrixM, 0, 3); // M is first square matrix of P. 580 581 assert(cvDet(tmpMatrixM) != 0.0); // So far only finite cameras could be decomposed, so M has to be nonsingular [det(M) != 0]. 582 583 CV_CALL(cvRQDecomp3x3(tmpMatrixM, calibMatr, rotMatr, rotMatrX, rotMatrY, rotMatrZ, eulerAngles)); 584 585 __END__; 586 587 cvReleaseMat(&tmpProjMatr); 588 cvReleaseMat(&tmpMatrixD); 589 cvReleaseMat(&tmpMatrixV); 590 cvReleaseMat(&tmpMatrixM); 591 } 592 593 /* End of file. */ 594