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 466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn calculate image homography 476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn\****************************************************************************************/ 496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_IMPL void 516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RenncvCalcImageHomography( float* line, CvPoint3D32f* _center, 526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float* _intrinsic, float* _homography ) 536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "cvCalcImageHomography" ); 556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double norm_xy, norm_xz, xy_sina, xy_cosa, xz_sina, xz_cosa, nx1, plane_dist; 596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float _ry[3], _rz[3], _r_trans[9]; 606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat rx = cvMat( 1, 3, CV_32F, line ); 616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat ry = cvMat( 1, 3, CV_32F, _ry ); 626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat rz = cvMat( 1, 3, CV_32F, _rz ); 636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat r_trans = cvMat( 3, 3, CV_32F, _r_trans ); 646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat center = cvMat( 3, 1, CV_32F, _center ); 656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float _sub[9]; 676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat sub = cvMat( 3, 3, CV_32F, _sub ); 686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float _t_trans[3]; 696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat t_trans = cvMat( 3, 1, CV_32F, _t_trans ); 706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat intrinsic = cvMat( 3, 3, CV_32F, _intrinsic ); 726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat homography = cvMat( 3, 3, CV_32F, _homography ); 736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !line || !_center || !_intrinsic || !_homography ) 756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNullPtr, "" ); 766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn norm_xy = cvSqrt( line[0] * line[0] + line[1] * line[1] ); 786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn xy_cosa = line[0] / norm_xy; 796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn xy_sina = line[1] / norm_xy; 806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn norm_xz = cvSqrt( line[0] * line[0] + line[2] * line[2] ); 826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn xz_cosa = line[0] / norm_xz; 836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn xz_sina = line[2] / norm_xz; 846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn nx1 = -xz_sina; 866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _rz[0] = (float)(xy_cosa * nx1); 886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _rz[1] = (float)(xy_sina * nx1); 896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn _rz[2] = (float)xz_cosa; 906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvScale( &rz, &rz, 1./cvNorm(&rz,0,CV_L2) ); 916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* new axe y */ 936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvCrossProduct( &rz, &rx, &ry ); 946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvScale( &ry, &ry, 1./cvNorm( &ry, 0, CV_L2 ) ); 956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* transpone rotation matrix */ 976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn memcpy( &_r_trans[0], line, 3*sizeof(float)); 986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn memcpy( &_r_trans[3], _ry, 3*sizeof(float)); 996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn memcpy( &_r_trans[6], _rz, 3*sizeof(float)); 1006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* calculate center distanse from arm plane */ 1026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn plane_dist = cvDotProduct( ¢er, &rz ); 1036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* calculate (I - r_trans)*center */ 1056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSetIdentity( &sub ); 1066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSub( &sub, &r_trans, &sub ); 1076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvMatMul( &sub, ¢er, &t_trans ); 1086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvMatMul( &t_trans, &rz, &sub ); 1106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvScaleAdd( &sub, cvRealScalar(1./plane_dist), &r_trans, &sub ); /* ? */ 1116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvMatMul( &intrinsic, &sub, &r_trans ); 1136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvInvert( &intrinsic, &sub, CV_SVD ); 1146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvMatMul( &r_trans, &sub, &homography ); 1156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 1176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 1186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* End of file. */ 1206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 121