16acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*M/////////////////////////////////////////////////////////////////////////////////////// 26acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 36acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. 46acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 56acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// By downloading, copying, installing or using the software you agree to this license. 66acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// If you do not agree to this license, do not download, install, 76acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// copy or use the software. 86acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 96acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Intel License Agreement 116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// For Open Source Computer Vision Library 126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Copyright (C) 2000, Intel Corporation, all rights reserved. 146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Third party copyrights are property of their respective owners. 156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Redistribution and use in source and binary forms, with or without modification, 176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// are permitted provided that the following conditions are met: 186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// * Redistribution's of source code must retain the above copyright notice, 206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// this list of conditions and the following disclaimer. 216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// * Redistribution's in binary form must reproduce the above copyright notice, 236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// this list of conditions and the following disclaimer in the documentation 246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// and/or other materials provided with the distribution. 256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// * The name of Intel Corporation may not be used to endorse or promote products 276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// derived from this software without specific prior written permission. 286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// This software is provided by the copyright holders and contributors "as is" and 306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// any express or implied warranties, including, but not limited to, the implied 316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// warranties of merchantability and fitness for a particular purpose are disclaimed. 326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// In no event shall the Intel Corporation or contributors be liable for any direct, 336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// indirect, incidental, special, exemplary, or consequential damages 346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// (including, but not limited to, procurement of substitute goods or services; 356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// loss of use, data, or profits; or business interruption) however caused 366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// and on any theory of liability, whether in contract, strict liability, 376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// or tort (including negligence or otherwise) arising in any way out of 386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// the use of this software, even if advised of the possibility of such damage. 396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn//M*/ 416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#include "_cv.h" 426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_IMPL CvRect 456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RenncvMaxRect( const CvRect* rect1, const CvRect* rect2 ) 466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( rect1 && rect2 ) 486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvRect max_rect; 506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int a, b; 516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn max_rect.x = a = rect1->x; 536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn b = rect2->x; 546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( max_rect.x > b ) 556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn max_rect.x = b; 566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn max_rect.width = a += rect1->width; 586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn b += rect2->width; 596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( max_rect.width < b ) 616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn max_rect.width = b; 626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn max_rect.width -= max_rect.x; 636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn max_rect.y = a = rect1->y; 656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn b = rect2->y; 666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( max_rect.y > b ) 676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn max_rect.y = b; 686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn max_rect.height = a += rect1->height; 706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn b += rect2->height; 716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( max_rect.height < b ) 736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn max_rect.height = b; 746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn max_rect.height -= max_rect.y; 756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return max_rect; 766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else if( rect1 ) 786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return *rect1; 796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else if( rect2 ) 806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return *rect2; 816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return cvRect(0,0,0,0); 836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_IMPL void 876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RenncvBoxPoints( CvBox2D box, CvPoint2D32f pt[4] ) 886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "cvBoxPoints" ); 906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double angle = box.angle*CV_PI/180.; 946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float a = (float)cos(angle)*0.5f; 956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float b = (float)sin(angle)*0.5f; 966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !pt ) 986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNullPtr, "NULL vertex array pointer" ); 996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pt[0].x = box.center.x - a*box.size.height - b*box.size.width; 1016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pt[0].y = box.center.y + b*box.size.height - a*box.size.width; 1026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pt[1].x = box.center.x + a*box.size.height - b*box.size.width; 1036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pt[1].y = box.center.y - b*box.size.height - a*box.size.width; 1046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pt[2].x = 2*box.center.x - pt[0].x; 1056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pt[2].y = 2*box.center.y - pt[0].y; 1066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pt[3].x = 2*box.center.x - pt[1].x; 1076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pt[3].y = 2*box.center.y - pt[1].y; 1086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 1106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 1116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennint 1146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennicvIntersectLines( double x1, double dx1, double y1, double dy1, 1156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double x2, double dx2, double y2, double dy2, double *t2 ) 1166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 1176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double d = dx1 * dy2 - dx2 * dy1; 1186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int result = -1; 1196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( d != 0 ) 1216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *t2 = ((x2 - x1) * dy1 - (y2 - y1) * dx1) / d; 1236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn result = 0; 1246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return result; 1266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 1276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid 1306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennicvCreateCenterNormalLine( CvSubdiv2DEdge edge, double *_a, double *_b, double *_c ) 1316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 1326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D32f org = cvSubdiv2DEdgeOrg( edge )->pt; 1336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D32f dst = cvSubdiv2DEdgeDst( edge )->pt; 1346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double a = dst.x - org.x; 1366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double b = dst.y - org.y; 1376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double c = -(a * (dst.x + org.x) + b * (dst.y + org.y)); 1386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *_a = a + a; 1406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *_b = b + b; 1416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *_c = c; 1426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 1436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid 1466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennicvIntersectLines3( double *a0, double *b0, double *c0, 1476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double *a1, double *b1, double *c1, CvPoint2D32f * point ) 1486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 1496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double det = a0[0] * b1[0] - a1[0] * b0[0]; 1506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( det != 0 ) 1526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn det = 1. / det; 1546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn point->x = (float) ((b0[0] * c1[0] - b1[0] * c0[0]) * det); 1556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn point->y = (float) ((a1[0] * c0[0] - a0[0] * c1[0]) * det); 1566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 1586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn point->x = point->y = FLT_MAX; 1606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 1626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_IMPL double 1656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RenncvPointPolygonTest( const CvArr* _contour, CvPoint2D32f pt, int measure_dist ) 1666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 1676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double result = 0; 1686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "cvCheckPointPolygon" ); 1696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 1716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvSeqBlock block; 1736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvContour header; 1746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvSeq* contour = (CvSeq*)_contour; 1756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvSeqReader reader; 1766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i, total, counter = 0; 1776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int is_float; 1786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double min_dist_num = FLT_MAX, min_dist_denom = 1; 1796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint ip = {0,0}; 1806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_SEQ(contour) ) 1826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( contour = cvPointSeqFromMat( CV_SEQ_KIND_CURVE + CV_SEQ_FLAG_CLOSED, 1846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _contour, &header, &block )); 1856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else if( CV_IS_SEQ_POLYGON(contour) ) 1876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( contour->header_size == sizeof(CvContour) && !measure_dist ) 1896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvRect r = ((CvContour*)contour)->rect; 1916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( pt.x < r.x || pt.y < r.y || 1926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pt.x >= r.x + r.width || pt.y >= r.y + r.height ) 1936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return -100; 1946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else if( CV_IS_SEQ_CHAIN(contour) ) 1976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadArg, 1996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn "Chains are not supported. Convert them to polygonal representation using cvApproxChains()" ); 2006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 2026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadArg, "Input contour is neither a valid sequence nor a matrix" ); 2036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn total = contour->total; 2056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn is_float = CV_SEQ_ELTYPE(contour) == CV_32FC2; 2066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvStartReadSeq( contour, &reader, -1 ); 2076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !is_float && !measure_dist && (ip.x = cvRound(pt.x)) == pt.x && (ip.y = cvRound(pt.y)) == pt.y ) 2096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // the fastest "pure integer" branch 2116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint v0, v; 2126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_READ_SEQ_ELEM( v, reader ); 2136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < total; i++ ) 2156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int dist; 2176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn v0 = v; 2186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_READ_SEQ_ELEM( v, reader ); 2196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( (v0.y <= ip.y && v.y <= ip.y) || 2216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (v0.y > ip.y && v.y > ip.y) || 2226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (v0.x < ip.x && v.x < ip.x) ) 2236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( ip.y == v.y && (ip.x == v.x || (ip.y == v0.y && 2256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ((v0.x <= ip.x && ip.x <= v.x) || (v.x <= ip.x && ip.x <= v0.x)))) ) 2266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn EXIT; 2276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn continue; 2286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dist = (ip.y - v0.y)*(v.x - v0.x) - (ip.x - v0.x)*(v.y - v0.y); 2316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( dist == 0 ) 2326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn EXIT; 2336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( v.y < v0.y ) 2346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dist = -dist; 2356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn counter += dist > 0; 2366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn result = counter % 2 == 0 ? -100 : 100; 2396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 2416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D32f v0, v; 2436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint iv; 2446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( is_float ) 2466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_READ_SEQ_ELEM( v, reader ); 2486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 2506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_READ_SEQ_ELEM( iv, reader ); 2526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn v = cvPointTo32f( iv ); 2536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !measure_dist ) 2566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < total; i++ ) 2586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double dist; 2606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn v0 = v; 2616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( is_float ) 2626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_READ_SEQ_ELEM( v, reader ); 2646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 2666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_READ_SEQ_ELEM( iv, reader ); 2686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn v = cvPointTo32f( iv ); 2696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( (v0.y <= pt.y && v.y <= pt.y) || 2726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (v0.y > pt.y && v.y > pt.y) || 2736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (v0.x < pt.x && v.x < pt.x) ) 2746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( pt.y == v.y && (pt.x == v.x || (pt.y == v0.y && 2766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ((v0.x <= pt.x && pt.x <= v.x) || (v.x <= pt.x && pt.x <= v0.x)))) ) 2776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn EXIT; 2786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn continue; 2796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dist = (double)(pt.y - v0.y)*(v.x - v0.x) - (double)(pt.x - v0.x)*(v.y - v0.y); 2826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( dist == 0 ) 2836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn EXIT; 2846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( v.y < v0.y ) 2856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dist = -dist; 2866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn counter += dist > 0; 2876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn result = counter % 2 == 0 ? -100 : 100; 2906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 2926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < total; i++ ) 2946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double dx, dy, dx1, dy1, dx2, dy2, dist_num, dist_denom = 1; 2966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn v0 = v; 2986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( is_float ) 2996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_READ_SEQ_ELEM( v, reader ); 3016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 3036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_READ_SEQ_ELEM( iv, reader ); 3056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn v = cvPointTo32f( iv ); 3066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dx = v.x - v0.x; dy = v.y - v0.y; 3096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dx1 = pt.x - v0.x; dy1 = pt.y - v0.y; 3106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dx2 = pt.x - v.x; dy2 = pt.y - v.y; 3116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( dx1*dx + dy1*dy <= 0 ) 3136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dist_num = dx1*dx1 + dy1*dy1; 3146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else if( dx2*dx + dy2*dy >= 0 ) 3156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dist_num = dx2*dx2 + dy2*dy2; 3166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 3176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dist_num = (dy1*dx - dx1*dy); 3196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dist_num *= dist_num; 3206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dist_denom = dx*dx + dy*dy; 3216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( dist_num*min_dist_denom < min_dist_num*dist_denom ) 3246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn min_dist_num = dist_num; 3266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn min_dist_denom = dist_denom; 3276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( min_dist_num == 0 ) 3286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 3296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( (v0.y <= pt.y && v.y <= pt.y) || 3326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (v0.y > pt.y && v.y > pt.y) || 3336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (v0.x < pt.x && v.x < pt.x) ) 3346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn continue; 3356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dist_num = dy1*dx - dx1*dy; 3376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( dy < 0 ) 3386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dist_num = -dist_num; 3396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn counter += dist_num > 0; 3406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn result = sqrt(min_dist_num/min_dist_denom); 3436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( counter % 2 == 0 ) 3446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn result = -result; 3456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 3496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return result; 3516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 3526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_IMPL void 3556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RenncvRQDecomp3x3( const CvMat *matrixM, CvMat *matrixR, CvMat *matrixQ, 3566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *matrixQx, CvMat *matrixQy, CvMat *matrixQz, 3576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint3D64f *eulerAngles) 3586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 3596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME("cvRQDecomp3x3"); 3606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 3616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double _M[3][3], _R[3][3], _Q[3][3]; 3636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat M = cvMat(3, 3, CV_64F, _M); 3646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat R = cvMat(3, 3, CV_64F, _R); 3656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat Q = cvMat(3, 3, CV_64F, _Q); 3666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double z, c, s; 3676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Validate parameters. */ 3696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ASSERT( CV_IS_MAT(matrixM) && CV_IS_MAT(matrixR) && CV_IS_MAT(matrixQ) && 3706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn matrixM->cols == 3 && matrixM->rows == 3 && 3716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ARE_SIZES_EQ(matrixM, matrixR) && CV_ARE_SIZES_EQ(matrixM, matrixQ)); 3726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert(matrixM, &M); 3746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Find Givens rotation Q_x for x axis (left multiplication). */ 3776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* 3786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ( 1 0 0 ) 3796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Qx = ( 0 c s ), c = m33/sqrt(m32^2 + m33^2), s = m32/sqrt(m32^2 + m33^2) 3806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ( 0 -s c ) 3816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn */ 3826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn s = _M[2][1]; 3836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn c = _M[2][2]; 3846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn z = 1./sqrt(c * c + s * s + DBL_EPSILON); 3856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn c *= z; 3866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn s *= z; 3876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double _Qx[3][3] = { {1, 0, 0}, {0, c, s}, {0, -s, c} }; 3896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat Qx = cvMat(3, 3, CV_64F, _Qx); 3906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvMatMul(&M, &Qx, &R); 3926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn assert(fabs(_R[2][1]) < FLT_EPSILON); 3936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _R[2][1] = 0; 3946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Find Givens rotation for y axis. */ 3966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* 3976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ( c 0 s ) 3986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Qy = ( 0 1 0 ), c = m33/sqrt(m31^2 + m33^2), s = m31/sqrt(m31^2 + m33^2) 3996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (-s 0 c ) 4006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn */ 4016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn s = _R[2][0]; 4026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn c = _R[2][2]; 4036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn z = 1./sqrt(c * c + s * s + DBL_EPSILON); 4046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn c *= z; 4056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn s *= z; 4066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double _Qy[3][3] = { {c, 0, s}, {0, 1, 0}, {-s, 0, c} }; 4086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat Qy = cvMat(3, 3, CV_64F, _Qy); 4096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvMatMul(&R, &Qy, &M); 4106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn assert(fabs(_M[2][0]) < FLT_EPSILON); 4126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _M[2][0] = 0; 4136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Find Givens rotation for z axis. */ 4156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* 4166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ( c s 0 ) 4176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Qz = (-s c 0 ), c = m22/sqrt(m21^2 + m22^2), s = m21/sqrt(m21^2 + m22^2) 4186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ( 0 0 1 ) 4196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn */ 4206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn s = _M[1][0]; 4226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn c = _M[1][1]; 4236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn z = 1./sqrt(c * c + s * s + DBL_EPSILON); 4246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn c *= z; 4256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn s *= z; 4266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double _Qz[3][3] = { {c, s, 0}, {-s, c, 0}, {0, 0, 1} }; 4286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat Qz = cvMat(3, 3, CV_64F, _Qz); 4296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvMatMul(&M, &Qz, &R); 4316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn assert(fabs(_R[1][0]) < FLT_EPSILON); 4326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _R[1][0] = 0; 4336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // Solve the decomposition ambiguity. 4356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // Diagonal entries of R, except the last one, shall be positive. 4366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // Further rotate R by 180 degree if necessary 4376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( _R[0][0] < 0 ) 4386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( _R[1][1] < 0 ) 4406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // rotate around z for 180 degree, i.e. a rotation matrix of 4426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // [-1, 0, 0], 4436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // [ 0, -1, 0], 4446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // [ 0, 0, 1] 4456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _R[0][0] *= -1; 4466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _R[0][1] *= -1; 4476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _R[1][1] *= -1; 4486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _Qz[0][0] *= -1; 4506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _Qz[0][1] *= -1; 4516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _Qz[1][0] *= -1; 4526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _Qz[1][1] *= -1; 4536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 4556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // rotate around y for 180 degree, i.e. a rotation matrix of 4576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // [-1, 0, 0], 4586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // [ 0, 1, 0], 4596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // [ 0, 0, -1] 4606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _R[0][0] *= -1; 4616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _R[0][2] *= -1; 4626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _R[1][2] *= -1; 4636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _R[2][2] *= -1; 4646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvTranspose( &Qz, &Qz ); 4666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _Qy[0][0] *= -1; 4686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _Qy[0][2] *= -1; 4696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _Qy[2][0] *= -1; 4706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _Qy[2][2] *= -1; 4716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else if( _R[1][1] < 0 ) 4746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // ??? for some reason, we never get here ??? 4766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // rotate around x for 180 degree, i.e. a rotation matrix of 4786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // [ 1, 0, 0], 4796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // [ 0, -1, 0], 4806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // [ 0, 0, -1] 4816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _R[0][1] *= -1; 4826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _R[0][2] *= -1; 4836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _R[1][1] *= -1; 4846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _R[1][2] *= -1; 4856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _R[2][2] *= -1; 4866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvTranspose( &Qz, &Qz ); 4886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvTranspose( &Qy, &Qy ); 4896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _Qx[1][1] *= -1; 4916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _Qx[1][2] *= -1; 4926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _Qx[2][1] *= -1; 4936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _Qx[2][2] *= -1; 4946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // calculate the euler angle 4976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( eulerAngles ) 4986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn eulerAngles->x = acos(_Qx[1][1]) * (_Qx[1][2] >= 0 ? 1 : -1) * (180.0 / CV_PI); 5006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn eulerAngles->y = acos(_Qy[0][0]) * (_Qy[0][2] >= 0 ? 1 : -1) * (180.0 / CV_PI); 5016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn eulerAngles->z = acos(_Qz[0][0]) * (_Qz[0][1] >= 0 ? 1 : -1) * (180.0 / CV_PI); 5026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Calulate orthogonal matrix. */ 5056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* 5066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Q = QzT * QyT * QxT 5076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn */ 5086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGEMM( &Qz, &Qy, 1, 0, 0, &M, CV_GEMM_A_T + CV_GEMM_B_T ); 5096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGEMM( &M, &Qx, 1, 0, 0, &Q, CV_GEMM_B_T ); 5106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Save R and Q matrices. */ 5126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( &R, matrixR ); 5136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( &Q, matrixQ ); 5146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( matrixQx ) 5166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert(&Qx, matrixQx); 5176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( matrixQy ) 5186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert(&Qy, matrixQy); 5196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( matrixQz ) 5206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert(&Qz, matrixQz); 5216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 5246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 5256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_IMPL void 5286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RenncvDecomposeProjectionMatrix( const CvMat *projMatr, CvMat *calibMatr, 5296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *rotMatr, CvMat *posVect, 5306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *rotMatrX, CvMat *rotMatrY, 5316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *rotMatrZ, CvPoint3D64f *eulerAngles) 5326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 5336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *tmpProjMatr = 0; 5346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *tmpMatrixD = 0; 5356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *tmpMatrixV = 0; 5366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *tmpMatrixM = 0; 5376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME("cvDecomposeProjectionMatrix"); 5396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 5406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Validate parameters. */ 5426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if(projMatr == 0 || calibMatr == 0 || rotMatr == 0 || posVect == 0) 5436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR(CV_StsNullPtr, "Some of parameters is a NULL pointer!"); 5446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if(!CV_IS_MAT(projMatr) || !CV_IS_MAT(calibMatr) || !CV_IS_MAT(rotMatr) || !CV_IS_MAT(posVect)) 5466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR(CV_StsUnsupportedFormat, "Input parameters must be a matrices!"); 5476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if(projMatr->cols != 4 || projMatr->rows != 3) 5496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR(CV_StsUnmatchedSizes, "Size of projection matrix must be 3x4!"); 5506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if(calibMatr->cols != 3 || calibMatr->rows != 3 || rotMatr->cols != 3 || rotMatr->rows != 3) 5526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR(CV_StsUnmatchedSizes, "Size of calibration and rotation matrices must be 3x3!"); 5536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if(posVect->cols != 1 || posVect->rows != 4) 5556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR(CV_StsUnmatchedSizes, "Size of position vector must be 4x1!"); 5566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL(tmpProjMatr = cvCreateMat(4, 4, CV_64F)); 5586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL(tmpMatrixD = cvCreateMat(4, 4, CV_64F)); 5596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL(tmpMatrixV = cvCreateMat(4, 4, CV_64F)); 5606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL(tmpMatrixM = cvCreateMat(3, 3, CV_64F)); 5616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Compute position vector. */ 5636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSetZero(tmpProjMatr); // Add zero row to make matrix square. 5656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i, k; 5666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for(i = 0; i < 3; i++) 5676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for(k = 0; k < 4; k++) 5686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(tmpProjMatr, i, k, cvmGet(projMatr, i, k)); 5696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL(cvSVD(tmpProjMatr, tmpMatrixD, NULL, tmpMatrixV, CV_SVD_MODIFY_A + CV_SVD_V_T)); 5716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Save position vector. */ 5736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for(i = 0; i < 4; i++) 5756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(posVect, i, 0, cvmGet(tmpMatrixV, 3, i)); // Solution is last row of V. 5766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Compute calibration and rotation matrices via RQ decomposition. */ 5786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetCols(projMatr, tmpMatrixM, 0, 3); // M is first square matrix of P. 5806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn assert(cvDet(tmpMatrixM) != 0.0); // So far only finite cameras could be decomposed, so M has to be nonsingular [det(M) != 0]. 5826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL(cvRQDecomp3x3(tmpMatrixM, calibMatr, rotMatr, rotMatrX, rotMatrY, rotMatrZ, eulerAngles)); 5846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 5866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat(&tmpProjMatr); 5886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat(&tmpMatrixD); 5896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat(&tmpMatrixV); 5906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat(&tmpMatrixM); 5916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 5926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* End of file. */ 594