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 426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#include "_cv.h" 436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* 456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn This is stright-forward port v3 of Matlab calibration engine by Jean-Yves Bouguet 466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn that is (in a large extent) based on the paper: 476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Z. Zhang. "A flexible new technique for camera calibration". 486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn IEEE Transactions on Pattern Analysis and Machine Intelligence, 22(11):1330-1334, 2000. 496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn The 1st initial port was done by Valery Mosyagin. 516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn*/ 526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCvLevMarq::CvLevMarq() 546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn mask = prevParam = param = J = err = JtJ = JtJN = JtErr = JtJV = JtJW = 0; 566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn lambdaLg10 = 0; state = DONE; 576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn criteria = cvTermCriteria(0,0,0); 586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn iters = 0; 596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn completeSymmFlag = false; 606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCvLevMarq::CvLevMarq( int nparams, int nerrs, CvTermCriteria criteria0, bool _completeSymmFlag ) 636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn mask = prevParam = param = J = err = JtJ = JtJN = JtErr = JtJV = JtJW = 0; 656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn init(nparams, nerrs, criteria0, _completeSymmFlag); 666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid CvLevMarq::clear() 696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat(&mask); 716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat(&prevParam); 726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat(¶m); 736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat(&J); 746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat(&err); 756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat(&JtJ); 766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat(&JtJN); 776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat(&JtErr); 786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat(&JtJV); 796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat(&JtJW); 806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCvLevMarq::~CvLevMarq() 836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn clear(); 856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid CvLevMarq::init( int nparams, int nerrs, CvTermCriteria criteria0, bool _completeSymmFlag ) 886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !param || param->rows != nparams || nerrs != (err ? err->rows : 0) ) 906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn clear(); 916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn mask = cvCreateMat( nparams, 1, CV_8U ); 926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSet(mask, cvScalarAll(1)); 936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn prevParam = cvCreateMat( nparams, 1, CV_64F ); 946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn param = cvCreateMat( nparams, 1, CV_64F ); 956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn JtJ = cvCreateMat( nparams, nparams, CV_64F ); 966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn JtJN = cvCreateMat( nparams, nparams, CV_64F ); 976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn JtJV = cvCreateMat( nparams, nparams, CV_64F ); 986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn JtJW = cvCreateMat( nparams, 1, CV_64F ); 996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn JtErr = cvCreateMat( nparams, 1, CV_64F ); 1006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( nerrs > 0 ) 1016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn J = cvCreateMat( nerrs, nparams, CV_64F ); 1036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn err = cvCreateMat( nerrs, 1, CV_64F ); 1046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn prevErrNorm = DBL_MAX; 1066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn lambdaLg10 = -3; 1076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn criteria = criteria0; 1086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( criteria.type & CV_TERMCRIT_ITER ) 1096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn criteria.max_iter = MIN(MAX(criteria.max_iter,1),1000); 1106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 1116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn criteria.max_iter = 30; 1126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( criteria.type & CV_TERMCRIT_EPS ) 1136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn criteria.epsilon = MAX(criteria.epsilon, 0); 1146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 1156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn criteria.epsilon = DBL_EPSILON; 1166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state = STARTED; 1176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn iters = 0; 1186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn completeSymmFlag = _completeSymmFlag; 1196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 1206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennbool CvLevMarq::update( const CvMat*& _param, CvMat*& _J, CvMat*& _err ) 1226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 1236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double change; 1246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn assert( err != 0 ); 1266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( state == DONE ) 1276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _param = param; 1296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return false; 1306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( state == STARTED ) 1336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _param = param; 1356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvZero( J ); 1366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvZero( err ); 1376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _J = J; 1386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _err = err; 1396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state = CALC_J; 1406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return true; 1416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( state == CALC_J ) 1446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvMulTransposed( J, JtJ, 1 ); 1466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGEMM( J, err, 1, 0, 0, JtErr, CV_GEMM_A_T ); 1476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvCopy( param, prevParam ); 1486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn step(); 1496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( iters == 0 ) 1506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn prevErrNorm = cvNorm(err, 0, CV_L2); 1516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _param = param; 1526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvZero( err ); 1536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _err = err; 1546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state = CHECK_ERR; 1556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return true; 1566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn assert( state == CHECK_ERR ); 1596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn errNorm = cvNorm( err, 0, CV_L2 ); 1606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( errNorm > prevErrNorm ) 1616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn lambdaLg10++; 1636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn step(); 1646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _param = param; 1656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvZero( err ); 1666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _err = err; 1676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state = CHECK_ERR; 1686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return true; 1696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn lambdaLg10 = MAX(lambdaLg10-1, -16); 1726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( ++iters >= criteria.max_iter || 1736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (change = cvNorm(param, prevParam, CV_RELATIVE_L2)) < criteria.epsilon ) 1746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _param = param; 1766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state = DONE; 1776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return true; 1786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn prevErrNorm = errNorm; 1816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _param = param; 1826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvZero(J); 1836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _J = J; 1846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state = CALC_J; 1856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return false; 1866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 1876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennbool CvLevMarq::updateAlt( const CvMat*& _param, CvMat*& _JtJ, CvMat*& _JtErr, double*& _errNorm ) 1906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 1916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double change; 1926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn assert( err == 0 ); 1946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( state == DONE ) 1956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _param = param; 1976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return false; 1986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( state == STARTED ) 2016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _param = param; 2036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvZero( JtJ ); 2046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvZero( JtErr ); 2056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn errNorm = 0; 2066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _JtJ = JtJ; 2076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _JtErr = JtErr; 2086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _errNorm = &errNorm; 2096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state = CALC_J; 2106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return true; 2116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( state == CALC_J ) 2146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvCopy( param, prevParam ); 2166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn step(); 2176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _param = param; 2186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn prevErrNorm = errNorm; 2196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn errNorm = 0; 2206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _errNorm = &errNorm; 2216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state = CHECK_ERR; 2226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return true; 2236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn assert( state == CHECK_ERR ); 2266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( errNorm > prevErrNorm ) 2276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn lambdaLg10++; 2296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn step(); 2306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _param = param; 2316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn errNorm = 0; 2326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _errNorm = &errNorm; 2336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state = CHECK_ERR; 2346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return true; 2356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn lambdaLg10 = MAX(lambdaLg10-1, -16); 2386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( ++iters >= criteria.max_iter || 2396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (change = cvNorm(param, prevParam, CV_RELATIVE_L2)) < criteria.epsilon ) 2406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _param = param; 2426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state = DONE; 2436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return false; 2446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn prevErrNorm = errNorm; 2476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvZero( JtJ ); 2486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvZero( JtErr ); 2496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _param = param; 2506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _JtJ = JtJ; 2516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _JtErr = JtErr; 2526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn state = CALC_J; 2536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return true; 2546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 2556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid CvLevMarq::step() 2576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 2586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const double LOG10 = log(10.); 2596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double lambda = exp(lambdaLg10*LOG10); 2606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i, j, nparams = param->rows; 2616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < nparams; i++ ) 2636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( mask->data.ptr[i] == 0 ) 2646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double *row = JtJ->data.db + i*nparams, *col = JtJ->data.db + i; 2666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < nparams; j++ ) 2676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn row[j] = col[j*nparams] = 0; 2686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn JtErr->data.db[i] = 0; 2696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !err ) 2726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvCompleteSymm( JtJ, completeSymmFlag ); 2736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSetIdentity( JtJN, cvRealScalar(lambda) ); 2746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvAdd( JtJ, JtJN, JtJN ); 2756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSVD( JtJN, JtJW, 0, JtJV, CV_SVD_MODIFY_A + CV_SVD_U_T + CV_SVD_V_T ); 2766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSVBkSb( JtJW, JtJV, JtJV, JtErr, param, CV_SVD_U_T + CV_SVD_V_T ); 2776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < nparams; i++ ) 2786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn param->data.db[i] = prevParam->data.db[i] - (mask->data.ptr[i] ? param->data.db[i] : 0); 2796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 2806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// reimplementation of dAB.m 2826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_IMPL void 2836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RenncvCalcMatMulDeriv( const CvMat* A, const CvMat* B, CvMat* dABdA, CvMat* dABdB ) 2846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 2856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "cvCalcMatMulDeriv" ); 2866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 2886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i, j, M, N, L; 2906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int bstep; 2916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ASSERT( CV_IS_MAT(A) && CV_IS_MAT(B) ); 2936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ASSERT( CV_ARE_TYPES_EQ(A, B) && 2946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (CV_MAT_TYPE(A->type) == CV_32F || CV_MAT_TYPE(A->type) == CV_64F) ); 2956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ASSERT( A->cols == B->rows ); 2966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn M = A->rows; 2986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn L = A->cols; 2996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn N = B->cols; 3006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn bstep = B->step/CV_ELEM_SIZE(B->type); 3016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( dABdA ) 3036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ASSERT( CV_ARE_TYPES_EQ(A, dABdA) && 3056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dABdA->rows == A->rows*B->cols && dABdA->cols == A->rows*A->cols ); 3066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( dABdB ) 3096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ASSERT( CV_ARE_TYPES_EQ(A, dABdB) && 3116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dABdB->rows == A->rows*B->cols && dABdB->cols == B->rows*B->cols ); 3126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( CV_MAT_TYPE(A->type) == CV_32F ) 3156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < M*N; i++ ) 3176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i1 = i / N, i2 = i % N; 3196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( dABdA ) 3216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float* dcda = (float*)(dABdA->data.ptr + dABdA->step*i); 3236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const float* b = (const float*)B->data.ptr + i2; 3246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < M*L; j++ ) 3266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dcda[j] = 0; 3276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < L; j++ ) 3286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dcda[i1*L + j] = b[j*bstep]; 3296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( dABdB ) 3326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float* dcdb = (float*)(dABdB->data.ptr + dABdB->step*i); 3346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const float* a = (const float*)(A->data.ptr + A->step*i1); 3356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < L*N; j++ ) 3376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dcdb[j] = 0; 3386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < L; j++ ) 3396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dcdb[j*N + i2] = a[j]; 3406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 3446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < M*N; i++ ) 3466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i1 = i / N, i2 = i % N; 3486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( dABdA ) 3506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double* dcda = (double*)(dABdA->data.ptr + dABdA->step*i); 3526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const double* b = (const double*)B->data.ptr + i2; 3536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < M*L; j++ ) 3556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dcda[j] = 0; 3566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < L; j++ ) 3576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dcda[i1*L + j] = b[j*bstep]; 3586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( dABdB ) 3616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 3626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double* dcdb = (double*)(dABdB->data.ptr + dABdB->step*i); 3636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const double* a = (const double*)(A->data.ptr + A->step*i1); 3646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < L*N; j++ ) 3666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dcdb[j] = 0; 3676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < L; j++ ) 3686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dcdb[j*N + i2] = a[j]; 3696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 3726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 3746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 3756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// reimplementation of compose_motion.m 3776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_IMPL void 3786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RenncvComposeRT( const CvMat* _rvec1, const CvMat* _tvec1, 3796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const CvMat* _rvec2, const CvMat* _tvec2, 3806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* _rvec3, CvMat* _tvec3, 3816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* dr3dr1, CvMat* dr3dt1, 3826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* dr3dr2, CvMat* dr3dt2, 3836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* dt3dr1, CvMat* dt3dt1, 3846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* dt3dr2, CvMat* dt3dt2 ) 3856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 3866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "cvComposeRT" ); 3876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 3896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double _r1[3], _r2[3]; 3916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double _R1[9], _d1[9*3], _R2[9], _d2[9*3]; 3926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat r1 = cvMat(3,1,CV_64F,_r1), r2 = cvMat(3,1,CV_64F,_r2); 3936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat R1 = cvMat(3,3,CV_64F,_R1), R2 = cvMat(3,3,CV_64F,_R2); 3946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat dR1dr1 = cvMat(9,3,CV_64F,_d1), dR2dr2 = cvMat(9,3,CV_64F,_d2); 3956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ASSERT( CV_IS_MAT(_rvec1) && CV_IS_MAT(_rvec2) ); 3976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ASSERT( CV_MAT_TYPE(_rvec1->type) == CV_32F || 3996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_MAT_TYPE(_rvec1->type) == CV_64F ); 4006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ASSERT( _rvec1->rows == 3 && _rvec1->cols == 1 && CV_ARE_SIZES_EQ(_rvec1, _rvec2) ); 4026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( _rvec1, &r1 ); 4046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( _rvec2, &r2 ); 4056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvRodrigues2( &r1, &R1, &dR1dr1 ); 4076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvRodrigues2( &r2, &R2, &dR2dr2 ); 4086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( _rvec3 || dr3dr1 || dr3dr1 ) 4106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double _r3[3], _R3[9], _dR3dR1[9*9], _dR3dR2[9*9], _dr3dR3[9*3]; 4126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double _W1[9*3], _W2[3*3]; 4136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat r3 = cvMat(3,1,CV_64F,_r3), R3 = cvMat(3,3,CV_64F,_R3); 4146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat dR3dR1 = cvMat(9,9,CV_64F,_dR3dR1), dR3dR2 = cvMat(9,9,CV_64F,_dR3dR2); 4156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat dr3dR3 = cvMat(3,9,CV_64F,_dr3dR3); 4166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat W1 = cvMat(3,9,CV_64F,_W1), W2 = cvMat(3,3,CV_64F,_W2); 4176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvMatMul( &R2, &R1, &R3 ); 4196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvCalcMatMulDeriv( &R2, &R1, &dR3dR2, &dR3dR1 ); 4206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvRodrigues2( &R3, &r3, &dr3dR3 ); 4226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( _rvec3 ) 4246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( &r3, _rvec3 ); 4256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( dr3dr1 ) 4276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvMatMul( &dr3dR3, &dR3dR1, &W1 ); 4296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvMatMul( &W1, &dR1dr1, &W2 ); 4306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( &W2, dr3dr1 ); 4316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( dr3dr2 ) 4346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvMatMul( &dr3dR3, &dR3dR2, &W1 ); 4366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvMatMul( &W1, &dR2dr2, &W2 ); 4376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( &W2, dr3dr2 ); 4386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( dr3dt1 ) 4426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvZero( dr3dt1 ); 4436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( dr3dt2 ) 4446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvZero( dr3dt2 ); 4456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( _tvec3 || dt3dr2 || dt3dt1 ) 4476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double _t1[3], _t2[3], _t3[3], _dxdR2[3*9], _dxdt1[3*3], _W3[3*3]; 4496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat t1 = cvMat(3,1,CV_64F,_t1), t2 = cvMat(3,1,CV_64F,_t2); 4506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat t3 = cvMat(3,1,CV_64F,_t3); 4516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat dxdR2 = cvMat(3, 9, CV_64F, _dxdR2); 4526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat dxdt1 = cvMat(3, 3, CV_64F, _dxdt1); 4536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat W3 = cvMat(3, 3, CV_64F, _W3); 4546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ASSERT( CV_IS_MAT(_tvec1) && CV_IS_MAT(_tvec2) ); 4566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ASSERT( CV_ARE_SIZES_EQ(_tvec1, _tvec2) && CV_ARE_SIZES_EQ(_tvec1, _rvec1) ); 4576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( _tvec1, &t1 ); 4596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( _tvec2, &t2 ); 4606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvMatMulAdd( &R2, &t1, &t2, &t3 ); 4616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( _tvec3 ) 4636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( &t3, _tvec3 ); 4646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( dt3dr2 || dt3dt1 ) 4666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvCalcMatMulDeriv( &R2, &t1, &dxdR2, &dxdt1 ); 4686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( dt3dr2 ) 4696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 4706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvMatMul( &dxdR2, &dR2dr2, &W3 ); 4716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( &W3, dt3dr2 ); 4726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( dt3dt1 ) 4746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( &dxdt1, dt3dt1 ); 4756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 4776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( dt3dt2 ) 4796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSetIdentity( dt3dt2 ); 4806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( dt3dr1 ) 4816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvZero( dt3dr1 ); 4826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 4846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 4856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_IMPL int 4876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RenncvRodrigues2( const CvMat* src, CvMat* dst, CvMat* jacobian ) 4886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 4896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int result = 0; 4906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "cvRogrigues2" ); 4926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 4946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 4956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int depth, elem_size; 4966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i, k; 4976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double J[27]; 4986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _J = cvMat( 3, 9, CV_64F, J ); 4996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(src) ) 5016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( !src ? CV_StsNullPtr : CV_StsBadArg, "Input argument is not a valid matrix" ); 5026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(dst) ) 5046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( !dst ? CV_StsNullPtr : CV_StsBadArg, 5056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn "The first output argument is not a valid matrix" ); 5066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn depth = CV_MAT_DEPTH(src->type); 5086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn elem_size = CV_ELEM_SIZE(depth); 5096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( depth != CV_32F && depth != CV_64F ) 5116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "The matrices must have 32f or 64f data type" ); 5126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_ARE_DEPTHS_EQ(src, dst) ) 5146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedFormats, "All the matrices must have the same data type" ); 5156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( jacobian ) 5176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(jacobian) ) 5196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadArg, "Jacobian is not a valid matrix" ); 5206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_ARE_DEPTHS_EQ(src, jacobian) || CV_MAT_CN(jacobian->type) != 1 ) 5226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedFormats, "Jacobian must have 32fC1 or 64fC1 datatype" ); 5236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( (jacobian->rows != 9 || jacobian->cols != 3) && 5256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (jacobian->rows != 3 || jacobian->cols != 9)) 5266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadSize, "Jacobian must be 3x9 or 9x3" ); 5276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( src->cols == 1 || src->rows == 1 ) 5306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double rx, ry, rz, theta; 5326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int step = src->rows > 1 ? src->step / elem_size : 1; 5336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( src->rows + src->cols*CV_MAT_CN(src->type) - 1 != 3 ) 5356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadSize, "Input matrix must be 1x3, 3x1 or 3x3" ); 5366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( dst->rows != 3 || dst->cols != 3 || CV_MAT_CN(dst->type) != 1 ) 5386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadSize, "Output matrix must be 3x3, single-channel floating point matrix" ); 5396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( depth == CV_32F ) 5416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rx = src->data.fl[0]; 5436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ry = src->data.fl[step]; 5446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rz = src->data.fl[step*2]; 5456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 5476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rx = src->data.db[0]; 5496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ry = src->data.db[step]; 5506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rz = src->data.db[step*2]; 5516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn theta = sqrt(rx*rx + ry*ry + rz*rz); 5536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( theta < DBL_EPSILON ) 5556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSetIdentity( dst ); 5576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( jacobian ) 5596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn memset( J, 0, sizeof(J) ); 5616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn J[5] = J[15] = J[19] = -1; 5626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn J[7] = J[11] = J[21] = 1; 5636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 5656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 5666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const double I[] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; 5686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double c = cos(theta); 5706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double s = sin(theta); 5716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double c1 = 1. - c; 5726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double itheta = theta ? 1./theta : 0.; 5736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rx *= itheta; ry *= itheta; rz *= itheta; 5756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double rrt[] = { rx*rx, rx*ry, rx*rz, rx*ry, ry*ry, ry*rz, rx*rz, ry*rz, rz*rz }; 5776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double _r_x_[] = { 0, -rz, ry, rz, 0, -rx, -ry, rx, 0 }; 5786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double R[9]; 5796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _R = cvMat( 3, 3, CV_64F, R ); 5806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // R = cos(theta)*I + (1 - cos(theta))*r*rT + sin(theta)*[r_x] 5826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // where [r_x] is [0 -rz ry; rz 0 -rx; -ry rx 0] 5836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( k = 0; k < 9; k++ ) 5846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn R[k] = c*I[k] + c1*rrt[k] + s*_r_x_[k]; 5856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( &_R, dst ); 5876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 5886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( jacobian ) 5896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double drrt[] = { rx+rx, ry, rz, ry, 0, 0, rz, 0, 0, 5916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 0, rx, 0, rx, ry+ry, rz, 0, rz, 0, 5926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 0, 0, rx, 0, 0, ry, rx, ry, rz+rz }; 5936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double d_r_x_[] = { 0, 0, 0, 0, 0, -1, 0, 1, 0, 5946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 0, 0, 1, 0, 0, 0, -1, 0, 0, 5956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 0, -1, 0, 1, 0, 0, 0, 0, 0 }; 5966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < 3; i++ ) 5976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 5986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double ri = i == 0 ? rx : i == 1 ? ry : rz; 5996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double a0 = -s*ri, a1 = (s - 2*c1*itheta)*ri, a2 = c1*itheta; 6006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double a3 = (c - s*itheta)*ri, a4 = s*itheta; 6016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( k = 0; k < 9; k++ ) 6026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn J[i*9+k] = a0*I[k] + a1*rrt[k] + a2*drrt[i*9+k] + 6036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a3*_r_x_[k] + a4*d_r_x_[i*9+k]; 6046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else if( src->cols == 3 && src->rows == 3 ) 6096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double R[9], U[9], V[9], W[3], rx, ry, rz; 6116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _R = cvMat( 3, 3, CV_64F, R ); 6126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _U = cvMat( 3, 3, CV_64F, U ); 6136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _V = cvMat( 3, 3, CV_64F, V ); 6146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _W = cvMat( 3, 1, CV_64F, W ); 6156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double theta, s, c; 6166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int step = dst->rows > 1 ? dst->step / elem_size : 1; 6176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( (dst->rows != 1 || dst->cols*CV_MAT_CN(dst->type) != 3) && 6196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (dst->rows != 3 || dst->cols != 1 || CV_MAT_CN(dst->type) != 1)) 6206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadSize, "Output matrix must be 1x3 or 3x1" ); 6216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( src, &_R ); 6236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !cvCheckArr( &_R, CV_CHECK_RANGE+CV_CHECK_QUIET, -100, 100 ) ) 6246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvZero(dst); 6266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( jacobian ) 6276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvZero(jacobian); 6286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn EXIT; 6296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSVD( &_R, &_W, &_U, &_V, CV_SVD_MODIFY_A + CV_SVD_U_T + CV_SVD_V_T ); 6326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGEMM( &_U, &_V, 1, 0, 0, &_R, CV_GEMM_A_T ); 6336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rx = R[7] - R[5]; 6356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ry = R[2] - R[6]; 6366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rz = R[3] - R[1]; 6376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn s = sqrt((rx*rx + ry*ry + rz*rz)*0.25); 6396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn c = (R[0] + R[4] + R[8] - 1)*0.5; 6406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn c = c > 1. ? 1. : c < -1. ? -1. : c; 6416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn theta = acos(c); 6426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( s < 1e-5 ) 6446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double t; 6466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( c > 0 ) 6486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rx = ry = rz = 0; 6496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 6506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t = (R[0] + 1)*0.5; 6526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rx = theta*sqrt(MAX(t,0.)); 6536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t = (R[4] + 1)*0.5; 6546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ry = theta*sqrt(MAX(t,0.))*(R[1] < 0 ? -1. : 1.); 6556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t = (R[8] + 1)*0.5; 6566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rz = theta*sqrt(MAX(t,0.))*(R[2] < 0 ? -1. : 1.); 6576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( jacobian ) 6606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn memset( J, 0, sizeof(J) ); 6626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( c > 0 ) 6636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn J[5] = J[15] = J[19] = -0.5; 6656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn J[7] = J[11] = J[21] = 0.5; 6666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 6696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 6706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double vth = 1/(2*s); 6726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 6736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( jacobian ) 6746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double t, dtheta_dtr = -1./s; 6766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // var1 = [vth;theta] 6776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // var = [om1;var1] = [om1;vth;theta] 6786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double dvth_dtheta = -vth*c/s; 6796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double d1 = 0.5*dvth_dtheta*dtheta_dtr; 6806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double d2 = 0.5*dtheta_dtr; 6816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // dvar1/dR = dvar1/dtheta*dtheta/dR = [dvth/dtheta; 1] * dtheta/dtr * dtr/dR 6826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double dvardR[5*9] = 6836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 0, 0, 0, 0, 0, 1, 0, -1, 0, 6856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 0, 0, -1, 0, 0, 0, 1, 0, 0, 6866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 0, 1, 0, -1, 0, 0, 0, 0, 0, 6876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn d1, 0, 0, 0, d1, 0, 0, 0, d1, 6886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn d2, 0, 0, 0, d2, 0, 0, 0, d2 6896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn }; 6906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // var2 = [om;theta] 6916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double dvar2dvar[] = 6926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 6936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn vth, 0, 0, rx, 0, 6946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 0, vth, 0, ry, 0, 6956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 0, 0, vth, rz, 0, 6966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 0, 0, 0, 0, 1 6976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn }; 6986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double domegadvar2[] = 6996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn theta, 0, 0, rx*vth, 7016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 0, theta, 0, ry*vth, 7026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 0, 0, theta, rz*vth 7036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn }; 7046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _dvardR = cvMat( 5, 9, CV_64FC1, dvardR ); 7066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _dvar2dvar = cvMat( 4, 5, CV_64FC1, dvar2dvar ); 7076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _domegadvar2 = cvMat( 3, 4, CV_64FC1, domegadvar2 ); 7086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double t0[3*5]; 7096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _t0 = cvMat( 3, 5, CV_64FC1, t0 ); 7106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvMatMul( &_domegadvar2, &_dvar2dvar, &_t0 ); 7126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvMatMul( &_t0, &_dvardR, &_J ); 7136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // transpose every row of _J (treat the rows as 3x3 matrices) 7156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_SWAP(J[1], J[3], t); CV_SWAP(J[2], J[6], t); CV_SWAP(J[5], J[7], t); 7166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_SWAP(J[10], J[12], t); CV_SWAP(J[11], J[15], t); CV_SWAP(J[14], J[16], t); 7176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_SWAP(J[19], J[21], t); CV_SWAP(J[20], J[24], t); CV_SWAP(J[23], J[25], t); 7186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn vth *= theta; 7216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rx *= vth; ry *= vth; rz *= vth; 7226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( depth == CV_32F ) 7256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dst->data.fl[0] = (float)rx; 7276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dst->data.fl[step] = (float)ry; 7286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dst->data.fl[step*2] = (float)rz; 7296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 7316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dst->data.db[0] = rx; 7336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dst->data.db[step] = ry; 7346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dst->data.db[step*2] = rz; 7356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( jacobian ) 7396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( depth == CV_32F ) 7416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( jacobian->rows == _J.rows ) 7436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( &_J, jacobian ); 7446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 7456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 7466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float Jf[3*9]; 7476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _Jf = cvMat( _J.rows, _J.cols, CV_32FC1, Jf ); 7486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( &_J, &_Jf ); 7496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvTranspose( &_Jf, jacobian ); 7506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else if( jacobian->rows == _J.rows ) 7536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvCopy( &_J, jacobian ); 7546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 7556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvTranspose( &_J, jacobian ); 7566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 7576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn result = 1; 7596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 7616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return result; 7636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 7646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_IMPL void 7676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RenncvProjectPoints2( const CvMat* objectPoints, 7686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const CvMat* r_vec, 7696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const CvMat* t_vec, 7706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const CvMat* A, 7716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const CvMat* distCoeffs, 7726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* imagePoints, CvMat* dpdr, 7736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* dpdt, CvMat* dpdf, 7746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* dpdc, CvMat* dpdk, 7756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double aspectRatio ) 7766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 7776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *_M = 0, *_m = 0; 7786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *_dpdr = 0, *_dpdt = 0, *_dpdc = 0, *_dpdf = 0, *_dpdk = 0; 7796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "cvProjectPoints2" ); 7816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 7836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i, j, count; 7856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int calc_derivatives; 7866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const CvPoint3D64f* M; 7876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D64f* m; 7886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double r[3], R[9], dRdr[27], t[3], a[9], k[5] = {0,0,0,0,0}, fx, fy, cx, cy; 7896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _r, _t, _a = cvMat( 3, 3, CV_64F, a ), _k; 7906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _R = cvMat( 3, 3, CV_64F, R ), _dRdr = cvMat( 3, 9, CV_64F, dRdr ); 7916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double *dpdr_p = 0, *dpdt_p = 0, *dpdk_p = 0, *dpdf_p = 0, *dpdc_p = 0; 7926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int dpdr_step = 0, dpdt_step = 0, dpdk_step = 0, dpdf_step = 0, dpdc_step = 0; 7936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn bool fixedAspectRatio = aspectRatio > FLT_EPSILON; 7946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 7956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(objectPoints) || !CV_IS_MAT(r_vec) || 7966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn !CV_IS_MAT(t_vec) || !CV_IS_MAT(A) || 7976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /*!CV_IS_MAT(distCoeffs) ||*/ !CV_IS_MAT(imagePoints) ) 7986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadArg, "One of required arguments is not a valid matrix" ); 7996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn count = MAX(objectPoints->rows, objectPoints->cols); 8016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( CV_IS_CONT_MAT(objectPoints->type) && CV_MAT_DEPTH(objectPoints->type) == CV_64F && 8036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ((objectPoints->rows == 1 && CV_MAT_CN(objectPoints->type) == 3) || 8046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (objectPoints->rows == count && CV_MAT_CN(objectPoints->type)*objectPoints->cols == 3))) 8056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _M = (CvMat*)objectPoints; 8066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 8076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( _M = cvCreateMat( 1, count, CV_64FC3 )); 8096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( cvConvertPointsHomogeneous( objectPoints, _M )); 8106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( CV_IS_CONT_MAT(imagePoints->type) && CV_MAT_DEPTH(imagePoints->type) == CV_64F && 8136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ((imagePoints->rows == 1 && CV_MAT_CN(imagePoints->type) == 2) || 8146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (imagePoints->rows == count && CV_MAT_CN(imagePoints->type)*imagePoints->cols == 2))) 8156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _m = imagePoints; 8166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 8176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( _m = cvCreateMat( 1, count, CV_64FC2 )); 8186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn M = (CvPoint3D64f*)_M->data.db; 8206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn m = (CvPoint2D64f*)_m->data.db; 8216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( (CV_MAT_DEPTH(r_vec->type) != CV_64F && CV_MAT_DEPTH(r_vec->type) != CV_32F) || 8236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (((r_vec->rows != 1 && r_vec->cols != 1) || 8246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn r_vec->rows*r_vec->cols*CV_MAT_CN(r_vec->type) != 3) && 8256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ((r_vec->rows != 3 && r_vec->cols != 3) || CV_MAT_CN(r_vec->type) != 1))) 8266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadArg, "Rotation must be represented by 1x3 or 3x1 " 8276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn "floating-point rotation vector, or 3x3 rotation matrix" ); 8286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( r_vec->rows == 3 && r_vec->cols == 3 ) 8306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _r = cvMat( 3, 1, CV_64FC1, r ); 8326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( cvRodrigues2( r_vec, &_r )); 8336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( cvRodrigues2( &_r, &_R, &_dRdr )); 8346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvCopy( r_vec, &_R ); 8356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 8376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _r = cvMat( r_vec->rows, r_vec->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(r_vec->type)), r ); 8396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( cvConvert( r_vec, &_r )); 8406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( cvRodrigues2( &_r, &_R, &_dRdr ) ); 8416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( (CV_MAT_DEPTH(t_vec->type) != CV_64F && CV_MAT_DEPTH(t_vec->type) != CV_32F) || 8446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (t_vec->rows != 1 && t_vec->cols != 1) || 8456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t_vec->rows*t_vec->cols*CV_MAT_CN(t_vec->type) != 3 ) 8466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadArg, 8476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn "Translation vector must be 1x3 or 3x1 floating-point vector" ); 8486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _t = cvMat( t_vec->rows, t_vec->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(t_vec->type)), t ); 8506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( cvConvert( t_vec, &_t )); 8516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( (CV_MAT_TYPE(A->type) != CV_64FC1 && CV_MAT_TYPE(A->type) != CV_32FC1) || 8536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn A->rows != 3 || A->cols != 3 ) 8546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadArg, "Instrinsic parameters must be 3x3 floating-point matrix" ); 8556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( cvConvert( A, &_a )); 8576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fx = a[0]; fy = a[4]; 8586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cx = a[2]; cy = a[5]; 8596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( fixedAspectRatio ) 8616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fx = fy*aspectRatio; 8626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( distCoeffs ) 8646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(distCoeffs) || 8666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (CV_MAT_DEPTH(distCoeffs->type) != CV_64F && 8676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_MAT_DEPTH(distCoeffs->type) != CV_32F) || 8686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (distCoeffs->rows != 1 && distCoeffs->cols != 1) || 8696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 4 && 8706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 5) ) 8716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadArg, 8726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn "Distortion coefficients must be 1x4, 4x1, 1x5 or 5x1 floating-point vector" ); 8736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _k = cvMat( distCoeffs->rows, distCoeffs->cols, 8756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_MAKETYPE(CV_64F,CV_MAT_CN(distCoeffs->type)), k ); 8766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( cvConvert( distCoeffs, &_k )); 8776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( dpdr ) 8806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(dpdr) || 8826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (CV_MAT_TYPE(dpdr->type) != CV_32FC1 && 8836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_MAT_TYPE(dpdr->type) != CV_64FC1) || 8846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dpdr->rows != count*2 || dpdr->cols != 3 ) 8856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadArg, "dp/drot must be 2Nx3 floating-point matrix" ); 8866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( CV_MAT_TYPE(dpdr->type) == CV_64FC1 ) 8886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _dpdr = dpdr; 8896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 8906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( _dpdr = cvCreateMat( 2*count, 3, CV_64FC1 )); 8916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dpdr_p = _dpdr->data.db; 8926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dpdr_step = _dpdr->step/sizeof(dpdr_p[0]); 8936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 8946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 8956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( dpdt ) 8966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 8976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(dpdt) || 8986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (CV_MAT_TYPE(dpdt->type) != CV_32FC1 && 8996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_MAT_TYPE(dpdt->type) != CV_64FC1) || 9006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dpdt->rows != count*2 || dpdt->cols != 3 ) 9016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadArg, "dp/dT must be 2Nx3 floating-point matrix" ); 9026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( CV_MAT_TYPE(dpdt->type) == CV_64FC1 ) 9046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _dpdt = dpdt; 9056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 9066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( _dpdt = cvCreateMat( 2*count, 3, CV_64FC1 )); 9076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dpdt_p = _dpdt->data.db; 9086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dpdt_step = _dpdt->step/sizeof(dpdt_p[0]); 9096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 9106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( dpdf ) 9126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 9136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(dpdf) || 9146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (CV_MAT_TYPE(dpdf->type) != CV_32FC1 && CV_MAT_TYPE(dpdf->type) != CV_64FC1) || 9156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dpdf->rows != count*2 || dpdf->cols != 2 ) 9166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadArg, "dp/df must be 2Nx2 floating-point matrix" ); 9176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( CV_MAT_TYPE(dpdf->type) == CV_64FC1 ) 9196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _dpdf = dpdf; 9206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 9216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( _dpdf = cvCreateMat( 2*count, 2, CV_64FC1 )); 9226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dpdf_p = _dpdf->data.db; 9236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dpdf_step = _dpdf->step/sizeof(dpdf_p[0]); 9246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 9256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( dpdc ) 9276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 9286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(dpdc) || 9296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (CV_MAT_TYPE(dpdc->type) != CV_32FC1 && CV_MAT_TYPE(dpdc->type) != CV_64FC1) || 9306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dpdc->rows != count*2 || dpdc->cols != 2 ) 9316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadArg, "dp/dc must be 2Nx2 floating-point matrix" ); 9326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( CV_MAT_TYPE(dpdc->type) == CV_64FC1 ) 9346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _dpdc = dpdc; 9356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 9366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( _dpdc = cvCreateMat( 2*count, 2, CV_64FC1 )); 9376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dpdc_p = _dpdc->data.db; 9386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dpdc_step = _dpdc->step/sizeof(dpdc_p[0]); 9396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 9406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( dpdk ) 9426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 9436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(dpdk) || 9446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (CV_MAT_TYPE(dpdk->type) != CV_32FC1 && CV_MAT_TYPE(dpdk->type) != CV_64FC1) || 9456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dpdk->rows != count*2 || (dpdk->cols != 5 && dpdk->cols != 4 && dpdk->cols != 2) ) 9466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadArg, "dp/df must be 2Nx5, 2Nx4 or 2Nx2 floating-point matrix" ); 9476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !distCoeffs ) 9496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNullPtr, "distCoeffs is NULL while dpdk is not" ); 9506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( CV_MAT_TYPE(dpdk->type) == CV_64FC1 ) 9526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _dpdk = dpdk; 9536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 9546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( _dpdk = cvCreateMat( dpdk->rows, dpdk->cols, CV_64FC1 )); 9556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dpdk_p = _dpdk->data.db; 9566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dpdk_step = _dpdk->step/sizeof(dpdk_p[0]); 9576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 9586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn calc_derivatives = dpdr || dpdt || dpdf || dpdc || dpdk; 9606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < count; i++ ) 9626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 9636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double X = M[i].x, Y = M[i].y, Z = M[i].z; 9646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double x = R[0]*X + R[1]*Y + R[2]*Z + t[0]; 9656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double y = R[3]*X + R[4]*Y + R[5]*Z + t[1]; 9666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double z = R[6]*X + R[7]*Y + R[8]*Z + t[2]; 9676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double r2, r4, r6, a1, a2, a3, cdist; 9686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double xd, yd; 9696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn z = z ? 1./z : 1; 9716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x *= z; y *= z; 9726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn r2 = x*x + y*y; 9746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn r4 = r2*r2; 9756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn r6 = r4*r2; 9766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a1 = 2*x*y; 9776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a2 = r2 + 2*x*x; 9786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a3 = r2 + 2*y*y; 9796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cdist = 1 + k[0]*r2 + k[1]*r4 + k[4]*r6; 9806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn xd = x*cdist + k[2]*a1 + k[3]*a2; 9816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn yd = y*cdist + k[2]*a3 + k[3]*a1; 9826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn m[i].x = xd*fx + cx; 9846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn m[i].y = yd*fy + cy; 9856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( calc_derivatives ) 9876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 9886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( dpdc_p ) 9896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 9906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dpdc_p[0] = 1; dpdc_p[1] = 0; 9916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dpdc_p[dpdc_step] = 0; 9926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dpdc_p[dpdc_step+1] = 1; 9936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dpdc_p += dpdc_step*2; 9946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 9956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 9966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( dpdf_p ) 9976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 9986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( fixedAspectRatio ) 9996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 10006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dpdf_p[0] = 0; dpdf_p[1] = xd*aspectRatio; 10016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dpdf_p[dpdf_step] = 0; 10026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dpdf_p[dpdf_step+1] = yd; 10036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 10056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 10066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dpdf_p[0] = xd; dpdf_p[1] = 0; 10076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dpdf_p[dpdf_step] = 0; 10086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dpdf_p[dpdf_step+1] = yd; 10096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dpdf_p += dpdf_step*2; 10116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( dpdk_p ) 10146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 10156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dpdk_p[0] = fx*x*r2; 10166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dpdk_p[1] = fx*x*r4; 10176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dpdk_p[dpdk_step] = fy*y*r2; 10186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dpdk_p[dpdk_step+1] = fy*y*r4; 10196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( _dpdk->cols > 2 ) 10206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 10216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dpdk_p[2] = fx*a1; 10226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dpdk_p[3] = fx*a2; 10236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dpdk_p[dpdk_step+2] = fy*a3; 10246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dpdk_p[dpdk_step+3] = fy*a1; 10256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( _dpdk->cols > 4 ) 10266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 10276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dpdk_p[4] = fx*x*r6; 10286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dpdk_p[dpdk_step+4] = fy*y*r6; 10296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dpdk_p += dpdk_step*2; 10326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( dpdt_p ) 10356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 10366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double dxdt[] = { z, 0, -x*z }, dydt[] = { 0, z, -y*z }; 10376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < 3; j++ ) 10386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 10396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double dr2dt = 2*x*dxdt[j] + 2*y*dydt[j]; 10406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double dcdist_dt = k[0]*dr2dt + 2*k[1]*r2*dr2dt + 3*k[4]*r4*dr2dt; 10416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double da1dt = 2*(x*dydt[j] + y*dxdt[j]); 10426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double dmxdt = fx*(dxdt[j]*cdist + x*dcdist_dt + 10436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn k[2]*da1dt + k[3]*(dr2dt + 2*x*dxdt[j])); 10446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double dmydt = fy*(dydt[j]*cdist + y*dcdist_dt + 10456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn k[2]*(dr2dt + 2*y*dydt[j]) + k[3]*da1dt); 10466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dpdt_p[j] = dmxdt; 10476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dpdt_p[dpdt_step+j] = dmydt; 10486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dpdt_p += dpdt_step*2; 10506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( dpdr_p ) 10536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 10546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double dx0dr[] = 10556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 10566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn X*dRdr[0] + Y*dRdr[1] + Z*dRdr[2], 10576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn X*dRdr[9] + Y*dRdr[10] + Z*dRdr[11], 10586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn X*dRdr[18] + Y*dRdr[19] + Z*dRdr[20] 10596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn }; 10606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double dy0dr[] = 10616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 10626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn X*dRdr[3] + Y*dRdr[4] + Z*dRdr[5], 10636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn X*dRdr[12] + Y*dRdr[13] + Z*dRdr[14], 10646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn X*dRdr[21] + Y*dRdr[22] + Z*dRdr[23] 10656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn }; 10666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double dz0dr[] = 10676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 10686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn X*dRdr[6] + Y*dRdr[7] + Z*dRdr[8], 10696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn X*dRdr[15] + Y*dRdr[16] + Z*dRdr[17], 10706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn X*dRdr[24] + Y*dRdr[25] + Z*dRdr[26] 10716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn }; 10726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < 3; j++ ) 10736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 10746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double dxdr = z*(dx0dr[j] - x*dz0dr[j]); 10756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double dydr = z*(dy0dr[j] - y*dz0dr[j]); 10766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double dr2dr = 2*x*dxdr + 2*y*dydr; 10776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double dcdist_dr = k[0]*dr2dr + 2*k[1]*r2*dr2dr + 3*k[4]*r4*dr2dr; 10786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double da1dr = 2*(x*dydr + y*dxdr); 10796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double dmxdr = fx*(dxdr*cdist + x*dcdist_dr + 10806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn k[2]*da1dr + k[3]*(dr2dr + 2*x*dxdr)); 10816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double dmydr = fy*(dydr*cdist + y*dcdist_dr + 10826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn k[2]*(dr2dr + 2*y*dydr) + k[3]*da1dr); 10836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dpdr_p[j] = dmxdr; 10846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dpdr_p[dpdr_step+j] = dmydr; 10856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dpdr_p += dpdr_step*2; 10876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 10906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 10916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( _m != imagePoints ) 10926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvertPointsHomogeneous( _m, imagePoints ); 10936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( _dpdr != dpdr ) 10946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( _dpdr, dpdr ); 10956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( _dpdt != dpdt ) 10966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( _dpdt, dpdt ); 10976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( _dpdf != dpdf ) 10986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( _dpdf, dpdf ); 10996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( _dpdc != dpdc ) 11006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( _dpdc, dpdc ); 11016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( _dpdk != dpdk ) 11026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( _dpdk, dpdk ); 11036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 11056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( _M != objectPoints ) 11076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &_M ); 11086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( _m != imagePoints ) 11096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &_m ); 11106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( _dpdr != dpdr ) 11116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &_dpdr ); 11126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( _dpdt != dpdt ) 11136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &_dpdt ); 11146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( _dpdf != dpdf ) 11156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &_dpdf ); 11166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( _dpdc != dpdc ) 11176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &_dpdc ); 11186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( _dpdk != dpdk ) 11196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &_dpdk ); 11206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 11216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_IMPL void 11246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RenncvFindExtrinsicCameraParams2( const CvMat* objectPoints, 11256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const CvMat* imagePoints, const CvMat* A, 11266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const CvMat* distCoeffs, 11276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* rvec, CvMat* tvec ) 11286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 11296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const int max_iter = 20; 11306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *_M = 0, *_Mxy = 0, *_m = 0, *_mn = 0, *_L = 0, *_J = 0; 11316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "cvFindExtrinsicCameraParams2" ); 11336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 11356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i, count; 11376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double a[9], ar[9]={1,0,0,0,1,0,0,0,1}, R[9]; 11386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double MM[9], U[9], V[9], W[3]; 11396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvScalar Mc; 11406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double JtJ[6*6], JtErr[6], JtJW[6], JtJV[6*6], delta[6], param[6]; 11416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _A = cvMat( 3, 3, CV_64F, a ); 11426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _Ar = cvMat( 3, 3, CV_64F, ar ); 11436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _R = cvMat( 3, 3, CV_64F, R ); 11446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _r = cvMat( 3, 1, CV_64F, param ); 11456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _t = cvMat( 3, 1, CV_64F, param + 3 ); 11466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _Mc = cvMat( 1, 3, CV_64F, Mc.val ); 11476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _MM = cvMat( 3, 3, CV_64F, MM ); 11486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _U = cvMat( 3, 3, CV_64F, U ); 11496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _V = cvMat( 3, 3, CV_64F, V ); 11506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _W = cvMat( 3, 1, CV_64F, W ); 11516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _JtJ = cvMat( 6, 6, CV_64F, JtJ ); 11526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _JtErr = cvMat( 6, 1, CV_64F, JtErr ); 11536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _JtJW = cvMat( 6, 1, CV_64F, JtJW ); 11546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _JtJV = cvMat( 6, 6, CV_64F, JtJV ); 11556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _delta = cvMat( 6, 1, CV_64F, delta ); 11566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _param = cvMat( 6, 1, CV_64F, param ); 11576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _dpdr, _dpdt; 11586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ASSERT( CV_IS_MAT(objectPoints) && CV_IS_MAT(imagePoints) && 11606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_IS_MAT(A) && CV_IS_MAT(rvec) && CV_IS_MAT(tvec) ); 11616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn count = MAX(objectPoints->cols, objectPoints->rows); 11636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( _M = cvCreateMat( 1, count, CV_64FC3 )); 11646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( _m = cvCreateMat( 1, count, CV_64FC2 )); 11656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( cvConvertPointsHomogeneous( objectPoints, _M )); 11676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( cvConvertPointsHomogeneous( imagePoints, _m )); 11686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( cvConvert( A, &_A )); 11696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ASSERT( (CV_MAT_DEPTH(rvec->type) == CV_64F || CV_MAT_DEPTH(rvec->type) == CV_32F) && 11716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (rvec->rows == 1 || rvec->cols == 1) && rvec->rows*rvec->cols*CV_MAT_CN(rvec->type) == 3 ); 11726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ASSERT( (CV_MAT_DEPTH(tvec->type) == CV_64F || CV_MAT_DEPTH(tvec->type) == CV_32F) && 11746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (tvec->rows == 1 || tvec->cols == 1) && tvec->rows*tvec->cols*CV_MAT_CN(tvec->type) == 3 ); 11756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( _mn = cvCreateMat( 1, count, CV_64FC2 )); 11776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( _Mxy = cvCreateMat( 1, count, CV_64FC2 )); 11786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // normalize image points 11806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // (unapply the intrinsic matrix transformation and distortion) 11816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvUndistortPoints( _m, _mn, &_A, distCoeffs, 0, &_Ar ); 11826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Mc = cvAvg(_M); 11846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReshape( _M, _M, 1, count ); 11856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvMulTransposed( _M, &_MM, 1, &_Mc ); 11866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSVD( &_MM, &_W, 0, &_V, CV_SVD_MODIFY_A + CV_SVD_V_T ); 11876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // initialize extrinsic parameters 11896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( W[2]/W[1] < 1e-3 || count < 4 ) 11906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 11916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // a planar structure case (all M's lie in the same plane) 11926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double tt[3], h[9], h1_norm, h2_norm; 11936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* R_transform = &_V; 11946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat T_transform = cvMat( 3, 1, CV_64F, tt ); 11956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _H = cvMat( 3, 3, CV_64F, h ); 11966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _h1, _h2, _h3; 11976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 11986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( V[2]*V[2] + V[5]*V[5] < 1e-10 ) 11996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSetIdentity( R_transform ); 12006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( cvDet(R_transform) < 0 ) 12026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvScale( R_transform, R_transform, -1 ); 12036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGEMM( R_transform, &_Mc, -1, 0, 0, &T_transform, CV_GEMM_B_T ); 12056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < count; i++ ) 12076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 12086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const double* Rp = R_transform->data.db; 12096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const double* Tp = T_transform.data.db; 12106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const double* src = _M->data.db + i*3; 12116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double* dst = _Mxy->data.db + i*2; 12126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dst[0] = Rp[0]*src[0] + Rp[1]*src[1] + Rp[2]*src[2] + Tp[0]; 12146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dst[1] = Rp[3]*src[0] + Rp[4]*src[1] + Rp[5]*src[2] + Tp[1]; 12156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 12166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFindHomography( _Mxy, _mn, &_H ); 12186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetCol( &_H, &_h1, 0 ); 12206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _h2 = _h1; _h2.data.db++; 12216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _h3 = _h2; _h3.data.db++; 12226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn h1_norm = sqrt(h[0]*h[0] + h[3]*h[3] + h[6]*h[6]); 12236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn h2_norm = sqrt(h[1]*h[1] + h[4]*h[4] + h[7]*h[7]); 12246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvScale( &_h1, &_h1, 1./h1_norm ); 12266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvScale( &_h2, &_h2, 1./h2_norm ); 12276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvScale( &_h3, &_t, 2./(h1_norm + h2_norm)); 12286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvCrossProduct( &_h1, &_h2, &_h3 ); 12296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvRodrigues2( &_H, &_r ); 12316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvRodrigues2( &_r, &_H ); 12326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvMatMulAdd( &_H, &T_transform, &_t, &_t ); 12336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvMatMul( &_H, R_transform, &_R ); 12346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvRodrigues2( &_R, &_r ); 12356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 12366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 12376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 12386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // non-planar structure. Use DLT method 12396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double* L; 12406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double LL[12*12], LW[12], LV[12*12], sc; 12416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _LL = cvMat( 12, 12, CV_64F, LL ); 12426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _LW = cvMat( 12, 1, CV_64F, LW ); 12436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _LV = cvMat( 12, 12, CV_64F, LV ); 12446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _RRt, _RR, _tt; 12456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint3D64f* M = (CvPoint3D64f*)_M->data.db; 12466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D64f* mn = (CvPoint2D64f*)_mn->data.db; 12476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( _L = cvCreateMat( 2*count, 12, CV_64F )); 12496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn L = _L->data.db; 12506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < count; i++, L += 24 ) 12526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 12536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double x = -mn[i].x, y = -mn[i].y; 12546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn L[0] = L[16] = M[i].x; 12556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn L[1] = L[17] = M[i].y; 12566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn L[2] = L[18] = M[i].z; 12576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn L[3] = L[19] = 1.; 12586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn L[4] = L[5] = L[6] = L[7] = 0.; 12596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn L[12] = L[13] = L[14] = L[15] = 0.; 12606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn L[8] = x*M[i].x; 12616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn L[9] = x*M[i].y; 12626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn L[10] = x*M[i].z; 12636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn L[11] = x; 12646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn L[20] = y*M[i].x; 12656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn L[21] = y*M[i].y; 12666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn L[22] = y*M[i].z; 12676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn L[23] = y; 12686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 12696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvMulTransposed( _L, &_LL, 1 ); 12716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSVD( &_LL, &_LW, 0, &_LV, CV_SVD_MODIFY_A + CV_SVD_V_T ); 12726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _RRt = cvMat( 3, 4, CV_64F, LV + 11*12 ); 12736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetCols( &_RRt, &_RR, 0, 3 ); 12746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetCol( &_RRt, &_tt, 3 ); 12756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( cvDet(&_RR) < 0 ) 12766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvScale( &_RRt, &_RRt, -1 ); 12776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sc = cvNorm(&_RR); 12786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSVD( &_RR, &_W, &_U, &_V, CV_SVD_MODIFY_A + CV_SVD_U_T + CV_SVD_V_T ); 12796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGEMM( &_U, &_V, 1, 0, 0, &_R, CV_GEMM_A_T ); 12806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvScale( &_tt, &_t, cvNorm(&_R)/sc ); 12816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvRodrigues2( &_R, &_r ); 12826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &_L ); 12836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 12846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReshape( _M, _M, 3, 1 ); 12866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReshape( _mn, _mn, 2, 1 ); 12876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( _J = cvCreateMat( 2*count, 6, CV_64FC1 )); 12896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetCols( _J, &_dpdr, 0, 3 ); 12906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetCols( _J, &_dpdt, 3, 6 ); 12916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 12926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // refine extrinsic parameters using iterative algorithm 12936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < max_iter; i++ ) 12946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 12956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double n1, n2; 12966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReshape( _mn, _mn, 2, 1 ); 12976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvProjectPoints2( _M, &_r, &_t, &_A, distCoeffs, 12986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _mn, &_dpdr, &_dpdt, 0, 0, 0 ); 12996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSub( _m, _mn, _mn ); 13006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReshape( _mn, _mn, 1, 2*count ); 13016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvMulTransposed( _J, &_JtJ, 1 ); 13036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGEMM( _J, _mn, 1, 0, 0, &_JtErr, CV_GEMM_A_T ); 13046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSVD( &_JtJ, &_JtJW, 0, &_JtJV, CV_SVD_MODIFY_A + CV_SVD_V_T ); 13056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( JtJW[5]/JtJW[0] < 1e-12 ) 13066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 13076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSVBkSb( &_JtJW, &_JtJV, &_JtJV, &_JtErr, 13086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn &_delta, CV_SVD_U_T + CV_SVD_V_T ); 13096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvAdd( &_delta, &_param, &_param ); 13106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn n1 = cvNorm( &_delta ); 13116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn n2 = cvNorm( &_param ); 13126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( n1/n2 < 1e-10 ) 13136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 13146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 13156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _r = cvMat( rvec->rows, rvec->cols, 13176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_MAKETYPE(CV_64F,CV_MAT_CN(rvec->type)), param ); 13186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _t = cvMat( tvec->rows, tvec->cols, 13196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_MAKETYPE(CV_64F,CV_MAT_CN(tvec->type)), param + 3 ); 13206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( &_r, rvec ); 13226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( &_t, tvec ); 13236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 13256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &_M ); 13276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &_Mxy ); 13286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &_m ); 13296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &_mn ); 13306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &_L ); 13316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &_J ); 13326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 13336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_IMPL void 13366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RenncvInitIntrinsicParams2D( const CvMat* objectPoints, 13376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const CvMat* imagePoints, 13386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const CvMat* npoints, 13396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvSize imageSize, 13406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* cameraMatrix, 13416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double aspectRatio ) 13426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 13436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *_A = 0, *_b = 0, *_allH = 0, *_allK = 0; 13446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "cvInitIntrinsicParams2D" ); 13466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 13486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i, j, pos, nimages, total, ni = 0; 13506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double a[9] = { 0, 0, 0, 0, 0, 0, 0, 0, 1 }; 13516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double H[9], f[2]; 13526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _a = cvMat( 3, 3, CV_64F, a ); 13536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _H = cvMat( 3, 3, CV_64F, H ); 13546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _f = cvMat( 2, 1, CV_64F, f ); 13556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn assert( CV_MAT_TYPE(npoints->type) == CV_32SC1 && 13576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_IS_MAT_CONT(npoints->type) ); 13586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn nimages = npoints->rows + npoints->cols - 1; 13596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( (CV_MAT_TYPE(objectPoints->type) != CV_32FC3 && 13616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_MAT_TYPE(objectPoints->type) != CV_64FC3) || 13626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (CV_MAT_TYPE(imagePoints->type) != CV_32FC2 && 13636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_MAT_TYPE(imagePoints->type) != CV_64FC2) ) 13646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "Both object points and image points must be 2D" ); 13656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( objectPoints->rows != 1 || imagePoints->rows != 1 ) 13676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadSize, "object points and image points must be a single-row matrices" ); 13686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _A = cvCreateMat( 2*nimages, 2, CV_64F ); 13706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _b = cvCreateMat( 2*nimages, 1, CV_64F ); 13716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a[2] = (imageSize.width - 1)*0.5; 13726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a[5] = (imageSize.height - 1)*0.5; 13736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _allH = cvCreateMat( nimages, 9, CV_64F ); 13746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn total = cvRound(cvSum(npoints).val[0]); 13766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // extract vanishing points in order to obtain initial value for the focal length 13786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0, pos = 0; i < nimages; i++, pos += ni ) 13796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 13806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double* Ap = _A->data.db + i*4; 13816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double* bp = _b->data.db + i*2; 13826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ni = npoints->data.i[i]; 13836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double h[3], v[3], d1[3], d2[3]; 13846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double n[4] = {0,0,0,0}; 13856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _m, _M; 13866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetCols( objectPoints, &_M, pos, pos + ni ); 13876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetCols( imagePoints, &_m, pos, pos + ni ); 13886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFindHomography( &_M, &_m, &_H ); 13906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn memcpy( _allH->data.db + i*9, H, sizeof(H) ); 13916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn H[0] -= H[6]*a[2]; H[1] -= H[7]*a[2]; H[2] -= H[8]*a[2]; 13936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn H[3] -= H[6]*a[5]; H[4] -= H[7]*a[5]; H[5] -= H[8]*a[5]; 13946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 13956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < 3; j++ ) 13966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 13976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double t0 = H[j*3], t1 = H[j*3+1]; 13986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn h[j] = t0; v[j] = t1; 13996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn d1[j] = (t0 + t1)*0.5; 14006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn d2[j] = (t0 - t1)*0.5; 14016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn n[0] += t0*t0; n[1] += t1*t1; 14026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn n[2] += d1[j]*d1[j]; n[3] += d2[j]*d2[j]; 14036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 14046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < 4; j++ ) 14066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn n[j] = 1./sqrt(n[j]); 14076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < 3; j++ ) 14096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 14106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn h[j] *= n[0]; v[j] *= n[1]; 14116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn d1[j] *= n[2]; d2[j] *= n[3]; 14126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 14136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Ap[0] = h[0]*v[0]; Ap[1] = h[1]*v[1]; 14156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Ap[2] = d1[0]*d2[0]; Ap[3] = d1[1]*d2[1]; 14166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn bp[0] = -h[2]*v[2]; bp[1] = -d1[2]*d2[2]; 14176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 14186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSolve( _A, _b, &_f, CV_LSQ | CV_SVD ); 14206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a[0] = sqrt(fabs(1./f[0])); 14216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a[4] = sqrt(fabs(1./f[1])); 14226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( aspectRatio != 0 ) 14236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 14246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double tf = (a[0] + a[4])/(aspectRatio + 1.); 14256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a[0] = aspectRatio*tf; 14266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn a[4] = tf; 14276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 14286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( &_a, cameraMatrix ); 14306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 14326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &_A ); 14346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &_b ); 14356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &_allH ); 14366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &_allK ); 14376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 14386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* finds intrinsic and extrinsic camera parameters 14416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn from a few views of known calibration pattern */ 14426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_IMPL void 14436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RenncvCalibrateCamera2( const CvMat* objectPoints, 14446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const CvMat* imagePoints, 14456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const CvMat* npoints, 14466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvSize imageSize, 14476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* cameraMatrix, CvMat* distCoeffs, 14486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* rvecs, CvMat* tvecs, 14496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int flags ) 14506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 14516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const int NINTRINSIC = 9; 14526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *_M = 0, *_m = 0, *_Ji = 0, *_Je = 0, *_err = 0; 14536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvLevMarq solver; 14546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "cvCalibrateCamera2" ); 14566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 14586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double A[9], k[5] = {0,0,0,0,0}; 14606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _A = cvMat(3, 3, CV_64F, A), _k; 14616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i, nimages, maxPoints = 0, ni = 0, pos, total = 0, nparams, npstep, cn; 14626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double aspectRatio = 0.; 14636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // 0. check the parameters & allocate buffers 14656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(objectPoints) || !CV_IS_MAT(imagePoints) || 14666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn !CV_IS_MAT(npoints) || !CV_IS_MAT(cameraMatrix) || !CV_IS_MAT(distCoeffs) ) 14676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadArg, "One of required vector arguments is not a valid matrix" ); 14686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( imageSize.width <= 0 || imageSize.height <= 0 ) 14706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "image width and height must be positive" ); 14716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( CV_MAT_TYPE(npoints->type) != CV_32SC1 || 14736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (npoints->rows != 1 && npoints->cols != 1) ) 14746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, 14756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn "the array of point counters must be 1-dimensional integer vector" ); 14766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn nimages = npoints->rows*npoints->cols; 14786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn npstep = npoints->rows == 1 ? 1 : npoints->step/CV_ELEM_SIZE(npoints->type); 14796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( rvecs ) 14816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 14826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cn = CV_MAT_CN(rvecs->type); 14836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(rvecs) || 14846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (CV_MAT_DEPTH(rvecs->type) != CV_32F && CV_MAT_DEPTH(rvecs->type) != CV_64F) || 14856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ((rvecs->rows != nimages || (rvecs->cols*cn != 3 && rvecs->cols*cn != 9)) && 14866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (rvecs->rows != 1 || rvecs->cols != nimages || cn != 3)) ) 14876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadArg, "the output array of rotation vectors must be 3-channel " 14886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn "1xn or nx1 array or 1-channel nx3 or nx9 array, where n is the number of views" ); 14896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 14906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 14916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( tvecs ) 14926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 14936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cn = CV_MAT_CN(tvecs->type); 14946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(tvecs) || 14956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (CV_MAT_DEPTH(tvecs->type) != CV_32F && CV_MAT_DEPTH(tvecs->type) != CV_64F) || 14966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ((tvecs->rows != nimages || tvecs->cols*cn != 3) && 14976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (tvecs->rows != 1 || tvecs->cols != nimages || cn != 3)) ) 14986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadArg, "the output array of translation vectors must be 3-channel " 14996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn "1xn or nx1 array or 1-channel nx3 array, where n is the number of views" ); 15006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 15016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( (CV_MAT_TYPE(cameraMatrix->type) != CV_32FC1 && 15036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_MAT_TYPE(cameraMatrix->type) != CV_64FC1) || 15046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cameraMatrix->rows != 3 || cameraMatrix->cols != 3 ) 15056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadArg, 15066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn "Intrinsic parameters must be 3x3 floating-point matrix" ); 15076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( (CV_MAT_TYPE(distCoeffs->type) != CV_32FC1 && 15096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_MAT_TYPE(distCoeffs->type) != CV_64FC1) || 15106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (distCoeffs->cols != 1 && distCoeffs->rows != 1) || 15116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (distCoeffs->cols*distCoeffs->rows != 4 && 15126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn distCoeffs->cols*distCoeffs->rows != 5) ) 15136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadArg, 15146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn "Distortion coefficients must be 4x1, 1x4, 5x1 or 1x5 floating-point matrix" ); 15156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < nimages; i++ ) 15176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 15186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ni = npoints->data.i[i*npstep]; 15196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( ni < 4 ) 15206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 15216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn char buf[100]; 15226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sprintf( buf, "The number of points in the view #%d is < 4", i ); 15236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, buf ); 15246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 15256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn maxPoints = MAX( maxPoints, ni ); 15266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn total += ni; 15276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 15286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( _M = cvCreateMat( 1, total, CV_64FC3 )); 15306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( _m = cvCreateMat( 1, total, CV_64FC2 )); 15316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( cvConvertPointsHomogeneous( objectPoints, _M )); 15336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( cvConvertPointsHomogeneous( imagePoints, _m )); 15346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn nparams = NINTRINSIC + nimages*6; 15366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( _Ji = cvCreateMat( maxPoints*2, NINTRINSIC, CV_64FC1 )); 15376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( _Je = cvCreateMat( maxPoints*2, 6, CV_64FC1 )); 15386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( _err = cvCreateMat( maxPoints*2, 1, CV_64FC1 )); 15396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvZero( _Ji ); 15406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _k = cvMat( distCoeffs->rows, distCoeffs->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(distCoeffs->type)), k); 15426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) == 4 ) 15436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn flags |= CV_CALIB_FIX_K3; 15446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // 1. initialize intrinsic parameters & LM solver 15466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( flags & CV_CALIB_USE_INTRINSIC_GUESS ) 15476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 15486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( cameraMatrix, &_A ); 15496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( A[0] <= 0 || A[4] <= 0 ) 15506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "Focal length (fx and fy) must be positive" ); 15516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( A[2] < 0 || A[2] >= imageSize.width || 15526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn A[5] < 0 || A[5] >= imageSize.height ) 15536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "Principal point must be within the image" ); 15546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( fabs(A[1]) > 1e-5 ) 15556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "Non-zero skew is not supported by the function" ); 15566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( fabs(A[3]) > 1e-5 || fabs(A[6]) > 1e-5 || 15576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fabs(A[7]) > 1e-5 || fabs(A[8]-1) > 1e-5 ) 15586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, 15596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn "The intrinsic matrix must have [fx 0 cx; 0 fy cy; 0 0 1] shape" ); 15606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn A[1] = A[3] = A[6] = A[7] = 0.; 15616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn A[8] = 1.; 15626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( flags & CV_CALIB_FIX_ASPECT_RATIO ) 15646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn aspectRatio = A[0]/A[4]; 15656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( distCoeffs, &_k ); 15666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 15676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 15686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 15696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvScalar mean, sdv; 15706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvAvgSdv( _M, &mean, &sdv ); 15716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( (fabs(mean.val[2]) > 1e-5 && fabs(mean.val[2] - 1) > 1e-5) || fabs(sdv.val[2]) > 1e-5 ) 15726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadArg, 15736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn "For non-planar calibration rigs the initial intrinsic matrix must be specified" ); 15746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < total; i++ ) 15756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ((CvPoint3D64f*)_M->data.db)[i].z = 0.; 15766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( flags & CV_CALIB_FIX_ASPECT_RATIO ) 15786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 15796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn aspectRatio = cvmGet(cameraMatrix,0,0); 15806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn aspectRatio /= cvmGet(cameraMatrix,1,1); 15816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( aspectRatio < 0.01 || aspectRatio > 100 ) 15826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, 15836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn "The specified aspect ratio (=A[0][0]/A[1][1]) is incorrect" ); 15846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 15856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvInitIntrinsicParams2D( _M, _m, npoints, imageSize, &_A, aspectRatio ); 15866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 15876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn solver.init( nparams, 0, cvTermCriteria(CV_TERMCRIT_ITER+CV_TERMCRIT_EPS,30,DBL_EPSILON) ); 15896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 15916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double* param = solver.param->data.db; 15926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar* mask = solver.mask->data.ptr; 15936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn param[0] = A[0]; param[1] = A[4]; param[2] = A[2]; param[3] = A[5]; 15956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn param[4] = k[0]; param[5] = k[1]; param[6] = k[2]; param[7] = k[3]; 15966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn param[8] = k[4]; 15976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 15986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( flags & CV_CALIB_FIX_FOCAL_LENGTH ) 15996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn mask[0] = mask[1] = 0; 16006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( flags & CV_CALIB_FIX_PRINCIPAL_POINT ) 16016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn mask[2] = mask[3] = 0; 16026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( flags & CV_CALIB_ZERO_TANGENT_DIST ) 16036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 16046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn param[6] = param[7] = 0; 16056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn mask[6] = mask[7] = 0; 16066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 16076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( flags & CV_CALIB_FIX_K1 ) 16086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn mask[4] = 0; 16096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( flags & CV_CALIB_FIX_K2 ) 16106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn mask[5] = 0; 16116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( flags & CV_CALIB_FIX_K3 ) 16126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn mask[8] = 0; 16136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 16146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // 2. initialize extrinsic parameters 16166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0, pos = 0; i < nimages; i++, pos += ni ) 16176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 16186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _Mi, _mi, _ri, _ti; 16196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ni = npoints->data.i[i*npstep]; 16206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetRows( solver.param, &_ri, NINTRINSIC + i*6, NINTRINSIC + i*6 + 3 ); 16226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetRows( solver.param, &_ti, NINTRINSIC + i*6 + 3, NINTRINSIC + i*6 + 6 ); 16236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetCols( _M, &_Mi, pos, pos + ni ); 16256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetCols( _m, &_mi, pos, pos + ni ); 16266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFindExtrinsicCameraParams2( &_Mi, &_mi, &_A, &_k, &_ri, &_ti ); 16286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 16296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // 3. run the optimization 16316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for(;;) 16326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 16336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const CvMat* _param = 0; 16346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *_JtJ = 0, *_JtErr = 0; 16356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double* _errNorm = 0; 16366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn bool proceed = solver.updateAlt( _param, _JtJ, _JtErr, _errNorm ); 16376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double *param = solver.param->data.db, *pparam = solver.prevParam->data.db; 16386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( flags & CV_CALIB_FIX_ASPECT_RATIO ) 16406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 16416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn param[0] = param[1]*aspectRatio; 16426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pparam[0] = pparam[1]*aspectRatio; 16436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 16446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn A[0] = param[0]; A[4] = param[1]; 16466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn A[2] = param[2]; A[5] = param[3]; 16476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn k[0] = param[4]; k[1] = param[5]; k[2] = param[6]; 16486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn k[3] = param[7]; 16496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn k[4] = param[8]; 16506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !proceed ) 16526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 16536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0, pos = 0; i < nimages; i++, pos += ni ) 16556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 16566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat _Mi, _mi, _ri, _ti, _dpdr, _dpdt, _dpdf, _dpdc, _dpdk, _mp, _part; 16576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ni = npoints->data.i[i*npstep]; 16586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetRows( solver.param, &_ri, NINTRINSIC + i*6, NINTRINSIC + i*6 + 3 ); 16606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetRows( solver.param, &_ti, NINTRINSIC + i*6 + 3, NINTRINSIC + i*6 + 6 ); 16616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetCols( _M, &_Mi, pos, pos + ni ); 16636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetCols( _m, &_mi, pos, pos + ni ); 16646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _Je->rows = _Ji->rows = _err->rows = ni*2; 16666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetCols( _Je, &_dpdr, 0, 3 ); 16676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetCols( _Je, &_dpdt, 3, 6 ); 16686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetCols( _Ji, &_dpdf, 0, 2 ); 16696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetCols( _Ji, &_dpdc, 2, 4 ); 16706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetCols( _Ji, &_dpdk, 4, NINTRINSIC ); 16716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReshape( _err, &_mp, 2, 1 ); 16726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( _JtJ || _JtErr ) 16746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 16756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvProjectPoints2( &_Mi, &_ri, &_ti, &_A, &_k, &_mp, &_dpdr, &_dpdt, 16766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (flags & CV_CALIB_FIX_FOCAL_LENGTH) ? 0 : &_dpdf, 16776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (flags & CV_CALIB_FIX_PRINCIPAL_POINT) ? 0 : &_dpdc, &_dpdk, 16786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (flags & CV_CALIB_FIX_ASPECT_RATIO) ? aspectRatio : 0); 16796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 16806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 16816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvProjectPoints2( &_Mi, &_ri, &_ti, &_A, &_k, &_mp ); 16826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSub( &_mp, &_mi, &_mp ); 16846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( _JtJ || _JtErr ) 16866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 16876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetSubRect( _JtJ, &_part, cvRect(0,0,NINTRINSIC,NINTRINSIC) ); 16886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGEMM( _Ji, _Ji, 1, &_part, 1, &_part, CV_GEMM_A_T ); 16896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetSubRect( _JtJ, &_part, cvRect(NINTRINSIC+i*6,NINTRINSIC+i*6,6,6) ); 16916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGEMM( _Je, _Je, 1, 0, 0, &_part, CV_GEMM_A_T ); 16926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetSubRect( _JtJ, &_part, cvRect(NINTRINSIC+i*6,0,6,NINTRINSIC) ); 16946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGEMM( _Ji, _Je, 1, 0, 0, &_part, CV_GEMM_A_T ); 16956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetRows( _JtErr, &_part, 0, NINTRINSIC ); 16976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGEMM( _Ji, _err, 1, &_part, 1, &_part, CV_GEMM_A_T ); 16986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 16996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetRows( _JtErr, &_part, NINTRINSIC + i*6, NINTRINSIC + (i+1)*6 ); 17006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGEMM( _Je, _err, 1, 0, 0, &_part, CV_GEMM_A_T ); 17016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 17026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( _errNorm ) 17046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 17056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double errNorm = cvNorm( &_mp, 0, CV_L2 ); 17066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *_errNorm += errNorm*errNorm; 17076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 17086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 17096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 17106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // 4. store the results 17126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( &_A, cameraMatrix ); 17136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( &_k, distCoeffs ); 17146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < nimages; i++ ) 17166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 17176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat src, dst; 17186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( rvecs ) 17196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 17206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn src = cvMat( 3, 1, CV_64F, solver.param->data.db + NINTRINSIC + i*6 ); 17216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( rvecs->rows == nimages && rvecs->cols*CV_MAT_CN(rvecs->type) == 9 ) 17226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 17236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dst = cvMat( 3, 3, CV_MAT_DEPTH(rvecs->type), 17246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rvecs->data.ptr + rvecs->step*i ); 17256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvRodrigues2( &src, &_A ); 17266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( &_A, &dst ); 17276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 17286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 17296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 17306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dst = cvMat( 3, 1, CV_MAT_DEPTH(rvecs->type), rvecs->rows == 1 ? 17316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rvecs->data.ptr + i*CV_ELEM_SIZE(rvecs->type) : 17326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn rvecs->data.ptr + rvecs->step*i ); 17336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( &src, &dst ); 17346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 17356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 17366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( tvecs ) 17376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 17386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn src = cvMat( 3, 1, CV_64F, solver.param->data.db + NINTRINSIC + i*6 + 3 ); 17396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dst = cvMat( 3, 1, CV_MAT_TYPE(tvecs->type), tvecs->rows == 1 ? 17406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tvecs->data.ptr + i*CV_ELEM_SIZE(tvecs->type) : 17416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tvecs->data.ptr + tvecs->step*i ); 17426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( &src, &dst ); 17436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 17446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 17456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 17476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &_M ); 17496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &_m ); 17506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &_Ji ); 17516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &_Je ); 17526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &_err ); 17536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 17546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid cvCalibrationMatrixValues( const CvMat *calibMatr, CvSize imgSize, 17576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double apertureWidth, double apertureHeight, double *fovx, double *fovy, 17586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double *focalLength, CvPoint2D64f *principalPoint, double *pasp ) 17596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 17606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double alphax, alphay, mx, my; 17616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int imgWidth = imgSize.width, imgHeight = imgSize.height; 17626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME("cvCalibrationMatrixValues"); 17646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 17656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Validate parameters. */ 17676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if(calibMatr == 0) 17696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR(CV_StsNullPtr, "Some of parameters is a NULL pointer!"); 17706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if(!CV_IS_MAT(calibMatr)) 17726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR(CV_StsUnsupportedFormat, "Input parameters must be a matrices!"); 17736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if(calibMatr->cols != 3 || calibMatr->rows != 3) 17756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR(CV_StsUnmatchedSizes, "Size of matrices must be 3x3!"); 17766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn alphax = cvmGet(calibMatr, 0, 0); 17786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn alphay = cvmGet(calibMatr, 1, 1); 17796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn assert(imgWidth != 0 && imgHeight != 0 && alphax != 0.0 && alphay != 0.0); 17806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Calculate pixel aspect ratio. */ 17826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if(pasp) 17836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *pasp = alphay / alphax; 17846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Calculate number of pixel per realworld unit. */ 17866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if(apertureWidth != 0.0 && apertureHeight != 0.0) { 17886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn mx = imgWidth / apertureWidth; 17896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn my = imgHeight / apertureHeight; 17906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } else { 17916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn mx = 1.0; 17926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn my = *pasp; 17936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 17946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Calculate fovx and fovy. */ 17966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 17976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if(fovx) 17986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *fovx = 2 * atan(imgWidth / (2 * alphax)) * 180.0 / CV_PI; 17996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if(fovy) 18016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *fovy = 2 * atan(imgHeight / (2 * alphay)) * 180.0 / CV_PI; 18026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Calculate focal length. */ 18046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if(focalLength) 18066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *focalLength = alphax / mx; 18076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Calculate principle point. */ 18096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if(principalPoint) 18116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *principalPoint = cvPoint2D64f(cvmGet(calibMatr, 0, 2) / mx, cvmGet(calibMatr, 1, 2) / my); 18126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 18146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 18156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn//////////////////////////////// Stereo Calibration /////////////////////////////////// 18186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennstatic int dbCmp( const void* _a, const void* _b ) 18206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 18216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double a = *(const double*)_a; 18226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double b = *(const double*)_b; 18236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return (a > b) - (a < b); 18256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 18266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid cvStereoCalibrate( const CvMat* _objectPoints, const CvMat* _imagePoints1, 18296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const CvMat* _imagePoints2, const CvMat* _npoints, 18306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* _cameraMatrix1, CvMat* _distCoeffs1, 18316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* _cameraMatrix2, CvMat* _distCoeffs2, 18326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvSize imageSize, CvMat* _R, CvMat* _T, 18336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* _E, CvMat* _F, 18346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvTermCriteria termCrit, int flags ) 18356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 18366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const int NINTRINSIC = 9; 18376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* npoints = 0; 18386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* err = 0; 18396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* J_LR = 0; 18406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* Je = 0; 18416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* Ji = 0; 18426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* imagePoints[2] = {0,0}; 18436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* objectPoints = 0; 18446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* RT0 = 0; 18456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvLevMarq solver; 18466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "cvStereoCalibrate" ); 18486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 18506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double A[2][9], dk[2][5]={{0,0,0,0,0},{0,0,0,0,0}}, rlr[9]; 18526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat K[2], Dist[2], om_LR, T_LR; 18536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat R_LR = cvMat(3, 3, CV_64F, rlr); 18546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i, k, p, ni = 0, ofs, nimages, pointsTotal, maxPoints = 0; 18556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int nparams; 18566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn bool recomputeIntrinsics = false; 18576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double aspectRatio[2] = {0,0}; 18586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ASSERT( CV_IS_MAT(_imagePoints1) && CV_IS_MAT(_imagePoints2) && 18606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_IS_MAT(_objectPoints) && CV_IS_MAT(_npoints) && 18616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_IS_MAT(_R) && CV_IS_MAT(_T) ); 18626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ASSERT( CV_ARE_TYPES_EQ(_imagePoints1, _imagePoints2) && 18646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ARE_DEPTHS_EQ(_imagePoints1, _objectPoints) ); 18656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ASSERT( (_npoints->cols == 1 || _npoints->rows == 1) && 18676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_MAT_TYPE(_npoints->type) == CV_32SC1 ); 18686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn nimages = _npoints->cols + _npoints->rows - 1; 18706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn npoints = cvCreateMat( _npoints->rows, _npoints->cols, _npoints->type ); 18716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvCopy( _npoints, npoints ); 18726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0, pointsTotal = 0; i < nimages; i++ ) 18746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 18756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn maxPoints = MAX(maxPoints, npoints->data.i[i]); 18766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pointsTotal += npoints->data.i[i]; 18776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 18786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn objectPoints = cvCreateMat( _objectPoints->rows, _objectPoints->cols, 18806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_64FC(CV_MAT_CN(_objectPoints->type))); 18816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( _objectPoints, objectPoints ); 18826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReshape( objectPoints, objectPoints, 3, 1 ); 18836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( k = 0; k < 2; k++ ) 18856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 18866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const CvMat* points = k == 0 ? _imagePoints1 : _imagePoints2; 18876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const CvMat* cameraMatrix = k == 0 ? _cameraMatrix1 : _cameraMatrix2; 18886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const CvMat* distCoeffs = k == 0 ? _distCoeffs1 : _distCoeffs2; 18896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int cn = CV_MAT_CN(_imagePoints1->type); 18916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ASSERT( (CV_MAT_DEPTH(_imagePoints1->type) == CV_32F || 18926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_MAT_DEPTH(_imagePoints1->type) == CV_64F) && 18936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ((_imagePoints1->rows == pointsTotal && _imagePoints1->cols*cn == 2) || 18946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (_imagePoints1->rows == 1 && _imagePoints1->cols == pointsTotal && cn == 2)) ); 18956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn K[k] = cvMat(3,3,CV_64F,A[k]); 18976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Dist[k] = cvMat(1,5,CV_64F,dk[k]); 18986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 18996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn imagePoints[k] = cvCreateMat( points->rows, points->cols, CV_64FC(CV_MAT_CN(points->type))); 19006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( points, imagePoints[k] ); 19016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReshape( imagePoints[k], imagePoints[k], 2, 1 ); 19026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( flags & (CV_CALIB_FIX_INTRINSIC|CV_CALIB_USE_INTRINSIC_GUESS| 19046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALIB_FIX_ASPECT_RATIO|CV_CALIB_FIX_FOCAL_LENGTH) ) 19056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( cameraMatrix, &K[k] ); 19066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( flags & (CV_CALIB_FIX_INTRINSIC|CV_CALIB_USE_INTRINSIC_GUESS| 19086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALIB_FIX_K1|CV_CALIB_FIX_K2|CV_CALIB_FIX_K3) ) 19096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 19106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat tdist = cvMat( distCoeffs->rows, distCoeffs->cols, 19116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_MAKETYPE(CV_64F,CV_MAT_CN(distCoeffs->type)), Dist[k].data.db ); 19126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( distCoeffs, &tdist ); 19136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 19146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !(flags & (CV_CALIB_FIX_INTRINSIC|CV_CALIB_USE_INTRINSIC_GUESS))) 19166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 19176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvCalibrateCamera2( objectPoints, imagePoints[k], 19186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn npoints, imageSize, &K[k], &Dist[k], 0, 0, flags ); 19196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 19206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 19216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( flags & CV_CALIB_SAME_FOCAL_LENGTH ) 19236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 19246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn static const int avg_idx[] = { 0, 4, 2, 5, -1 }; 19256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( k = 0; avg_idx[k] >= 0; k++ ) 19266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn A[0][avg_idx[k]] = A[1][avg_idx[k]] = (A[0][avg_idx[k]] + A[1][avg_idx[k]])*0.5; 19276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 19286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( flags & CV_CALIB_FIX_ASPECT_RATIO ) 19306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 19316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( k = 0; k < 2; k++ ) 19326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn aspectRatio[k] = A[k][0]/A[k][4]; 19336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 19346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn recomputeIntrinsics = (flags & CV_CALIB_FIX_INTRINSIC) == 0; 19366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn err = cvCreateMat( maxPoints*2, 1, CV_64F ); 19386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Je = cvCreateMat( maxPoints*2, 6, CV_64F ); 19396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn J_LR = cvCreateMat( maxPoints*2, 6, CV_64F ); 19406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Ji = cvCreateMat( maxPoints*2, NINTRINSIC, CV_64F ); 19416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvZero( Ji ); 19426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // we optimize for the inter-camera R(3),t(3), then, optionally, 19446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // for intrinisic parameters of each camera ((fx,fy,cx,cy,k1,k2,p1,p2) ~ 8 parameters). 19456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn nparams = 6*(nimages+1) + (recomputeIntrinsics ? NINTRINSIC*2 : 0); 19466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // storage for initial [om(R){i}|t{i}] (in order to compute the median for each component) 19486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn RT0 = cvCreateMat( 6, nimages, CV_64F ); 19496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn solver.init( nparams, 0, termCrit ); 19516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( recomputeIntrinsics ) 19526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 19536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn uchar* imask = solver.mask->data.ptr + nparams - NINTRINSIC*2; 19546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( flags & CV_CALIB_FIX_ASPECT_RATIO ) 19556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn imask[0] = imask[NINTRINSIC] = 0; 19566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( flags & CV_CALIB_FIX_FOCAL_LENGTH ) 19576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn imask[0] = imask[1] = imask[NINTRINSIC] = imask[NINTRINSIC+1] = 0; 19586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( flags & CV_CALIB_FIX_PRINCIPAL_POINT ) 19596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn imask[2] = imask[3] = imask[NINTRINSIC+2] = imask[NINTRINSIC+3] = 0; 19606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( flags & CV_CALIB_ZERO_TANGENT_DIST ) 19616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn imask[6] = imask[7] = imask[NINTRINSIC+6] = imask[NINTRINSIC+7] = 0; 19626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( flags & CV_CALIB_FIX_K1 ) 19636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn imask[4] = imask[NINTRINSIC+4] = 0; 19646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( flags & CV_CALIB_FIX_K2 ) 19656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn imask[5] = imask[NINTRINSIC+5] = 0; 19666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( flags & CV_CALIB_FIX_K3 ) 19676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn imask[8] = imask[NINTRINSIC+8] = 0; 19686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 19696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* 19716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Compute initial estimate of pose 19726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn For each image, compute: 19746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn R(om) is the rotation matrix of om 19756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn om(R) is the rotation vector of R 19766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn R_ref = R(om_right) * R(om_left)' 19776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn T_ref_list = [T_ref_list; T_right - R_ref * T_left] 19786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn om_ref_list = {om_ref_list; om(R_ref)] 19796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn om = median(om_ref_list) 19816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn T = median(T_ref_list) 19826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn */ 19836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = ofs = 0; i < nimages; ofs += ni, i++ ) 19846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 19856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ni = npoints->data.i[i]; 19866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat objpt_i; 19876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double _om[2][3], r[2][9], t[2][3]; 19886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat om[2], R[2], T[2], imgpt_i[2]; 19896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn objpt_i = cvMat(1, ni, CV_64FC3, objectPoints->data.db + ofs*3); 19916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( k = 0; k < 2; k++ ) 19926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 19936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn imgpt_i[k] = cvMat(1, ni, CV_64FC2, imagePoints[k]->data.db + ofs*2); 19946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn om[k] = cvMat(3, 1, CV_64F, _om[k]); 19956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn R[k] = cvMat(3, 3, CV_64F, r[k]); 19966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn T[k] = cvMat(3, 1, CV_64F, t[k]); 19976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 19986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // FIXME: here we ignore activePoints[k] because of 19996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // the limited API of cvFindExtrnisicCameraParams2 20006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFindExtrinsicCameraParams2( &objpt_i, &imgpt_i[k], &K[k], &Dist[k], &om[k], &T[k] ); 20016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvRodrigues2( &om[k], &R[k] ); 20026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( k == 0 ) 20036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 20046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // save initial om_left and T_left 20056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn solver.param->data.db[(i+1)*6] = _om[0][0]; 20066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn solver.param->data.db[(i+1)*6 + 1] = _om[0][1]; 20076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn solver.param->data.db[(i+1)*6 + 2] = _om[0][2]; 20086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn solver.param->data.db[(i+1)*6 + 3] = t[0][0]; 20096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn solver.param->data.db[(i+1)*6 + 4] = t[0][1]; 20106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn solver.param->data.db[(i+1)*6 + 5] = t[0][2]; 20116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 20126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 20136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGEMM( &R[1], &R[0], 1, 0, 0, &R[0], CV_GEMM_B_T ); 20146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGEMM( &R[0], &T[0], -1, &T[1], 1, &T[1] ); 20156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvRodrigues2( &R[0], &T[0] ); 20166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn RT0->data.db[i] = t[0][0]; 20176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn RT0->data.db[i + nimages] = t[0][1]; 20186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn RT0->data.db[i + nimages*2] = t[0][2]; 20196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn RT0->data.db[i + nimages*3] = t[1][0]; 20206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn RT0->data.db[i + nimages*4] = t[1][1]; 20216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn RT0->data.db[i + nimages*5] = t[1][2]; 20226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 20236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 20246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // find the medians and save the first 6 parameters 20256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < 6; i++ ) 20266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 20276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn qsort( RT0->data.db + i*nimages, nimages, CV_ELEM_SIZE(RT0->type), dbCmp ); 20286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn solver.param->data.db[i] = nimages % 2 != 0 ? RT0->data.db[i*nimages + nimages/2] : 20296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (RT0->data.db[i*nimages + nimages/2 - 1] + RT0->data.db[i*nimages + nimages/2])*0.5; 20306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 20316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 20326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( recomputeIntrinsics ) 20336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( k = 0; k < 2; k++ ) 20346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 20356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double* iparam = solver.param->data.db + (nimages+1)*6 + k*NINTRINSIC; 20366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( flags & CV_CALIB_ZERO_TANGENT_DIST ) 20376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dk[k][2] = dk[k][3] = 0; 20386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn iparam[0] = A[k][0]; iparam[1] = A[k][4]; iparam[2] = A[k][2]; iparam[3] = A[k][5]; 20396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn iparam[4] = dk[k][0]; iparam[5] = dk[k][1]; iparam[6] = dk[k][2]; 20406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn iparam[7] = dk[k][3]; iparam[8] = dk[k][4]; 20416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 20426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 20436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn om_LR = cvMat(3, 1, CV_64F, solver.param->data.db); 20446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn T_LR = cvMat(3, 1, CV_64F, solver.param->data.db + 3); 20456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 20466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for(;;) 20476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 20486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const CvMat* param = 0; 20496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat tmpimagePoints; 20506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *JtJ = 0, *JtErr = 0; 20516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double* errNorm = 0; 20526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double _omR[3], _tR[3]; 20536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double _dr3dr1[9], _dr3dr2[9], /*_dt3dr1[9],*/ _dt3dr2[9], _dt3dt1[9], _dt3dt2[9]; 20546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat dr3dr1 = cvMat(3, 3, CV_64F, _dr3dr1); 20556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat dr3dr2 = cvMat(3, 3, CV_64F, _dr3dr2); 20566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn //CvMat dt3dr1 = cvMat(3, 3, CV_64F, _dt3dr1); 20576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat dt3dr2 = cvMat(3, 3, CV_64F, _dt3dr2); 20586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat dt3dt1 = cvMat(3, 3, CV_64F, _dt3dt1); 20596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat dt3dt2 = cvMat(3, 3, CV_64F, _dt3dt2); 20606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat om[2], T[2], imgpt_i[2]; 20616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat dpdrot_hdr, dpdt_hdr, dpdf_hdr, dpdc_hdr, dpdk_hdr; 20626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *dpdrot = &dpdrot_hdr, *dpdt = &dpdt_hdr, *dpdf = 0, *dpdc = 0, *dpdk = 0; 20636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 20646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !solver.updateAlt( param, JtJ, JtErr, errNorm )) 20656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 20666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 20676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvRodrigues2( &om_LR, &R_LR ); 20686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn om[1] = cvMat(3,1,CV_64F,_omR); 20696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn T[1] = cvMat(3,1,CV_64F,_tR); 20706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 20716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( recomputeIntrinsics ) 20726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 20736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double* iparam = solver.param->data.db + (nimages+1)*6; 20746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double* ipparam = solver.prevParam->data.db + (nimages+1)*6; 20756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dpdf = &dpdf_hdr; 20766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dpdc = &dpdc_hdr; 20776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dpdk = &dpdk_hdr; 20786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( flags & CV_CALIB_SAME_FOCAL_LENGTH ) 20796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 20806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn iparam[NINTRINSIC] = iparam[0]; 20816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn iparam[NINTRINSIC+1] = iparam[1]; 20826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ipparam[NINTRINSIC] = ipparam[0]; 20836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ipparam[NINTRINSIC+1] = ipparam[1]; 20846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 20856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( flags & CV_CALIB_FIX_ASPECT_RATIO ) 20866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 20876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn iparam[0] = iparam[1]*aspectRatio[0]; 20886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn iparam[NINTRINSIC] = iparam[NINTRINSIC+1]*aspectRatio[1]; 20896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ipparam[0] = ipparam[1]*aspectRatio[0]; 20906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ipparam[NINTRINSIC] = ipparam[NINTRINSIC+1]*aspectRatio[1]; 20916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 20926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( k = 0; k < 2; k++ ) 20936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 20946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn A[k][0] = iparam[k*NINTRINSIC+0]; 20956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn A[k][4] = iparam[k*NINTRINSIC+1]; 20966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn A[k][2] = iparam[k*NINTRINSIC+2]; 20976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn A[k][5] = iparam[k*NINTRINSIC+3]; 20986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dk[k][0] = iparam[k*NINTRINSIC+4]; 20996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dk[k][1] = iparam[k*NINTRINSIC+5]; 21006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dk[k][2] = iparam[k*NINTRINSIC+6]; 21016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dk[k][3] = iparam[k*NINTRINSIC+7]; 21026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dk[k][4] = iparam[k*NINTRINSIC+8]; 21036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 21046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 21056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 21066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = ofs = 0; i < nimages; ofs += ni, i++ ) 21076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 21086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ni = npoints->data.i[i]; 21096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat objpt_i, _part; 21106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 21116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn om[0] = cvMat(3,1,CV_64F,solver.param->data.db+(i+1)*6); 21126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn T[0] = cvMat(3,1,CV_64F,solver.param->data.db+(i+1)*6+3); 21136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 21146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( JtJ || JtErr ) 21156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvComposeRT( &om[0], &T[0], &om_LR, &T_LR, &om[1], &T[1], &dr3dr1, 0, 21166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn &dr3dr2, 0, 0, &dt3dt1, &dt3dr2, &dt3dt2 ); 21176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 21186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvComposeRT( &om[0], &T[0], &om_LR, &T_LR, &om[1], &T[1] ); 21196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 21206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn objpt_i = cvMat(1, ni, CV_64FC3, objectPoints->data.db + ofs*3); 21216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn err->rows = Je->rows = J_LR->rows = Ji->rows = ni*2; 21226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReshape( err, &tmpimagePoints, 2, 1 ); 21236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 21246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetCols( Ji, &dpdf_hdr, 0, 2 ); 21256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetCols( Ji, &dpdc_hdr, 2, 4 ); 21266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetCols( Ji, &dpdk_hdr, 4, NINTRINSIC ); 21276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetCols( Je, &dpdrot_hdr, 0, 3 ); 21286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetCols( Je, &dpdt_hdr, 3, 6 ); 21296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 21306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( k = 0; k < 2; k++ ) 21316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 21326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double maxErr, l2err; 21336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn imgpt_i[k] = cvMat(1, ni, CV_64FC2, imagePoints[k]->data.db + ofs*2); 21346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 21356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( JtJ || JtErr ) 21366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvProjectPoints2( &objpt_i, &om[k], &T[k], &K[k], &Dist[k], 21376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn &tmpimagePoints, dpdrot, dpdt, dpdf, dpdc, dpdk, 21386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (flags & CV_CALIB_FIX_ASPECT_RATIO) ? aspectRatio[k] : 0); 21396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 21406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvProjectPoints2( &objpt_i, &om[k], &T[k], &K[k], &Dist[k], &tmpimagePoints ); 21416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSub( &tmpimagePoints, &imgpt_i[k], &tmpimagePoints ); 21426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 21436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn l2err = cvNorm( &tmpimagePoints, 0, CV_L2 ); 21446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn maxErr = cvNorm( &tmpimagePoints, 0, CV_C ); 21456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 21466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( JtJ || JtErr ) 21476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 21486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int iofs = (nimages+1)*6 + k*NINTRINSIC, eofs = (i+1)*6; 21496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn assert( JtJ && JtErr ); 21506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 21516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( k == 1 ) 21526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 21536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // d(err_{x|y}R) ~ de3 21546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // convert de3/{dr3,dt3} => de3{dr1,dt1} & de3{dr2,dt2} 21556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( p = 0; p < ni*2; p++ ) 21566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 21576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat de3dr3 = cvMat( 1, 3, CV_64F, Je->data.ptr + Je->step*p ); 21586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat de3dt3 = cvMat( 1, 3, CV_64F, de3dr3.data.db + 3 ); 21596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat de3dr2 = cvMat( 1, 3, CV_64F, J_LR->data.ptr + J_LR->step*p ); 21606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat de3dt2 = cvMat( 1, 3, CV_64F, de3dr2.data.db + 3 ); 21616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double _de3dr1[3], _de3dt1[3]; 21626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat de3dr1 = cvMat( 1, 3, CV_64F, _de3dr1 ); 21636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat de3dt1 = cvMat( 1, 3, CV_64F, _de3dt1 ); 21646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 21656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvMatMul( &de3dr3, &dr3dr1, &de3dr1 ); 21666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvMatMul( &de3dt3, &dt3dt1, &de3dt1 ); 21676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 21686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvMatMul( &de3dr3, &dr3dr2, &de3dr2 ); 21696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvMatMulAdd( &de3dt3, &dt3dr2, &de3dr2, &de3dr2 ); 21706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 21716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvMatMul( &de3dt3, &dt3dt2, &de3dt2 ); 21726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 21736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvCopy( &de3dr1, &de3dr3 ); 21746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvCopy( &de3dt1, &de3dt3 ); 21756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 21766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 21776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetSubRect( JtJ, &_part, cvRect(0, 0, 6, 6) ); 21786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGEMM( J_LR, J_LR, 1, &_part, 1, &_part, CV_GEMM_A_T ); 21796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 21806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetSubRect( JtJ, &_part, cvRect(eofs, 0, 6, 6) ); 21816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGEMM( J_LR, Je, 1, 0, 0, &_part, CV_GEMM_A_T ); 21826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 21836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetRows( JtErr, &_part, 0, 6 ); 21846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGEMM( J_LR, err, 1, &_part, 1, &_part, CV_GEMM_A_T ); 21856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 21866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 21876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetSubRect( JtJ, &_part, cvRect(eofs, eofs, 6, 6) ); 21886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGEMM( Je, Je, 1, &_part, 1, &_part, CV_GEMM_A_T ); 21896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 21906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetRows( JtErr, &_part, eofs, eofs + 6 ); 21916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGEMM( Je, err, 1, &_part, 1, &_part, CV_GEMM_A_T ); 21926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 21936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( recomputeIntrinsics ) 21946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 21956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetSubRect( JtJ, &_part, cvRect(iofs, iofs, NINTRINSIC, NINTRINSIC) ); 21966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGEMM( Ji, Ji, 1, &_part, 1, &_part, CV_GEMM_A_T ); 21976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetSubRect( JtJ, &_part, cvRect(iofs, eofs, NINTRINSIC, 6) ); 21986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGEMM( Je, Ji, 1, &_part, 1, &_part, CV_GEMM_A_T ); 21996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( k == 1 ) 22006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 22016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetSubRect( JtJ, &_part, cvRect(iofs, 0, NINTRINSIC, 6) ); 22026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGEMM( J_LR, Ji, 1, &_part, 1, &_part, CV_GEMM_A_T ); 22036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 22046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetRows( JtErr, &_part, iofs, iofs + NINTRINSIC ); 22056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGEMM( Ji, err, 1, &_part, 1, &_part, CV_GEMM_A_T ); 22066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 22076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 22086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 22096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( errNorm ) 22106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn *errNorm += l2err*l2err; 22116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 22126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 22136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 22146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 22156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvRodrigues2( &om_LR, &R_LR ); 22166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( _R->rows == 1 || _R->cols == 1 ) 22176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( &om_LR, _R ); 22186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 22196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( &R_LR, _R ); 22206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( &T_LR, _T ); 22216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 22226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( recomputeIntrinsics ) 22236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 22246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( &K[0], _cameraMatrix1 ); 22256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( &K[1], _cameraMatrix2 ); 22266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 22276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( k = 0; k < 2; k++ ) 22286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 22296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* distCoeffs = k == 0 ? _distCoeffs1 : _distCoeffs2; 22306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat tdist = cvMat( distCoeffs->rows, distCoeffs->cols, 22316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_MAKETYPE(CV_64F,CV_MAT_CN(distCoeffs->type)), Dist[k].data.db ); 22326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( &tdist, distCoeffs ); 22336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 22346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 22356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 22366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( _E || _F ) 22376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 22386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double* t = T_LR.data.db; 22396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double tx[] = 22406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 22416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 0, -t[2], t[1], 22426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn t[2], 0, -t[0], 22436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn -t[1], t[0], 0 22446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn }; 22456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat Tx = cvMat(3, 3, CV_64F, tx); 22466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double e[9], f[9]; 22476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat E = cvMat(3, 3, CV_64F, e); 22486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat F = cvMat(3, 3, CV_64F, f); 22496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvMatMul( &Tx, &R_LR, &E ); 22506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( _E ) 22516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( &E, _E ); 22526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( _F ) 22536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 22546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double ik[9]; 22556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat iK = cvMat(3, 3, CV_64F, ik); 22566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvInvert(&K[1], &iK); 22576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGEMM( &iK, &E, 1, 0, 0, &E, CV_GEMM_A_T ); 22586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvInvert(&K[0], &iK); 22596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvMatMul(&E, &iK, &F); 22606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvertScale( &F, _F, fabs(f[8]) > 0 ? 1./f[8] : 1 ); 22616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 22626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 22636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 22646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 22656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 22666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &npoints ); 22676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &err ); 22686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &J_LR ); 22696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &Je ); 22706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &Ji ); 22716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &RT0 ); 22726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &objectPoints ); 22736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &imagePoints[0] ); 22746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &imagePoints[1] ); 22756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 22766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 22776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 22786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid cvStereoRectify( const CvMat* _cameraMatrix1, const CvMat* _cameraMatrix2, 22796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const CvMat* _distCoeffs1, const CvMat* _distCoeffs2, 22806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvSize imageSize, const CvMat* _R, const CvMat* _T, 22816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* _R1, CvMat* _R2, CvMat* _P1, CvMat* _P2, 22826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* _Q, int flags ) 22836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 22846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double _om[3], _t[3], _uu[3]={0,0,0}, _r_r[3][3], _pp[3][4]; 22856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double _ww[3], _wr[3][3], _z[3] = {0,0,0}, _ri[3][3]; 22866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat om = cvMat(3, 1, CV_64F, _om); 22876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat t = cvMat(3, 1, CV_64F, _t); 22886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat uu = cvMat(3, 1, CV_64F, _uu); 22896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat r_r = cvMat(3, 3, CV_64F, _r_r); 22906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat pp = cvMat(3, 4, CV_64F, _pp); 22916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat ww = cvMat(3, 1, CV_64F, _ww); // temps 22926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat wR = cvMat(3, 3, CV_64F, _wr); 22936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat Z = cvMat(3, 1, CV_64F, _z); 22946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat Ri = cvMat(3, 3, CV_64F, _ri); 22956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double nx = imageSize.width, ny = imageSize.height; 22966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i, k; 22976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 22986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( _R->rows == 3 && _R->cols == 3 ) 22996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvRodrigues2(_R, &om); // get vector rotation 23006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 23016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert(_R, &om); // it's already a rotation vector 23026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvertScale(&om, &om, -0.5); // get average rotation 23036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvRodrigues2(&om, &r_r); // rotate cameras to same orientation by averaging 23046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvMatMul(&r_r, _T, &t); 23056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 23066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int idx = fabs(_t[0]) > fabs(_t[1]) ? 0 : 1; 23076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double c = _t[idx], nt = cvNorm(&t, 0, CV_L2); 23086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _uu[idx] = c > 0 ? 1 : -1; 23096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 23106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // calculate global Z rotation 23116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvCrossProduct(&t,&uu,&ww); 23126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double nw = cvNorm(&ww, 0, CV_L2); 23136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvertScale(&ww, &ww, acos(fabs(c)/nt)/nw); 23146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvRodrigues2(&ww, &wR); 23156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 23166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // apply to both views 23176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGEMM(&wR, &r_r, 1, 0, 0, &Ri, CV_GEMM_B_T); 23186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( &Ri, _R1 ); 23196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGEMM(&wR, &r_r, 1, 0, 0, &Ri, 0); 23206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( &Ri, _R2 ); 23216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvMatMul(&r_r, _T, &t); 23226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 23236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // calculate projection/camera matrices 23246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // these contain the relevant rectified image internal params (fx, fy=fx, cx, cy) 23256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double fc_new = DBL_MAX; 23266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D64f cc_new[2] = {{0,0}, {0,0}}; 23276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 23286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( k = 0; k < 2; k++ ) 23296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 23306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const CvMat* A = k == 0 ? _cameraMatrix1 : _cameraMatrix2; 23316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const CvMat* Dk = k == 0 ? _distCoeffs1 : _distCoeffs2; 23326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D32f _pts[4]; 23336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint3D32f _pts_3[4]; 23346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat pts = cvMat(1, 4, CV_32FC2, _pts); 23356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat pts_3 = cvMat(1, 4, CV_32FC3, _pts_3); 23366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double fc, dk1 = Dk ? cvmGet(Dk, 0, 0) : 0; 23376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 23386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fc = cvmGet(A,idx^1,idx^1); 23396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( dk1 < 0 ) 23406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fc *= 1 + 0.2*dk1*(nx*nx + ny*ny)/(8*fc*fc); 23416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fc_new = MIN(fc_new, fc); 23426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 23436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < 4; i++ ) 23446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 23456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _pts[i].x = (float)(((i % 2) + 0.5)*nx*0.5); 23466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _pts[i].y = (float)(((i / 2) + 0.5)*ny*0.5); 23476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 23486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvUndistortPoints( &pts, &pts, A, Dk, 0, 0 ); 23496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvertPointsHomogeneous( &pts, &pts_3 ); 23506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvProjectPoints2( &pts_3, k == 0 ? _R1 : _R2, &Z, A, 0, &pts ); 23516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvScalar avg = cvAvg(&pts); 23526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cc_new[k].x = avg.val[0]; 23536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cc_new[k].y = avg.val[1]; 23546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 23556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 23566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // vertical focal length must be the same for both images to keep the epipolar constraint 23576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // (for horizontal epipolar lines -- TBD: check for vertical epipolar lines) 23586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // use fy for fx also, for simplicity 23596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 23606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // For simplicity, set the principal points for both cameras to be the average 23616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // of the two principal points (either one of or both x- and y- coordinates) 23626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( flags & CV_CALIB_ZERO_DISPARITY ) 23636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 23646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cc_new[0].x = cc_new[1].x = (cc_new[0].x + cc_new[1].x)*0.5; 23656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cc_new[0].y = cc_new[1].y = (cc_new[0].y + cc_new[1].y)*0.5; 23666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 23676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else if( idx == 0 ) // horizontal stereo 23686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cc_new[0].y = cc_new[1].y = (cc_new[0].y + cc_new[1].y)*0.5; 23696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else // vertical stereo 23706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cc_new[0].x = cc_new[1].x = (cc_new[0].x + cc_new[1].x)*0.5; 23716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 23726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvZero( &pp ); 23736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _pp[0][0] = _pp[1][1] = fc_new; 23746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _pp[0][2] = cc_new[0].x; 23756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _pp[1][2] = cc_new[0].y; 23766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _pp[2][2] = 1; 23776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert(&pp, _P1); 23786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 23796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _pp[0][2] = cc_new[1].x; 23806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _pp[1][2] = cc_new[1].y; 23816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _pp[idx][3] = _t[idx]*fc_new; // baseline * focal length 23826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert(&pp, _P2); 23836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 23846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( _Q ) 23856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 23866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double q[] = 23876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 23886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1, 0, 0, -cc_new[0].x, 23896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 0, 1, 0, -cc_new[0].y, 23906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 0, 0, 0, fc_new, 23916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 0, 0, 1./_t[idx], 23926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (idx == 0 ? cc_new[0].x - cc_new[1].x : cc_new[0].y - cc_new[1].y)/_t[idx] 23936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn }; 23946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat Q = cvMat(4, 4, CV_64F, q); 23956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( &Q, _Q ); 23966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 23976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 23986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 23996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 24006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_IMPL int 24016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RenncvStereoRectifyUncalibrated( 24026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const CvMat* _points1, const CvMat* _points2, 24036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const CvMat* F0, CvSize imgSize, CvMat* _H1, CvMat* _H2, double threshold ) 24046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 24056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int result = 0; 24066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* _m1 = 0; 24076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* _m2 = 0; 24086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* _lines1 = 0; 24096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat* _lines2 = 0; 24106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 24116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "cvStereoCalcHomographiesFromF" ); 24126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 24136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 24146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 24156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i, j, npoints; 24166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double cx, cy; 24176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double u[9], v[9], w[9], f[9], h1[9], h2[9], h0[9], e2[3]; 24186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat E2 = cvMat( 3, 1, CV_64F, e2 ); 24196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat U = cvMat( 3, 3, CV_64F, u ); 24206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat V = cvMat( 3, 3, CV_64F, v ); 24216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat W = cvMat( 3, 3, CV_64F, w ); 24226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat F = cvMat( 3, 3, CV_64F, f ); 24236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat H1 = cvMat( 3, 3, CV_64F, h1 ); 24246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat H2 = cvMat( 3, 3, CV_64F, h2 ); 24256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat H0 = cvMat( 3, 3, CV_64F, h0 ); 24266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 24276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D64f* m1; 24286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint2D64f* m2; 24296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint3D64f* lines1; 24306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvPoint3D64f* lines2; 24316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 24326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ASSERT( CV_IS_MAT(_points1) && CV_IS_MAT(_points2) && 24336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (_points1->rows == 1 || _points1->cols == 1) && 24346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (_points2->rows == 1 || _points2->cols == 1) && 24356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ARE_SIZES_EQ(_points1, _points2) ); 24366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 24376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn npoints = _points1->rows * _points1->cols * CV_MAT_CN(_points1->type) / 2; 24386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 24396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _m1 = cvCreateMat( _points1->rows, _points1->cols, CV_64FC(CV_MAT_CN(_points1->type)) ); 24406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _m2 = cvCreateMat( _points2->rows, _points2->cols, CV_64FC(CV_MAT_CN(_points2->type)) ); 24416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _lines1 = cvCreateMat( 1, npoints, CV_64FC3 ); 24426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _lines2 = cvCreateMat( 1, npoints, CV_64FC3 ); 24436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 24446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( F0, &F ); 24456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 24466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSVD( (CvMat*)&F, &W, &U, &V, CV_SVD_U_T + CV_SVD_V_T ); 24476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn W.data.db[8] = 0.; 24486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGEMM( &U, &W, 1, 0, 0, &W, CV_GEMM_A_T ); 24496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvMatMul( &W, &V, &F ); 24506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 24516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cx = cvRound( (imgSize.width-1)*0.5 ); 24526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cy = cvRound( (imgSize.height-1)*0.5 ); 24536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 24546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvZero( _H1 ); 24556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvZero( _H2 ); 24566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 24576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( _points1, _m1 ); 24586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( _points2, _m2 ); 24596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReshape( _m1, _m1, 2, 1 ); 24606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReshape( _m1, _m1, 2, 1 ); 24616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 24626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn m1 = (CvPoint2D64f*)_m1->data.ptr; 24636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn m2 = (CvPoint2D64f*)_m2->data.ptr; 24646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn lines1 = (CvPoint3D64f*)_lines1->data.ptr; 24656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn lines2 = (CvPoint3D64f*)_lines2->data.ptr; 24666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 24676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( threshold > 0 ) 24686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 24696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvComputeCorrespondEpilines( _m1, 1, &F, _lines1 ); 24706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvComputeCorrespondEpilines( _m2, 2, &F, _lines2 ); 24716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 24726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn // measure distance from points to the corresponding epilines, mark outliers 24736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = j = 0; i < npoints; i++ ) 24746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 24756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( fabs(m1[i].x*lines2[i].x + 24766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn m1[i].y*lines2[i].y + 24776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn lines2[i].z) <= threshold && 24786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn fabs(m2[i].x*lines1[i].x + 24796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn m2[i].y*lines1[i].y + 24806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn lines1[i].z) <= threshold ) 24816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 24826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( j > i ) 24836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 24846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn m1[j] = m1[i]; 24856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn m2[j] = m2[i]; 24866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 24876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn j++; 24886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 24896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 24906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 24916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn npoints = j; 24926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( npoints == 0 ) 24936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn EXIT; 24946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 24956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 24966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 24976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _m1->cols = _m2->cols = npoints; 24986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn memcpy( E2.data.db, U.data.db + 6, sizeof(e2)); 24996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvScale( &E2, &E2, e2[2] > 0 ? 1 : -1 ); 25006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 25016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double t[] = 25026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 25036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1, 0, -cx, 25046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 0, 1, -cy, 25056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 0, 0, 1 25066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn }; 25076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat T = cvMat(3, 3, CV_64F, t); 25086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvMatMul( &T, &E2, &E2 ); 25096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 25106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int mirror = e2[0] < 0; 25116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double d = MAX(sqrt(e2[0]*e2[0] + e2[1]*e2[1]),DBL_EPSILON); 25126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double alpha = e2[0]/d; 25136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double beta = e2[1]/d; 25146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double r[] = 25156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 25166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn alpha, beta, 0, 25176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn -beta, alpha, 0, 25186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 0, 0, 1 25196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn }; 25206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat R = cvMat(3, 3, CV_64F, r); 25216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvMatMul( &R, &T, &T ); 25226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvMatMul( &R, &E2, &E2 ); 25236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double invf = fabs(e2[2]) < 1e-6*fabs(e2[0]) ? 0 : -e2[2]/e2[0]; 25246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double k[] = 25256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 25266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1, 0, 0, 25276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 0, 1, 0, 25286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn invf, 0, 1 25296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn }; 25306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat K = cvMat(3, 3, CV_64F, k); 25316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvMatMul( &K, &T, &H2 ); 25326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvMatMul( &K, &E2, &E2 ); 25336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 25346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double it[] = 25356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 25366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1, 0, cx, 25376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 0, 1, cy, 25386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 0, 0, 1 25396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn }; 25406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat iT = cvMat( 3, 3, CV_64F, it ); 25416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvMatMul( &iT, &H2, &H2 ); 25426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 25436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn memcpy( E2.data.db, U.data.db + 6, sizeof(e2)); 25446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvScale( &E2, &E2, e2[2] > 0 ? 1 : -1 ); 25456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 25466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double e2_x[] = 25476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 25486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 0, -e2[2], e2[1], 25496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn e2[2], 0, -e2[0], 25506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn -e2[1], e2[0], 0 25516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn }; 25526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double e2_111[] = 25536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 25546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn e2[0], e2[0], e2[0], 25556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn e2[1], e2[1], e2[1], 25566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn e2[2], e2[2], e2[2], 25576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn }; 25586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat E2_x = cvMat(3, 3, CV_64F, e2_x); 25596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat E2_111 = cvMat(3, 3, CV_64F, e2_111); 25606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvMatMulAdd(&E2_x, &F, &E2_111, &H0 ); 25616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvMatMul(&H2, &H0, &H0); 25626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat E1=cvMat(3, 1, CV_64F, V.data.db+6); 25636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvMatMul(&H0, &E1, &E1); 25646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 25656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvPerspectiveTransform( _m1, _m1, &H0 ); 25666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvPerspectiveTransform( _m2, _m2, &H2 ); 25676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat A = cvMat( 1, npoints, CV_64FC3, lines1 ), BxBy, B; 25686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double a[9], atb[3], x[3]; 25696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat AtA = cvMat( 3, 3, CV_64F, a ); 25706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat AtB = cvMat( 3, 1, CV_64F, atb ); 25716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat X = cvMat( 3, 1, CV_64F, x ); 25726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvertPointsHomogeneous( _m1, &A ); 25736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReshape( &A, &A, 1, npoints ); 25746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReshape( _m2, &BxBy, 1, npoints ); 25756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGetCol( &BxBy, &B, 0 ); 25766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGEMM( &A, &A, 1, 0, 0, &AtA, CV_GEMM_A_T ); 25776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvGEMM( &A, &B, 1, 0, 0, &AtB, CV_GEMM_A_T ); 25786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSolve( &AtA, &AtB, &X, CV_SVD_SYM ); 25796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 25806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double ha[] = 25816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 25826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn x[0], x[1], x[2], 25836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 0, 1, 0, 25846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 0, 0, 1 25856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn }; 25866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat Ha = cvMat(3, 3, CV_64F, ha); 25876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvMatMul( &Ha, &H0, &H1 ); 25886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvPerspectiveTransform( _m1, _m1, &Ha ); 25896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 25906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( mirror ) 25916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 25926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double mm[] = { -1, 0, cx*2, 0, -1, cy*2, 0, 0, 1 }; 25936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat MM = cvMat(3, 3, CV_64F, mm); 25946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvMatMul( &MM, &H1, &H1 ); 25956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvMatMul( &MM, &H2, &H2 ); 25966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 25976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 25986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( &H1, _H1 ); 25996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( &H2, _H2 ); 26006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 26016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn result = 1; 26026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 26036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 26046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 26056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 26066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &_m1 ); 26076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &_m2 ); 26086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &_lines1 ); 26096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat( &_lines2 ); 26106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 26116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return result; 26126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 26136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 26146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 26156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_IMPL void 26166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RenncvReprojectImageTo3D( 26176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const CvArr* disparityImage, 26186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvArr* _3dImage, const CvMat* _Q ) 26196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 26206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "cvReprojectImageTo3D" ); 26216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 26226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 26236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 26246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double q[4][4]; 26256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat Q = cvMat(4, 4, CV_64F, q); 26266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat sstub, *src = cvGetMat( disparityImage, &sstub ); 26276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat dstub, *dst = cvGetMat( _3dImage, &dstub ); 26286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int stype = CV_MAT_TYPE(src->type), dtype = CV_MAT_TYPE(dst->type); 26296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int x, y, rows = src->rows, cols = src->cols; 26306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float* sbuf = (float*)cvStackAlloc( cols*sizeof(sbuf[0]) ); 26316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float* dbuf = (float*)cvStackAlloc( cols*3*sizeof(dbuf[0]) ); 26326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 26336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ASSERT( CV_ARE_SIZES_EQ(src, dst) && 26346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (CV_MAT_TYPE(stype) == CV_16SC1 || CV_MAT_TYPE(stype) == CV_32FC1) && 26356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (CV_MAT_TYPE(dtype) == CV_16SC3 || CV_MAT_TYPE(dtype) == CV_32FC3) ); 26366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 26376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvConvert( _Q, &Q ); 26386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 26396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( y = 0; y < rows; y++ ) 26406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 26416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const float* sptr = (const float*)(src->data.ptr + src->step*y); 26426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float* dptr0 = (float*)(dst->data.ptr + dst->step*y), *dptr = dptr0; 26436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double qx = q[0][1]*y + q[0][3], qy = q[1][1]*y + q[1][3]; 26446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double qz = q[2][1]*y + q[2][3], qw = q[3][1]*y + q[3][3]; 26456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 26466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( stype == CV_16SC1 ) 26476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 26486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn const short* sptr0 = (const short*)sptr; 26496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( x = 0; x < cols; x++ ) 26506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sbuf[x] = (float)sptr0[x]; 26516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn sptr = sbuf; 26526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 26536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( dtype != CV_32FC3 ) 26546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dptr = dbuf; 26556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 26566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( x = 0; x < cols; x++, qx += q[0][0], qy += q[1][0], qz += q[2][0], qw += q[3][0] ) 26576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 26586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double d = sptr[x]; 26596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double iW = 1./(qw + q[3][2]*d); 26606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double X = (qx + q[0][2]*d)*iW; 26616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double Y = (qy + q[1][2]*d)*iW; 26626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double Z = (qz + q[2][2]*d)*iW; 26636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 26646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dptr[x*3] = (float)X; 26656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dptr[x*3+1] = (float)Y; 26666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn dptr[x*3+2] = (float)Z; 26676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 26686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 26696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( dtype == CV_16SC3 ) 26706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 26716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( x = 0; x < cols*3; x++ ) 26726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 26736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int ival = cvRound(dptr[x]); 26746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ((short*)dptr0)[x] = CV_CAST_16S(ival); 26756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 26766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 26776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 26786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 26796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 26806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 26816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 26826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 26836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* End of file. */ 2684