2026-08-02 01:36:11 +00:00
<!-- HTML header for doxygen 1.9.8-->
<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "https://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
< html xmlns = "http://www.w3.org/1999/xhtml" lang = "en-US" >
< head >
< meta http-equiv = "Content-Type" content = "text/xhtml;charset=UTF-8" />
< meta http-equiv = "X-UA-Compatible" content = "IE=11" />
< meta name = "generator" content = "Doxygen 1.9.8" />
< meta name = "viewport" content = "width=device-width, initial-scale=1" />
< title > RTAB-Map: corelib/include/rtabmap/core/stereo/stereoRectifyFisheye.h Source File</ title >
< link href = "tabs.css" rel = "stylesheet" type = "text/css" />
< script type = "text/javascript" src = "jquery.js" ></ script >
< script type = "text/javascript" src = "dynsections.js" ></ script >
2026-08-02 07:56:38 +00:00
< link href = "navtree.css" rel = "stylesheet" type = "text/css" />
< script type = "text/javascript" src = "resize.js" ></ script >
< script type = "text/javascript" src = "navtreedata.js" ></ script >
< script type = "text/javascript" src = "navtree.js" ></ script >
2026-08-02 01:36:11 +00:00
< link href = "search/search.css" rel = "stylesheet" type = "text/css" />
< script type = "text/javascript" src = "search/searchdata.js" ></ script >
< script type = "text/javascript" src = "search/search.js" ></ script >
< link href = "doxygen.css" rel = "stylesheet" type = "text/css" />
2026-08-02 17:35:14 +00:00
< link href = "doxygen-awesome.css" rel = "stylesheet" type = "text/css" />
< link href = "custom.css" rel = "stylesheet" type = "text/css" />
2026-08-02 01:36:11 +00:00
< script type = "text/javascript" > window . RTABMAP_DOC_ROOT = "" ;</ script >
< script type = "text/javascript" src = "../versions.js" ></ script >
< script type = "text/javascript" src = "version-switcher.js" ></ script >
2026-08-02 17:35:14 +00:00
< script type = "text/javascript" src = "doxygen-awesome-darkmode-toggle.js" ></ script >
< script type = "text/javascript" > DoxygenAwesomeDarkModeToggle . init ();</ script >
2026-08-02 01:36:11 +00:00
</ head >
< body >
< div id = "top" > <!-- do not remove this div, it is closed by doxygen! -->
< div id = "titlearea" >
< table cellspacing = "0" cellpadding = "0" >
< tbody >
< tr id = "projectrow" >
< td id = "projectalign" >
< div id = "projectname" > RTAB-Map< span id = "projectnumber" >   0.23.10</ span >
</ div >
2026-08-02 07:56:38 +00:00
< div id = "projectbrief" > Real-Time Appearance-Based Mapping</ div >
2026-08-02 01:36:11 +00:00
</ td >
</ tr >
</ tbody >
</ table >
</ div >
<!-- end header part -->
<!-- Generated by Doxygen 1.9.8 -->
< script type = "text/javascript" >
/* @license magnet:?xt=urn:btih:d3d9a9a6595521f9666a5e94cc830dab83b65699&dn=expat.txt MIT */
var searchBox = new SearchBox ( "searchBox" , "search/" , '.html' );
/* @license-end */
</ script >
< script type = "text/javascript" src = "menudata.js" ></ script >
< script type = "text/javascript" src = "menu.js" ></ script >
< script type = "text/javascript" >
/* @license magnet:?xt=urn:btih:d3d9a9a6595521f9666a5e94cc830dab83b65699&dn=expat.txt MIT */
$ ( function () {
initMenu ( '' , true , false , 'search.php' , 'Search' );
$ ( document ). ready ( function () { init_search (); });
});
/* @license-end */
</ script >
< div id = "main-nav" ></ div >
< script type = "text/javascript" >
/* @license magnet:?xt=urn:btih:d3d9a9a6595521f9666a5e94cc830dab83b65699&dn=expat.txt MIT */
$ ( document ). ready ( function () { init_codefold ( 0 ); });
/* @license-end */
</ script >
2026-08-02 07:56:38 +00:00
</ div > <!-- top -->
< div id = "side-nav" class = "ui-resizable side-nav-resizable" >
< div id = "nav-tree" >
< div id = "nav-tree-contents" >
< div id = "nav-sync" class = "sync" ></ div >
</ div >
</ div >
< div id = "splitbar" style = "-moz-user-select:none;"
class = "ui-resizable-handle" >
</ div >
</ div >
< script type = "text/javascript" >
/* @license magnet:?xt=urn:btih:d3d9a9a6595521f9666a5e94cc830dab83b65699&dn=expat.txt MIT */
$ ( document ). ready ( function (){ initNavTree ( 'stereoRectifyFisheye_8h_source.html' , '' ); initResizable (); });
/* @license-end */
</ script >
< div id = "doc-content" >
2026-08-02 01:36:11 +00:00
<!-- window showing the filter options -->
< div id = "MSearchSelectWindow"
onmouseover = "return searchBox.OnSearchSelectShow()"
onmouseout = "return searchBox.OnSearchSelectHide()"
onkeydown = "return searchBox.OnSearchSelectKey(event)" >
</ div >
<!-- iframe showing the search results (closed by default) -->
< div id = "MSearchResultsWindow" >
< div id = "MSearchResults" >
< div class = "SRPage" >
< div id = "SRIndex" >
< div id = "SRResults" ></ div >
< div class = "SRStatus" id = "Loading" > Loading...</ div >
< div class = "SRStatus" id = "Searching" > Searching...</ div >
< div class = "SRStatus" id = "NoMatches" > No Matches</ div >
</ div >
</ div >
</ div >
</ div >
< div class = "header" >
< div class = "headertitle" >< div class = "title" > stereoRectifyFisheye.h</ div ></ div >
</ div > <!--header-->
< div class = "contents" >
< div class = "fragment" >< div class = "line" >< a id = "l00001" name = "l00001" ></ a >< span class = "lineno" > 1</ span >< span class = "comment" > /*</ span ></ div >
< div class = "line" >< a id = "l00002" name = "l00002" ></ a >< span class = "lineno" > 2</ span >< span class = "comment" > Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke</ span ></ div >
< div class = "line" >< a id = "l00003" name = "l00003" ></ a >< span class = "lineno" > 3</ span >< span class = "comment" > All rights reserved.</ span ></ div >
< div class = "line" >< a id = "l00004" name = "l00004" ></ a >< span class = "lineno" > 4</ span >< span class = "comment" ></ span > </ div >
< div class = "line" >< a id = "l00005" name = "l00005" ></ a >< span class = "lineno" > 5</ span >< span class = "comment" > This code is the same has cv::stereoRectify() but accepting fisheye distortion model:</ span ></ div >
< div class = "line" >< a id = "l00006" name = "l00006" ></ a >< span class = "lineno" > 6</ span >< span class = "comment" > All cvUndistortPoints() have been replaced by cv::fisheye::undistortPoints()</ span ></ div >
< div class = "line" >< a id = "l00007" name = "l00007" ></ a >< span class = "lineno" > 7</ span >< span class = "comment" > See https://github.com/opencv/opencv/blob/master/modules/calib3d/src/calibration.cpp</ span ></ div >
< div class = "line" >< a id = "l00008" name = "l00008" ></ a >< span class = "lineno" > 8</ span >< span class = "comment" ></ span > </ div >
< div class = "line" >< a id = "l00009" name = "l00009" ></ a >< span class = "lineno" > 9</ span >< span class = "comment" > Redistribution and use in source and binary forms, with or without</ span ></ div >
< div class = "line" >< a id = "l00010" name = "l00010" ></ a >< span class = "lineno" > 10</ span >< span class = "comment" > modification, are permitted provided that the following conditions are met:</ span ></ div >
< div class = "line" >< a id = "l00011" name = "l00011" ></ a >< span class = "lineno" > 11</ span >< span class = "comment" > * Redistributions of source code must retain the above copyright</ span ></ div >
< div class = "line" >< a id = "l00012" name = "l00012" ></ a >< span class = "lineno" > 12</ span >< span class = "comment" > notice, this list of conditions and the following disclaimer.</ span ></ div >
< div class = "line" >< a id = "l00013" name = "l00013" ></ a >< span class = "lineno" > 13</ span >< span class = "comment" > * Redistributions in binary form must reproduce the above copyright</ span ></ div >
< div class = "line" >< a id = "l00014" name = "l00014" ></ a >< span class = "lineno" > 14</ span >< span class = "comment" > notice, this list of conditions and the following disclaimer in the</ span ></ div >
< div class = "line" >< a id = "l00015" name = "l00015" ></ a >< span class = "lineno" > 15</ span >< span class = "comment" > documentation and/or other materials provided with the distribution.</ span ></ div >
< div class = "line" >< a id = "l00016" name = "l00016" ></ a >< span class = "lineno" > 16</ span >< span class = "comment" > * Neither the name of the Universite de Sherbrooke nor the</ span ></ div >
< div class = "line" >< a id = "l00017" name = "l00017" ></ a >< span class = "lineno" > 17</ span >< span class = "comment" > names of its contributors may be used to endorse or promote products</ span ></ div >
< div class = "line" >< a id = "l00018" name = "l00018" ></ a >< span class = "lineno" > 18</ span >< span class = "comment" > derived from this software without specific prior written permission.</ span ></ div >
< div class = "line" >< a id = "l00019" name = "l00019" ></ a >< span class = "lineno" > 19</ span >< span class = "comment" ></ span > </ div >
< div class = "line" >< a id = "l00020" name = "l00020" ></ a >< span class = "lineno" > 20</ span >< span class = "comment" > THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS " AS IS" AND</ span ></ div >
< div class = "line" >< a id = "l00021" name = "l00021" ></ a >< span class = "lineno" > 21</ span >< span class = "comment" > ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED</ span ></ div >
< div class = "line" >< a id = "l00022" name = "l00022" ></ a >< span class = "lineno" > 22</ span >< span class = "comment" > WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE</ span ></ div >
< div class = "line" >< a id = "l00023" name = "l00023" ></ a >< span class = "lineno" > 23</ span >< span class = "comment" > DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY</ span ></ div >
< div class = "line" >< a id = "l00024" name = "l00024" ></ a >< span class = "lineno" > 24</ span >< span class = "comment" > DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES</ span ></ div >
< div class = "line" >< a id = "l00025" name = "l00025" ></ a >< span class = "lineno" > 25</ span >< span class = "comment" > (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;</ span ></ div >
< div class = "line" >< a id = "l00026" name = "l00026" ></ a >< span class = "lineno" > 26</ span >< span class = "comment" > LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND</ span ></ div >
< div class = "line" >< a id = "l00027" name = "l00027" ></ a >< span class = "lineno" > 27</ span >< span class = "comment" > ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT</ span ></ div >
< div class = "line" >< a id = "l00028" name = "l00028" ></ a >< span class = "lineno" > 28</ span >< span class = "comment" > (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS</ span ></ div >
< div class = "line" >< a id = "l00029" name = "l00029" ></ a >< span class = "lineno" > 29</ span >< span class = "comment" > SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.</ span ></ div >
< div class = "line" >< a id = "l00030" name = "l00030" ></ a >< span class = "lineno" > 30</ span >< span class = "comment" > */</ span ></ div >
< div class = "line" >< a id = "l00031" name = "l00031" ></ a >< span class = "lineno" > 31</ span > </ div >
< div class = "line" >< a id = "l00032" name = "l00032" ></ a >< span class = "lineno" > 32</ span >< span class = "preprocessor" > #ifndef CORELIB_SRC_OPENCV_STEREORECTIFYFISHEYE_H_</ span ></ div >
< div class = "line" >< a id = "l00033" name = "l00033" ></ a >< span class = "lineno" > 33</ span >< span class = "preprocessor" > #define CORELIB_SRC_OPENCV_STEREORECTIFYFISHEYE_H_</ span ></ div >
< div class = "line" >< a id = "l00034" name = "l00034" ></ a >< span class = "lineno" > 34</ span > </ div >
< div class = "line" >< a id = "l00035" name = "l00035" ></ a >< span class = "lineno" > 35</ span >< span class = "comment" > // This header relies on the OpenCV C API (cvRodrigues2, cvProjectPoints2, ...)</ span ></ div >
< div class = "line" >< a id = "l00036" name = "l00036" ></ a >< span class = "lineno" > 36</ span >< span class = "comment" > // which was removed in OpenCV 5. Pull in only the version macros (available in</ span ></ div >
< div class = "line" >< a id = "l00037" name = "l00037" ></ a >< span class = "lineno" > 37</ span >< span class = "comment" > // all OpenCV versions) so we can fail early with a clear message rather than</ span ></ div >
< div class = "line" >< a id = "l00038" name = "l00038" ></ a >< span class = "lineno" > 38</ span >< span class = "comment" > // with cryptic errors from the includes below.</ span ></ div >
< div class = "line" >< a id = "l00039" name = "l00039" ></ a >< span class = "lineno" > 39</ span >< span class = "preprocessor" > #include < opencv2/core/version.hpp> </ span ></ div >
< div class = "line" >< a id = "l00040" name = "l00040" ></ a >< span class = "lineno" > 40</ span >< span class = "preprocessor" > #if CV_MAJOR_VERSION > = 5</ span ></ div >
< div class = "line" >< a id = "l00041" name = "l00041" ></ a >< span class = "lineno" > 41</ span >< span class = "preprocessor" > #error " stereoRectifyFisheye.h is not supported with OpenCV 5 or later (it uses the removed OpenCV C API). Use cv::fisheye::stereoRectify() instead, or guard your include with ' #if CV_MAJOR_VERSION < 5' ." </ span ></ div >
< div class = "line" >< a id = "l00042" name = "l00042" ></ a >< span class = "lineno" > 42</ span >< span class = "preprocessor" > #endif</ span ></ div >
< div class = "line" >< a id = "l00043" name = "l00043" ></ a >< span class = "lineno" > 43</ span > </ div >
< div class = "line" >< a id = "l00044" name = "l00044" ></ a >< span class = "lineno" > 44</ span >< span class = "preprocessor" > #include < opencv2/calib3d/calib3d.hpp> </ span ></ div >
< div class = "line" >< a id = "l00045" name = "l00045" ></ a >< span class = "lineno" > 45</ span >< span class = "preprocessor" > #if CV_MAJOR_VERSION > = 3</ span ></ div >
< div class = "line" >< a id = "l00046" name = "l00046" ></ a >< span class = "lineno" > 46</ span >< span class = "preprocessor" > #include < opencv2/calib3d/calib3d_c.h> </ span ></ div >
< div class = "line" >< a id = "l00047" name = "l00047" ></ a >< span class = "lineno" > 47</ span > </ div >
< div class = "line" >< a id = "l00048" name = "l00048" ></ a >< span class = "lineno" > 48</ span >< span class = "preprocessor" > #if CV_MAJOR_VERSION > = 4</ span ></ div >
< div class = "line" >< a id = "l00049" name = "l00049" ></ a >< span class = "lineno" > 49</ span >< span class = "preprocessor" > #include < opencv2/core/core_c.h> </ span ></ div >
< div class = "line" >< a id = "l00050" name = "l00050" ></ a >< span class = "lineno" > 50</ span > </ div >
< div class = "line" >< a id = "l00051" name = "l00051" ></ a >< span class = "lineno" > 51</ span >< span class = "comment" > // Opencv4 doesn' t expose those functions below anymore, we should recopy all of them!</ span ></ div >
< div class = "line" >< a id = "l00052" name = "l00052" ></ a >< span class = "lineno" > 52</ span >< span class = "keywordtype" > int</ span > cvRodrigues2( < span class = "keyword" > const</ span > CvMat* src, CvMat* dst, CvMat* jacobian CV_DEFAULT(0))</ div >
< div class = "line" >< a id = "l00053" name = "l00053" ></ a >< span class = "lineno" > 53</ span > {</ div >
< div class = "line" >< a id = "l00054" name = "l00054" ></ a >< span class = "lineno" > 54</ span > < span class = "keywordtype" > int</ span > depth, elem_size;</ div >
< div class = "line" >< a id = "l00055" name = "l00055" ></ a >< span class = "lineno" > 55</ span > < span class = "keywordtype" > int</ span > i, k;</ div >
< div class = "line" >< a id = "l00056" name = "l00056" ></ a >< span class = "lineno" > 56</ span > < span class = "keywordtype" > double</ span > J[27] = {0};</ div >
< div class = "line" >< a id = "l00057" name = "l00057" ></ a >< span class = "lineno" > 57</ span > CvMat matJ = cvMat( 3, 9, CV_64F, J );</ div >
< div class = "line" >< a id = "l00058" name = "l00058" ></ a >< span class = "lineno" > 58</ span > </ div >
< div class = "line" >< a id = "l00059" name = "l00059" ></ a >< span class = "lineno" > 59</ span > < span class = "keywordflow" > if</ span > ( !CV_IS_MAT(src) )</ div >
< div class = "line" >< a id = "l00060" name = "l00060" ></ a >< span class = "lineno" > 60</ span > CV_Error( !src ? CV_StsNullPtr : CV_StsBadArg, < span class = "stringliteral" > " Input argument is not a valid matrix" </ span > );</ div >
< div class = "line" >< a id = "l00061" name = "l00061" ></ a >< span class = "lineno" > 61</ span > </ div >
< div class = "line" >< a id = "l00062" name = "l00062" ></ a >< span class = "lineno" > 62</ span > < span class = "keywordflow" > if</ span > ( !CV_IS_MAT(dst) )</ div >
< div class = "line" >< a id = "l00063" name = "l00063" ></ a >< span class = "lineno" > 63</ span > CV_Error( !dst ? CV_StsNullPtr : CV_StsBadArg,</ div >
< div class = "line" >< a id = "l00064" name = "l00064" ></ a >< span class = "lineno" > 64</ span > < span class = "stringliteral" > " The first output argument is not a valid matrix" </ span > );</ div >
< div class = "line" >< a id = "l00065" name = "l00065" ></ a >< span class = "lineno" > 65</ span > </ div >
< div class = "line" >< a id = "l00066" name = "l00066" ></ a >< span class = "lineno" > 66</ span > depth = CV_MAT_DEPTH(src-> type);</ div >
< div class = "line" >< a id = "l00067" name = "l00067" ></ a >< span class = "lineno" > 67</ span > elem_size = CV_ELEM_SIZE(depth);</ div >
< div class = "line" >< a id = "l00068" name = "l00068" ></ a >< span class = "lineno" > 68</ span > </ div >
< div class = "line" >< a id = "l00069" name = "l00069" ></ a >< span class = "lineno" > 69</ span > < span class = "keywordflow" > if</ span > ( depth != CV_32F && depth != CV_64F )</ div >
< div class = "line" >< a id = "l00070" name = "l00070" ></ a >< span class = "lineno" > 70</ span > CV_Error( CV_StsUnsupportedFormat, < span class = "stringliteral" > " The matrices must have 32f or 64f data type" </ span > );</ div >
< div class = "line" >< a id = "l00071" name = "l00071" ></ a >< span class = "lineno" > 71</ span > </ div >
< div class = "line" >< a id = "l00072" name = "l00072" ></ a >< span class = "lineno" > 72</ span > < span class = "keywordflow" > if</ span > ( !CV_ARE_DEPTHS_EQ(src, dst) )</ div >
< div class = "line" >< a id = "l00073" name = "l00073" ></ a >< span class = "lineno" > 73</ span > CV_Error( CV_StsUnmatchedFormats, < span class = "stringliteral" > " All the matrices must have the same data type" </ span > );</ div >
< div class = "line" >< a id = "l00074" name = "l00074" ></ a >< span class = "lineno" > 74</ span > </ div >
< div class = "line" >< a id = "l00075" name = "l00075" ></ a >< span class = "lineno" > 75</ span > < span class = "keywordflow" > if</ span > ( jacobian )</ div >
< div class = "line" >< a id = "l00076" name = "l00076" ></ a >< span class = "lineno" > 76</ span > {</ div >
< div class = "line" >< a id = "l00077" name = "l00077" ></ a >< span class = "lineno" > 77</ span > < span class = "keywordflow" > if</ span > ( !CV_IS_MAT(jacobian) )</ div >
< div class = "line" >< a id = "l00078" name = "l00078" ></ a >< span class = "lineno" > 78</ span > CV_Error( CV_StsBadArg, < span class = "stringliteral" > " Jacobian is not a valid matrix" </ span > );</ div >
< div class = "line" >< a id = "l00079" name = "l00079" ></ a >< span class = "lineno" > 79</ span > </ div >
< div class = "line" >< a id = "l00080" name = "l00080" ></ a >< span class = "lineno" > 80</ span > < span class = "keywordflow" > if</ span > ( !CV_ARE_DEPTHS_EQ(src, jacobian) || CV_MAT_CN(jacobian-> type) != 1 )</ div >
< div class = "line" >< a id = "l00081" name = "l00081" ></ a >< span class = "lineno" > 81</ span > CV_Error( CV_StsUnmatchedFormats, < span class = "stringliteral" > " Jacobian must have 32fC1 or 64fC1 datatype" </ span > );</ div >
< div class = "line" >< a id = "l00082" name = "l00082" ></ a >< span class = "lineno" > 82</ span > </ div >
< div class = "line" >< a id = "l00083" name = "l00083" ></ a >< span class = "lineno" > 83</ span > < span class = "keywordflow" > if</ span > ( (jacobian-> rows != 9 || jacobian-> cols != 3) && </ div >
< div class = "line" >< a id = "l00084" name = "l00084" ></ a >< span class = "lineno" > 84</ span > (jacobian-> rows != 3 || jacobian-> cols != 9))</ div >
< div class = "line" >< a id = "l00085" name = "l00085" ></ a >< span class = "lineno" > 85</ span > CV_Error( CV_StsBadSize, < span class = "stringliteral" > " Jacobian must be 3x9 or 9x3" </ span > );</ div >
< div class = "line" >< a id = "l00086" name = "l00086" ></ a >< span class = "lineno" > 86</ span > }</ div >
< div class = "line" >< a id = "l00087" name = "l00087" ></ a >< span class = "lineno" > 87</ span > </ div >
< div class = "line" >< a id = "l00088" name = "l00088" ></ a >< span class = "lineno" > 88</ span > < span class = "keywordflow" > if</ span > ( src-> cols == 1 || src-> rows == 1 )</ div >
< div class = "line" >< a id = "l00089" name = "l00089" ></ a >< span class = "lineno" > 89</ span > {</ div >
< div class = "line" >< a id = "l00090" name = "l00090" ></ a >< span class = "lineno" > 90</ span > < span class = "keywordtype" > int</ span > step = src-> rows > 1 ? src-> step / elem_size : 1;</ div >
< div class = "line" >< a id = "l00091" name = "l00091" ></ a >< span class = "lineno" > 91</ span > </ div >
< div class = "line" >< a id = "l00092" name = "l00092" ></ a >< span class = "lineno" > 92</ span > < span class = "keywordflow" > if</ span > ( src-> rows + src-> cols*CV_MAT_CN(src-> type) - 1 != 3 )</ div >
< div class = "line" >< a id = "l00093" name = "l00093" ></ a >< span class = "lineno" > 93</ span > CV_Error( CV_StsBadSize, < span class = "stringliteral" > " Input matrix must be 1x3, 3x1 or 3x3" </ span > );</ div >
< div class = "line" >< a id = "l00094" name = "l00094" ></ a >< span class = "lineno" > 94</ span > </ div >
< div class = "line" >< a id = "l00095" name = "l00095" ></ a >< span class = "lineno" > 95</ span > < span class = "keywordflow" > if</ span > ( dst-> rows != 3 || dst-> cols != 3 || CV_MAT_CN(dst-> type) != 1 )</ div >
< div class = "line" >< a id = "l00096" name = "l00096" ></ a >< span class = "lineno" > 96</ span > CV_Error( CV_StsBadSize, < span class = "stringliteral" > " Output matrix must be 3x3, single-channel floating point matrix" </ span > );</ div >
< div class = "line" >< a id = "l00097" name = "l00097" ></ a >< span class = "lineno" > 97</ span > </ div >
< div class = "line" >< a id = "l00098" name = "l00098" ></ a >< span class = "lineno" > 98</ span > cv::Point3d r;</ div >
< div class = "line" >< a id = "l00099" name = "l00099" ></ a >< span class = "lineno" > 99</ span > < span class = "keywordflow" > if</ span > ( depth == CV_32F )</ div >
< div class = "line" >< a id = "l00100" name = "l00100" ></ a >< span class = "lineno" > 100</ span > {</ div >
< div class = "line" >< a id = "l00101" name = "l00101" ></ a >< span class = "lineno" > 101</ span > r.x = src-> data.fl[0];</ div >
< div class = "line" >< a id = "l00102" name = "l00102" ></ a >< span class = "lineno" > 102</ span > r.y = src-> data.fl[step];</ div >
< div class = "line" >< a id = "l00103" name = "l00103" ></ a >< span class = "lineno" > 103</ span > r.z = src-> data.fl[step*2];</ div >
< div class = "line" >< a id = "l00104" name = "l00104" ></ a >< span class = "lineno" > 104</ span > }</ div >
< div class = "line" >< a id = "l00105" name = "l00105" ></ a >< span class = "lineno" > 105</ span > < span class = "keywordflow" > else</ span ></ div >
< div class = "line" >< a id = "l00106" name = "l00106" ></ a >< span class = "lineno" > 106</ span > {</ div >
< div class = "line" >< a id = "l00107" name = "l00107" ></ a >< span class = "lineno" > 107</ span > r.x = src-> data.db[0];</ div >
< div class = "line" >< a id = "l00108" name = "l00108" ></ a >< span class = "lineno" > 108</ span > r.y = src-> data.db[step];</ div >
< div class = "line" >< a id = "l00109" name = "l00109" ></ a >< span class = "lineno" > 109</ span > r.z = src-> data.db[step*2];</ div >
< div class = "line" >< a id = "l00110" name = "l00110" ></ a >< span class = "lineno" > 110</ span > }</ div >
< div class = "line" >< a id = "l00111" name = "l00111" ></ a >< span class = "lineno" > 111</ span > </ div >
< div class = "line" >< a id = "l00112" name = "l00112" ></ a >< span class = "lineno" > 112</ span > < span class = "keywordtype" > double</ span > theta = cv::norm(r);</ div >
< div class = "line" >< a id = "l00113" name = "l00113" ></ a >< span class = "lineno" > 113</ span > </ div >
< div class = "line" >< a id = "l00114" name = "l00114" ></ a >< span class = "lineno" > 114</ span > < span class = "keywordflow" > if</ span > ( theta < DBL_EPSILON )</ div >
< div class = "line" >< a id = "l00115" name = "l00115" ></ a >< span class = "lineno" > 115</ span > {</ div >
< div class = "line" >< a id = "l00116" name = "l00116" ></ a >< span class = "lineno" > 116</ span > cvSetIdentity( dst );</ div >
< div class = "line" >< a id = "l00117" name = "l00117" ></ a >< span class = "lineno" > 117</ span > </ div >
< div class = "line" >< a id = "l00118" name = "l00118" ></ a >< span class = "lineno" > 118</ span > < span class = "keywordflow" > if</ span > ( jacobian )</ div >
< div class = "line" >< a id = "l00119" name = "l00119" ></ a >< span class = "lineno" > 119</ span > {</ div >
< div class = "line" >< a id = "l00120" name = "l00120" ></ a >< span class = "lineno" > 120</ span > memset( J, 0, < span class = "keyword" > sizeof</ span > (J) );</ div >
< div class = "line" >< a id = "l00121" name = "l00121" ></ a >< span class = "lineno" > 121</ span > J[5] = J[15] = J[19] = -1;</ div >
< div class = "line" >< a id = "l00122" name = "l00122" ></ a >< span class = "lineno" > 122</ span > J[7] = J[11] = J[21] = 1;</ div >
< div class = "line" >< a id = "l00123" name = "l00123" ></ a >< span class = "lineno" > 123</ span > }</ div >
< div class = "line" >< a id = "l00124" name = "l00124" ></ a >< span class = "lineno" > 124</ span > }</ div >
< div class = "line" >< a id = "l00125" name = "l00125" ></ a >< span class = "lineno" > 125</ span > < span class = "keywordflow" > else</ span ></ div >
< div class = "line" >< a id = "l00126" name = "l00126" ></ a >< span class = "lineno" > 126</ span > {</ div >
< div class = "line" >< a id = "l00127" name = "l00127" ></ a >< span class = "lineno" > 127</ span > < span class = "keywordtype" > double</ span > c = cos(theta);</ div >
< div class = "line" >< a id = "l00128" name = "l00128" ></ a >< span class = "lineno" > 128</ span > < span class = "keywordtype" > double</ span > s = sin(theta);</ div >
< div class = "line" >< a id = "l00129" name = "l00129" ></ a >< span class = "lineno" > 129</ span > < span class = "keywordtype" > double</ span > c1 = 1. - c;</ div >
< div class = "line" >< a id = "l00130" name = "l00130" ></ a >< span class = "lineno" > 130</ span > < span class = "keywordtype" > double</ span > itheta = theta ? 1./theta : 0.;</ div >
< div class = "line" >< a id = "l00131" name = "l00131" ></ a >< span class = "lineno" > 131</ span > </ div >
< div class = "line" >< a id = "l00132" name = "l00132" ></ a >< span class = "lineno" > 132</ span > r *= itheta;</ div >
< div class = "line" >< a id = "l00133" name = "l00133" ></ a >< span class = "lineno" > 133</ span > </ div >
< div class = "line" >< a id = "l00134" name = "l00134" ></ a >< span class = "lineno" > 134</ span > cv::Matx33d rrt( r.x*r.x, r.x*r.y, r.x*r.z, r.x*r.y, r.y*r.y, r.y*r.z, r.x*r.z, r.y*r.z, r.z*r.z );</ div >
< div class = "line" >< a id = "l00135" name = "l00135" ></ a >< span class = "lineno" > 135</ span > cv::Matx33d r_x( 0, -r.z, r.y,</ div >
< div class = "line" >< a id = "l00136" name = "l00136" ></ a >< span class = "lineno" > 136</ span > r.z, 0, -r.x,</ div >
< div class = "line" >< a id = "l00137" name = "l00137" ></ a >< span class = "lineno" > 137</ span > -r.y, r.x, 0 );</ div >
< div class = "line" >< a id = "l00138" name = "l00138" ></ a >< span class = "lineno" > 138</ span > </ div >
< div class = "line" >< a id = "l00139" name = "l00139" ></ a >< span class = "lineno" > 139</ span > < span class = "comment" > // R = cos(theta)*I + (1 - cos(theta))*r*rT + sin(theta)*[r_x]</ span ></ div >
< div class = "line" >< a id = "l00140" name = "l00140" ></ a >< span class = "lineno" > 140</ span > cv::Matx33d R = c*cv::Matx33d::eye() + c1*rrt + s*r_x;</ div >
< div class = "line" >< a id = "l00141" name = "l00141" ></ a >< span class = "lineno" > 141</ span > </ div >
< div class = "line" >< a id = "l00142" name = "l00142" ></ a >< span class = "lineno" > 142</ span > cv::Mat(R).convertTo(cv::cvarrToMat(dst), dst-> type);</ div >
< div class = "line" >< a id = "l00143" name = "l00143" ></ a >< span class = "lineno" > 143</ span > </ div >
< div class = "line" >< a id = "l00144" name = "l00144" ></ a >< span class = "lineno" > 144</ span > < span class = "keywordflow" > if</ span > ( jacobian )</ div >
< div class = "line" >< a id = "l00145" name = "l00145" ></ a >< span class = "lineno" > 145</ span > {</ div >
< div class = "line" >< a id = "l00146" name = "l00146" ></ a >< span class = "lineno" > 146</ span > < span class = "keyword" > const</ span > < span class = "keywordtype" > double</ span > I[] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 };</ div >
< div class = "line" >< a id = "l00147" name = "l00147" ></ a >< span class = "lineno" > 147</ span > < span class = "keywordtype" > double</ span > drrt[] = { r.x+r.x, r.y, r.z, r.y, 0, 0, r.z, 0, 0,</ div >
< div class = "line" >< a id = "l00148" name = "l00148" ></ a >< span class = "lineno" > 148</ span > 0, r.x, 0, r.x, r.y+r.y, r.z, 0, r.z, 0,</ div >
< div class = "line" >< a id = "l00149" name = "l00149" ></ a >< span class = "lineno" > 149</ span > 0, 0, r.x, 0, 0, r.y, r.x, r.y, r.z+r.z };</ div >
< div class = "line" >< a id = "l00150" name = "l00150" ></ a >< span class = "lineno" > 150</ span > < span class = "keywordtype" > double</ span > d_r_x_[] = { 0, 0, 0, 0, 0, -1, 0, 1, 0,</ div >
< div class = "line" >< a id = "l00151" name = "l00151" ></ a >< span class = "lineno" > 151</ span > 0, 0, 1, 0, 0, 0, -1, 0, 0,</ div >
< div class = "line" >< a id = "l00152" name = "l00152" ></ a >< span class = "lineno" > 152</ span > 0, -1, 0, 1, 0, 0, 0, 0, 0 };</ div >
< div class = "line" >< a id = "l00153" name = "l00153" ></ a >< span class = "lineno" > 153</ span > < span class = "keywordflow" > for</ span > ( i = 0; i < 3; i++ )</ div >
< div class = "line" >< a id = "l00154" name = "l00154" ></ a >< span class = "lineno" > 154</ span > {</ div >
< div class = "line" >< a id = "l00155" name = "l00155" ></ a >< span class = "lineno" > 155</ span > < span class = "keywordtype" > double</ span > ri = i == 0 ? r.x : i == 1 ? r.y : r.z;</ div >
< div class = "line" >< a id = "l00156" name = "l00156" ></ a >< span class = "lineno" > 156</ span > < span class = "keywordtype" > double</ span > a0 = -s*ri, a1 = (s - 2*c1*itheta)*ri, a2 = c1*itheta;</ div >
< div class = "line" >< a id = "l00157" name = "l00157" ></ a >< span class = "lineno" > 157</ span > < span class = "keywordtype" > double</ span > a3 = (c - s*itheta)*ri, a4 = s*itheta;</ div >
< div class = "line" >< a id = "l00158" name = "l00158" ></ a >< span class = "lineno" > 158</ span > < span class = "keywordflow" > for</ span > ( k = 0; k < 9; k++ )</ div >
< div class = "line" >< a id = "l00159" name = "l00159" ></ a >< span class = "lineno" > 159</ span > J[i*9+k] = a0*I[k] + a1*rrt.val[k] + a2*drrt[i*9+k] +</ div >
< div class = "line" >< a id = "l00160" name = "l00160" ></ a >< span class = "lineno" > 160</ span > a3*r_x.val[k] + a4*d_r_x_[i*9+k];</ div >
< div class = "line" >< a id = "l00161" name = "l00161" ></ a >< span class = "lineno" > 161</ span > }</ div >
< div class = "line" >< a id = "l00162" name = "l00162" ></ a >< span class = "lineno" > 162</ span > }</ div >
< div class = "line" >< a id = "l00163" name = "l00163" ></ a >< span class = "lineno" > 163</ span > }</ div >
< div class = "line" >< a id = "l00164" name = "l00164" ></ a >< span class = "lineno" > 164</ span > }</ div >
< div class = "line" >< a id = "l00165" name = "l00165" ></ a >< span class = "lineno" > 165</ span > < span class = "keywordflow" > else</ span > < span class = "keywordflow" > if</ span > ( src-> cols == 3 && src-> rows == 3 )</ div >
< div class = "line" >< a id = "l00166" name = "l00166" ></ a >< span class = "lineno" > 166</ span > {</ div >
< div class = "line" >< a id = "l00167" name = "l00167" ></ a >< span class = "lineno" > 167</ span > cv::Matx33d U, Vt;</ div >
< div class = "line" >< a id = "l00168" name = "l00168" ></ a >< span class = "lineno" > 168</ span > cv::Vec3d W;</ div >
< div class = "line" >< a id = "l00169" name = "l00169" ></ a >< span class = "lineno" > 169</ span > < span class = "keywordtype" > double</ span > theta, s, c;</ div >
< div class = "line" >< a id = "l00170" name = "l00170" ></ a >< span class = "lineno" > 170</ span > < span class = "keywordtype" > int</ span > step = dst-> rows > 1 ? dst-> step / elem_size : 1;</ div >
< div class = "line" >< a id = "l00171" name = "l00171" ></ a >< span class = "lineno" > 171</ span > </ div >
< div class = "line" >< a id = "l00172" name = "l00172" ></ a >< span class = "lineno" > 172</ span > < span class = "keywordflow" > if</ span > ( (dst-> rows != 1 || dst-> cols*CV_MAT_CN(dst-> type) != 3) && </ div >
< div class = "line" >< a id = "l00173" name = "l00173" ></ a >< span class = "lineno" > 173</ span > (dst-> rows != 3 || dst-> cols != 1 || CV_MAT_CN(dst-> type) != 1))</ div >
< div class = "line" >< a id = "l00174" name = "l00174" ></ a >< span class = "lineno" > 174</ span > CV_Error( CV_StsBadSize, < span class = "stringliteral" > " Output matrix must be 1x3 or 3x1" </ span > );</ div >
< div class = "line" >< a id = "l00175" name = "l00175" ></ a >< span class = "lineno" > 175</ span > </ div >
< div class = "line" >< a id = "l00176" name = "l00176" ></ a >< span class = "lineno" > 176</ span > cv::Matx33d R = cv::cvarrToMat(src);</ div >
< div class = "line" >< a id = "l00177" name = "l00177" ></ a >< span class = "lineno" > 177</ span > </ div >
< div class = "line" >< a id = "l00178" name = "l00178" ></ a >< span class = "lineno" > 178</ span > < span class = "keywordflow" > if</ span > ( !cv::checkRange(R, < span class = "keyword" > true</ span > , NULL, -100, 100) )</ div >
< div class = "line" >< a id = "l00179" name = "l00179" ></ a >< span class = "lineno" > 179</ span > {</ div >
< div class = "line" >< a id = "l00180" name = "l00180" ></ a >< span class = "lineno" > 180</ span > cvZero(dst);</ div >
< div class = "line" >< a id = "l00181" name = "l00181" ></ a >< span class = "lineno" > 181</ span > < span class = "keywordflow" > if</ span > ( jacobian )</ div >
< div class = "line" >< a id = "l00182" name = "l00182" ></ a >< span class = "lineno" > 182</ span > cvZero(jacobian);</ div >
< div class = "line" >< a id = "l00183" name = "l00183" ></ a >< span class = "lineno" > 183</ span > < span class = "keywordflow" > return</ span > 0;</ div >
< div class = "line" >< a id = "l00184" name = "l00184" ></ a >< span class = "lineno" > 184</ span > }</ div >
< div class = "line" >< a id = "l00185" name = "l00185" ></ a >< span class = "lineno" > 185</ span > </ div >
< div class = "line" >< a id = "l00186" name = "l00186" ></ a >< span class = "lineno" > 186</ span > cv::SVD::compute(R, W, U, Vt);</ div >
< div class = "line" >< a id = "l00187" name = "l00187" ></ a >< span class = "lineno" > 187</ span > R = U*Vt;</ div >
< div class = "line" >< a id = "l00188" name = "l00188" ></ a >< span class = "lineno" > 188</ span > </ div >
< div class = "line" >< a id = "l00189" name = "l00189" ></ a >< span class = "lineno" > 189</ span > cv::Point3d r(R(2, 1) - R(1, 2), R(0, 2) - R(2, 0), R(1, 0) - R(0, 1));</ div >
< div class = "line" >< a id = "l00190" name = "l00190" ></ a >< span class = "lineno" > 190</ span > </ div >
< div class = "line" >< a id = "l00191" name = "l00191" ></ a >< span class = "lineno" > 191</ span > s = std::sqrt((r.x*r.x + r.y*r.y + r.z*r.z)*0.25);</ div >
< div class = "line" >< a id = "l00192" name = "l00192" ></ a >< span class = "lineno" > 192</ span > c = (R(0, 0) + R(1, 1) + R(2, 2) - 1)*0.5;</ div >
< div class = "line" >< a id = "l00193" name = "l00193" ></ a >< span class = "lineno" > 193</ span > c = c > 1. ? 1. : c < -1. ? -1. : c;</ div >
< div class = "line" >< a id = "l00194" name = "l00194" ></ a >< span class = "lineno" > 194</ span > theta = acos(c);</ div >
< div class = "line" >< a id = "l00195" name = "l00195" ></ a >< span class = "lineno" > 195</ span > </ div >
< div class = "line" >< a id = "l00196" name = "l00196" ></ a >< span class = "lineno" > 196</ span > < span class = "keywordflow" > if</ span > ( s < 1e-5 )</ div >
< div class = "line" >< a id = "l00197" name = "l00197" ></ a >< span class = "lineno" > 197</ span > {</ div >
< div class = "line" >< a id = "l00198" name = "l00198" ></ a >< span class = "lineno" > 198</ span > < span class = "keywordtype" > double</ span > t;</ div >
< div class = "line" >< a id = "l00199" name = "l00199" ></ a >< span class = "lineno" > 199</ span > </ div >
< div class = "line" >< a id = "l00200" name = "l00200" ></ a >< span class = "lineno" > 200</ span > < span class = "keywordflow" > if</ span > ( c > 0 )</ div >
< div class = "line" >< a id = "l00201" name = "l00201" ></ a >< span class = "lineno" > 201</ span > r = cv::Point3d(0, 0, 0);</ div >
< div class = "line" >< a id = "l00202" name = "l00202" ></ a >< span class = "lineno" > 202</ span > < span class = "keywordflow" > else</ span ></ div >
< div class = "line" >< a id = "l00203" name = "l00203" ></ a >< span class = "lineno" > 203</ span > {</ div >
< div class = "line" >< a id = "l00204" name = "l00204" ></ a >< span class = "lineno" > 204</ span > t = (R(0, 0) + 1)*0.5;</ div >
< div class = "line" >< a id = "l00205" name = "l00205" ></ a >< span class = "lineno" > 205</ span > r.x = std::sqrt(MAX(t,0.));</ div >
< div class = "line" >< a id = "l00206" name = "l00206" ></ a >< span class = "lineno" > 206</ span > t = (R(1, 1) + 1)*0.5;</ div >
< div class = "line" >< a id = "l00207" name = "l00207" ></ a >< span class = "lineno" > 207</ span > r.y = std::sqrt(MAX(t,0.))*(R(0, 1) < 0 ? -1. : 1.);</ div >
< div class = "line" >< a id = "l00208" name = "l00208" ></ a >< span class = "lineno" > 208</ span > t = (R(2, 2) + 1)*0.5;</ div >
< div class = "line" >< a id = "l00209" name = "l00209" ></ a >< span class = "lineno" > 209</ span > r.z = std::sqrt(MAX(t,0.))*(R(0, 2) < 0 ? -1. : 1.);</ div >
< div class = "line" >< a id = "l00210" name = "l00210" ></ a >< span class = "lineno" > 210</ span > < span class = "keywordflow" > if</ span > ( fabs(r.x) < fabs(r.y) && fabs(r.x) < fabs(r.z) && (R(1, 2) > 0) != (r.y*r.z > 0) )</ div >
< div class = "line" >< a id = "l00211" name = "l00211" ></ a >< span class = "lineno" > 211</ span > r.z = -r.z;</ div >
< div class = "line" >< a id = "l00212" name = "l00212" ></ a >< span class = "lineno" > 212</ span > theta /= cv::norm(r);</ div >
< div class = "line" >< a id = "l00213" name = "l00213" ></ a >< span class = "lineno" > 213</ span > r *= theta;</ div >
< div class = "line" >< a id = "l00214" name = "l00214" ></ a >< span class = "lineno" > 214</ span > }</ div >
< div class = "line" >< a id = "l00215" name = "l00215" ></ a >< span class = "lineno" > 215</ span > </ div >
< div class = "line" >< a id = "l00216" name = "l00216" ></ a >< span class = "lineno" > 216</ span > < span class = "keywordflow" > if</ span > ( jacobian )</ div >
< div class = "line" >< a id = "l00217" name = "l00217" ></ a >< span class = "lineno" > 217</ span > {</ div >
< div class = "line" >< a id = "l00218" name = "l00218" ></ a >< span class = "lineno" > 218</ span > memset( J, 0, < span class = "keyword" > sizeof</ span > (J) );</ div >
< div class = "line" >< a id = "l00219" name = "l00219" ></ a >< span class = "lineno" > 219</ span > < span class = "keywordflow" > if</ span > ( c > 0 )</ div >
< div class = "line" >< a id = "l00220" name = "l00220" ></ a >< span class = "lineno" > 220</ span > {</ div >
< div class = "line" >< a id = "l00221" name = "l00221" ></ a >< span class = "lineno" > 221</ span > J[5] = J[15] = J[19] = -0.5;</ div >
< div class = "line" >< a id = "l00222" name = "l00222" ></ a >< span class = "lineno" > 222</ span > J[7] = J[11] = J[21] = 0.5;</ div >
< div class = "line" >< a id = "l00223" name = "l00223" ></ a >< span class = "lineno" > 223</ span > }</ div >
< div class = "line" >< a id = "l00224" name = "l00224" ></ a >< span class = "lineno" > 224</ span > }</ div >
< div class = "line" >< a id = "l00225" name = "l00225" ></ a >< span class = "lineno" > 225</ span > }</ div >
< div class = "line" >< a id = "l00226" name = "l00226" ></ a >< span class = "lineno" > 226</ span > < span class = "keywordflow" > else</ span ></ div >
< div class = "line" >< a id = "l00227" name = "l00227" ></ a >< span class = "lineno" > 227</ span > {</ div >
< div class = "line" >< a id = "l00228" name = "l00228" ></ a >< span class = "lineno" > 228</ span > < span class = "keywordtype" > double</ span > vth = 1/(2*s);</ div >
< div class = "line" >< a id = "l00229" name = "l00229" ></ a >< span class = "lineno" > 229</ span > </ div >
< div class = "line" >< a id = "l00230" name = "l00230" ></ a >< span class = "lineno" > 230</ span > < span class = "keywordflow" > if</ span > ( jacobian )</ div >
< div class = "line" >< a id = "l00231" name = "l00231" ></ a >< span class = "lineno" > 231</ span > {</ div >
< div class = "line" >< a id = "l00232" name = "l00232" ></ a >< span class = "lineno" > 232</ span > < span class = "keywordtype" > double</ span > t, dtheta_dtr = -1./s;</ div >
< div class = "line" >< a id = "l00233" name = "l00233" ></ a >< span class = "lineno" > 233</ span > < span class = "comment" > // var1 = [vth;theta]</ span ></ div >
< div class = "line" >< a id = "l00234" name = "l00234" ></ a >< span class = "lineno" > 234</ span > < span class = "comment" > // var = [om1;var1] = [om1;vth;theta]</ span ></ div >
< div class = "line" >< a id = "l00235" name = "l00235" ></ a >< span class = "lineno" > 235</ span > < span class = "keywordtype" > double</ span > dvth_dtheta = -vth*c/s;</ div >
< div class = "line" >< a id = "l00236" name = "l00236" ></ a >< span class = "lineno" > 236</ span > < span class = "keywordtype" > double</ span > d1 = 0.5*dvth_dtheta*dtheta_dtr;</ div >
< div class = "line" >< a id = "l00237" name = "l00237" ></ a >< span class = "lineno" > 237</ span > < span class = "keywordtype" > double</ span > d2 = 0.5*dtheta_dtr;</ div >
< div class = "line" >< a id = "l00238" name = "l00238" ></ a >< span class = "lineno" > 238</ span > < span class = "comment" > // dvar1/dR = dvar1/dtheta*dtheta/dR = [dvth/dtheta; 1] * dtheta/dtr * dtr/dR</ span ></ div >
< div class = "line" >< a id = "l00239" name = "l00239" ></ a >< span class = "lineno" > 239</ span > < span class = "keywordtype" > double</ span > dvardR[5*9] =</ div >
< div class = "line" >< a id = "l00240" name = "l00240" ></ a >< span class = "lineno" > 240</ span > {</ div >
< div class = "line" >< a id = "l00241" name = "l00241" ></ a >< span class = "lineno" > 241</ span > 0, 0, 0, 0, 0, 1, 0, -1, 0,</ div >
< div class = "line" >< a id = "l00242" name = "l00242" ></ a >< span class = "lineno" > 242</ span > 0, 0, -1, 0, 0, 0, 1, 0, 0,</ div >
< div class = "line" >< a id = "l00243" name = "l00243" ></ a >< span class = "lineno" > 243</ span > 0, 1, 0, -1, 0, 0, 0, 0, 0,</ div >
< div class = "line" >< a id = "l00244" name = "l00244" ></ a >< span class = "lineno" > 244</ span > d1, 0, 0, 0, d1, 0, 0, 0, d1,</ div >
< div class = "line" >< a id = "l00245" name = "l00245" ></ a >< span class = "lineno" > 245</ span > d2, 0, 0, 0, d2, 0, 0, 0, d2</ div >
< div class = "line" >< a id = "l00246" name = "l00246" ></ a >< span class = "lineno" > 246</ span > };</ div >
< div class = "line" >< a id = "l00247" name = "l00247" ></ a >< span class = "lineno" > 247</ span > < span class = "comment" > // var2 = [om;theta]</ span ></ div >
< div class = "line" >< a id = "l00248" name = "l00248" ></ a >< span class = "lineno" > 248</ span > < span class = "keywordtype" > double</ span > dvar2dvar[] =</ div >
< div class = "line" >< a id = "l00249" name = "l00249" ></ a >< span class = "lineno" > 249</ span > {</ div >
< div class = "line" >< a id = "l00250" name = "l00250" ></ a >< span class = "lineno" > 250</ span > vth, 0, 0, r.x, 0,</ div >
< div class = "line" >< a id = "l00251" name = "l00251" ></ a >< span class = "lineno" > 251</ span > 0, vth, 0, r.y, 0,</ div >
< div class = "line" >< a id = "l00252" name = "l00252" ></ a >< span class = "lineno" > 252</ span > 0, 0, vth, r.z, 0,</ div >
< div class = "line" >< a id = "l00253" name = "l00253" ></ a >< span class = "lineno" > 253</ span > 0, 0, 0, 0, 1</ div >
< div class = "line" >< a id = "l00254" name = "l00254" ></ a >< span class = "lineno" > 254</ span > };</ div >
< div class = "line" >< a id = "l00255" name = "l00255" ></ a >< span class = "lineno" > 255</ span > < span class = "keywordtype" > double</ span > domegadvar2[] =</ div >
< div class = "line" >< a id = "l00256" name = "l00256" ></ a >< span class = "lineno" > 256</ span > {</ div >
< div class = "line" >< a id = "l00257" name = "l00257" ></ a >< span class = "lineno" > 257</ span > theta, 0, 0, r.x*vth,</ div >
< div class = "line" >< a id = "l00258" name = "l00258" ></ a >< span class = "lineno" > 258</ span > 0, theta, 0, r.y*vth,</ div >
< div class = "line" >< a id = "l00259" name = "l00259" ></ a >< span class = "lineno" > 259</ span > 0, 0, theta, r.z*vth</ div >
< div class = "line" >< a id = "l00260" name = "l00260" ></ a >< span class = "lineno" > 260</ span > };</ div >
< div class = "line" >< a id = "l00261" name = "l00261" ></ a >< span class = "lineno" > 261</ span > </ div >
< div class = "line" >< a id = "l00262" name = "l00262" ></ a >< span class = "lineno" > 262</ span > CvMat _dvardR = cvMat( 5, 9, CV_64FC1, dvardR );</ div >
< div class = "line" >< a id = "l00263" name = "l00263" ></ a >< span class = "lineno" > 263</ span > CvMat _dvar2dvar = cvMat( 4, 5, CV_64FC1, dvar2dvar );</ div >
< div class = "line" >< a id = "l00264" name = "l00264" ></ a >< span class = "lineno" > 264</ span > CvMat _domegadvar2 = cvMat( 3, 4, CV_64FC1, domegadvar2 );</ div >
< div class = "line" >< a id = "l00265" name = "l00265" ></ a >< span class = "lineno" > 265</ span > < span class = "keywordtype" > double</ span > t0[3*5];</ div >
< div class = "line" >< a id = "l00266" name = "l00266" ></ a >< span class = "lineno" > 266</ span > CvMat _t0 = cvMat( 3, 5, CV_64FC1, t0 );</ div >
< div class = "line" >< a id = "l00267" name = "l00267" ></ a >< span class = "lineno" > 267</ span > </ div >
< div class = "line" >< a id = "l00268" name = "l00268" ></ a >< span class = "lineno" > 268</ span > cvMatMul( & _domegadvar2, & _dvar2dvar, & _t0 );</ div >
< div class = "line" >< a id = "l00269" name = "l00269" ></ a >< span class = "lineno" > 269</ span > cvMatMul( & _t0, & _dvardR, & matJ );</ div >
< div class = "line" >< a id = "l00270" name = "l00270" ></ a >< span class = "lineno" > 270</ span > </ div >
< div class = "line" >< a id = "l00271" name = "l00271" ></ a >< span class = "lineno" > 271</ span > < span class = "comment" > // transpose every row of matJ (treat the rows as 3x3 matrices)</ span ></ div >
< div class = "line" >< a id = "l00272" name = "l00272" ></ a >< span class = "lineno" > 272</ span > CV_SWAP(J[1], J[3], t); CV_SWAP(J[2], J[6], t); CV_SWAP(J[5], J[7], t);</ div >
< div class = "line" >< a id = "l00273" name = "l00273" ></ a >< span class = "lineno" > 273</ span > CV_SWAP(J[10], J[12], t); CV_SWAP(J[11], J[15], t); CV_SWAP(J[14], J[16], t);</ div >
< div class = "line" >< a id = "l00274" name = "l00274" ></ a >< span class = "lineno" > 274</ span > CV_SWAP(J[19], J[21], t); CV_SWAP(J[20], J[24], t); CV_SWAP(J[23], J[25], t);</ div >
< div class = "line" >< a id = "l00275" name = "l00275" ></ a >< span class = "lineno" > 275</ span > }</ div >
< div class = "line" >< a id = "l00276" name = "l00276" ></ a >< span class = "lineno" > 276</ span > </ div >
< div class = "line" >< a id = "l00277" name = "l00277" ></ a >< span class = "lineno" > 277</ span > vth *= theta;</ div >
< div class = "line" >< a id = "l00278" name = "l00278" ></ a >< span class = "lineno" > 278</ span > r *= vth;</ div >
< div class = "line" >< a id = "l00279" name = "l00279" ></ a >< span class = "lineno" > 279</ span > }</ div >
< div class = "line" >< a id = "l00280" name = "l00280" ></ a >< span class = "lineno" > 280</ span > </ div >
< div class = "line" >< a id = "l00281" name = "l00281" ></ a >< span class = "lineno" > 281</ span > < span class = "keywordflow" > if</ span > ( depth == CV_32F )</ div >
< div class = "line" >< a id = "l00282" name = "l00282" ></ a >< span class = "lineno" > 282</ span > {</ div >
< div class = "line" >< a id = "l00283" name = "l00283" ></ a >< span class = "lineno" > 283</ span > dst-> data.fl[0] = (float)r.x;</ div >
< div class = "line" >< a id = "l00284" name = "l00284" ></ a >< span class = "lineno" > 284</ span > dst-> data.fl[step] = (float)r.y;</ div >
< div class = "line" >< a id = "l00285" name = "l00285" ></ a >< span class = "lineno" > 285</ span > dst-> data.fl[step*2] = (float)r.z;</ div >
< div class = "line" >< a id = "l00286" name = "l00286" ></ a >< span class = "lineno" > 286</ span > }</ div >
< div class = "line" >< a id = "l00287" name = "l00287" ></ a >< span class = "lineno" > 287</ span > < span class = "keywordflow" > else</ span ></ div >
< div class = "line" >< a id = "l00288" name = "l00288" ></ a >< span class = "lineno" > 288</ span > {</ div >
< div class = "line" >< a id = "l00289" name = "l00289" ></ a >< span class = "lineno" > 289</ span > dst-> data.db[0] = r.x;</ div >
< div class = "line" >< a id = "l00290" name = "l00290" ></ a >< span class = "lineno" > 290</ span > dst-> data.db[step] = r.y;</ div >
< div class = "line" >< a id = "l00291" name = "l00291" ></ a >< span class = "lineno" > 291</ span > dst-> data.db[step*2] = r.z;</ div >
< div class = "line" >< a id = "l00292" name = "l00292" ></ a >< span class = "lineno" > 292</ span > }</ div >
< div class = "line" >< a id = "l00293" name = "l00293" ></ a >< span class = "lineno" > 293</ span > }</ div >
< div class = "line" >< a id = "l00294" name = "l00294" ></ a >< span class = "lineno" > 294</ span > </ div >
< div class = "line" >< a id = "l00295" name = "l00295" ></ a >< span class = "lineno" > 295</ span > < span class = "keywordflow" > if</ span > ( jacobian )</ div >
< div class = "line" >< a id = "l00296" name = "l00296" ></ a >< span class = "lineno" > 296</ span > {</ div >
< div class = "line" >< a id = "l00297" name = "l00297" ></ a >< span class = "lineno" > 297</ span > < span class = "keywordflow" > if</ span > ( depth == CV_32F )</ div >
< div class = "line" >< a id = "l00298" name = "l00298" ></ a >< span class = "lineno" > 298</ span > {</ div >
< div class = "line" >< a id = "l00299" name = "l00299" ></ a >< span class = "lineno" > 299</ span > < span class = "keywordflow" > if</ span > ( jacobian-> rows == matJ.rows )</ div >
< div class = "line" >< a id = "l00300" name = "l00300" ></ a >< span class = "lineno" > 300</ span > cvConvert( & matJ, jacobian );</ div >
< div class = "line" >< a id = "l00301" name = "l00301" ></ a >< span class = "lineno" > 301</ span > < span class = "keywordflow" > else</ span ></ div >
< div class = "line" >< a id = "l00302" name = "l00302" ></ a >< span class = "lineno" > 302</ span > {</ div >
< div class = "line" >< a id = "l00303" name = "l00303" ></ a >< span class = "lineno" > 303</ span > < span class = "keywordtype" > float</ span > Jf[3*9];</ div >
< div class = "line" >< a id = "l00304" name = "l00304" ></ a >< span class = "lineno" > 304</ span > CvMat _Jf = cvMat( matJ.rows, matJ.cols, CV_32FC1, Jf );</ div >
< div class = "line" >< a id = "l00305" name = "l00305" ></ a >< span class = "lineno" > 305</ span > cvConvert( & matJ, & _Jf );</ div >
< div class = "line" >< a id = "l00306" name = "l00306" ></ a >< span class = "lineno" > 306</ span > cvTranspose( & _Jf, jacobian );</ div >
< div class = "line" >< a id = "l00307" name = "l00307" ></ a >< span class = "lineno" > 307</ span > }</ div >
< div class = "line" >< a id = "l00308" name = "l00308" ></ a >< span class = "lineno" > 308</ span > }</ div >
< div class = "line" >< a id = "l00309" name = "l00309" ></ a >< span class = "lineno" > 309</ span > < span class = "keywordflow" > else</ span > < span class = "keywordflow" > if</ span > ( jacobian-> rows == matJ.rows )</ div >
< div class = "line" >< a id = "l00310" name = "l00310" ></ a >< span class = "lineno" > 310</ span > cvCopy( & matJ, jacobian );</ div >
< div class = "line" >< a id = "l00311" name = "l00311" ></ a >< span class = "lineno" > 311</ span > < span class = "keywordflow" > else</ span ></ div >
< div class = "line" >< a id = "l00312" name = "l00312" ></ a >< span class = "lineno" > 312</ span > cvTranspose( & matJ, jacobian );</ div >
< div class = "line" >< a id = "l00313" name = "l00313" ></ a >< span class = "lineno" > 313</ span > }</ div >
< div class = "line" >< a id = "l00314" name = "l00314" ></ a >< span class = "lineno" > 314</ span > </ div >
< div class = "line" >< a id = "l00315" name = "l00315" ></ a >< span class = "lineno" > 315</ span > < span class = "keywordflow" > return</ span > 1;</ div >
< div class = "line" >< a id = "l00316" name = "l00316" ></ a >< span class = "lineno" > 316</ span > }</ div >
< div class = "line" >< a id = "l00317" name = "l00317" ></ a >< span class = "lineno" > 317</ span > </ div >
< div class = "line" >< a id = "l00318" name = "l00318" ></ a >< span class = "lineno" > 318</ span >< span class = "keyword" > template</ span > < < span class = "keyword" > typename</ span > FLOAT> </ div >
< div class = "line" >< a id = "l00319" name = "l00319" ></ a >< span class = "lineno" > 319</ span >< span class = "keywordtype" > void</ span > computeTiltProjectionMatrix(FLOAT tauX,</ div >
< div class = "line" >< a id = "l00320" name = "l00320" ></ a >< span class = "lineno" > 320</ span > FLOAT tauY,</ div >
< div class = "line" >< a id = "l00321" name = "l00321" ></ a >< span class = "lineno" > 321</ span > cv::Matx< FLOAT, 3, 3> * matTilt = 0,</ div >
< div class = "line" >< a id = "l00322" name = "l00322" ></ a >< span class = "lineno" > 322</ span > cv::Matx< FLOAT, 3, 3> * dMatTiltdTauX = 0,</ div >
< div class = "line" >< a id = "l00323" name = "l00323" ></ a >< span class = "lineno" > 323</ span > cv::Matx< FLOAT, 3, 3> * dMatTiltdTauY = 0,</ div >
< div class = "line" >< a id = "l00324" name = "l00324" ></ a >< span class = "lineno" > 324</ span > cv::Matx< FLOAT, 3, 3> * invMatTilt = 0)</ div >
< div class = "line" >< a id = "l00325" name = "l00325" ></ a >< span class = "lineno" > 325</ span > {</ div >
< div class = "line" >< a id = "l00326" name = "l00326" ></ a >< span class = "lineno" > 326</ span > FLOAT cTauX = cos(tauX);</ div >
< div class = "line" >< a id = "l00327" name = "l00327" ></ a >< span class = "lineno" > 327</ span > FLOAT sTauX = sin(tauX);</ div >
< div class = "line" >< a id = "l00328" name = "l00328" ></ a >< span class = "lineno" > 328</ span > FLOAT cTauY = cos(tauY);</ div >
< div class = "line" >< a id = "l00329" name = "l00329" ></ a >< span class = "lineno" > 329</ span > FLOAT sTauY = sin(tauY);</ div >
< div class = "line" >< a id = "l00330" name = "l00330" ></ a >< span class = "lineno" > 330</ span > cv::Matx< FLOAT, 3, 3> matRotX = cv::Matx< FLOAT, 3, 3> (1,0,0,0,cTauX,sTauX,0,-sTauX,cTauX);</ div >
< div class = "line" >< a id = "l00331" name = "l00331" ></ a >< span class = "lineno" > 331</ span > cv::Matx< FLOAT, 3, 3> matRotY = cv::Matx< FLOAT, 3, 3> (cTauY,0,-sTauY,0,1,0,sTauY,0,cTauY);</ div >
< div class = "line" >< a id = "l00332" name = "l00332" ></ a >< span class = "lineno" > 332</ span > cv::Matx< FLOAT, 3, 3> matRotXY = matRotY * matRotX;</ div >
< div class = "line" >< a id = "l00333" name = "l00333" ></ a >< span class = "lineno" > 333</ span > cv::Matx< FLOAT, 3, 3> matProjZ = cv::Matx< FLOAT, 3, 3> (matRotXY(2,2),0,-matRotXY(0,2),0,matRotXY(2,2),-matRotXY(1,2),0,0,1);</ div >
< div class = "line" >< a id = "l00334" name = "l00334" ></ a >< span class = "lineno" > 334</ span > < span class = "keywordflow" > if</ span > (matTilt)</ div >
< div class = "line" >< a id = "l00335" name = "l00335" ></ a >< span class = "lineno" > 335</ span > {</ div >
< div class = "line" >< a id = "l00336" name = "l00336" ></ a >< span class = "lineno" > 336</ span > < span class = "comment" > // Matrix for trapezoidal distortion of tilted image sensor</ span ></ div >
< div class = "line" >< a id = "l00337" name = "l00337" ></ a >< span class = "lineno" > 337</ span > *matTilt = matProjZ * matRotXY;</ div >
< div class = "line" >< a id = "l00338" name = "l00338" ></ a >< span class = "lineno" > 338</ span > }</ div >
< div class = "line" >< a id = "l00339" name = "l00339" ></ a >< span class = "lineno" > 339</ span > < span class = "keywordflow" > if</ span > (dMatTiltdTauX)</ div >
< div class = "line" >< a id = "l00340" name = "l00340" ></ a >< span class = "lineno" > 340</ span > {</ div >
< div class = "line" >< a id = "l00341" name = "l00341" ></ a >< span class = "lineno" > 341</ span > < span class = "comment" > // Derivative with respect to tauX</ span ></ div >
< div class = "line" >< a id = "l00342" name = "l00342" ></ a >< span class = "lineno" > 342</ span > cv::Matx< FLOAT, 3, 3> dMatRotXYdTauX = matRotY * cv::Matx< FLOAT, 3, 3> (0,0,0,0,-sTauX,cTauX,0,-cTauX,-sTauX);</ div >
< div class = "line" >< a id = "l00343" name = "l00343" ></ a >< span class = "lineno" > 343</ span > cv::Matx< FLOAT, 3, 3> dMatProjZdTauX = cv::Matx< FLOAT, 3, 3> (dMatRotXYdTauX(2,2),0,-dMatRotXYdTauX(0,2),</ div >
< div class = "line" >< a id = "l00344" name = "l00344" ></ a >< span class = "lineno" > 344</ span > 0,dMatRotXYdTauX(2,2),-dMatRotXYdTauX(1,2),0,0,0);</ div >
< div class = "line" >< a id = "l00345" name = "l00345" ></ a >< span class = "lineno" > 345</ span > *dMatTiltdTauX = (matProjZ * dMatRotXYdTauX) + (dMatProjZdTauX * matRotXY);</ div >
< div class = "line" >< a id = "l00346" name = "l00346" ></ a >< span class = "lineno" > 346</ span > }</ div >
< div class = "line" >< a id = "l00347" name = "l00347" ></ a >< span class = "lineno" > 347</ span > < span class = "keywordflow" > if</ span > (dMatTiltdTauY)</ div >
< div class = "line" >< a id = "l00348" name = "l00348" ></ a >< span class = "lineno" > 348</ span > {</ div >
< div class = "line" >< a id = "l00349" name = "l00349" ></ a >< span class = "lineno" > 349</ span > < span class = "comment" > // Derivative with respect to tauY</ span ></ div >
< div class = "line" >< a id = "l00350" name = "l00350" ></ a >< span class = "lineno" > 350</ span > cv::Matx< FLOAT, 3, 3> dMatRotXYdTauY = cv::Matx< FLOAT, 3, 3> (-sTauY,0,-cTauY,0,0,0,cTauY,0,-sTauY) * matRotX;</ div >
< div class = "line" >< a id = "l00351" name = "l00351" ></ a >< span class = "lineno" > 351</ span > cv::Matx< FLOAT, 3, 3> dMatProjZdTauY = cv::Matx< FLOAT, 3, 3> (dMatRotXYdTauY(2,2),0,-dMatRotXYdTauY(0,2),</ div >
< div class = "line" >< a id = "l00352" name = "l00352" ></ a >< span class = "lineno" > 352</ span > 0,dMatRotXYdTauY(2,2),-dMatRotXYdTauY(1,2),0,0,0);</ div >
< div class = "line" >< a id = "l00353" name = "l00353" ></ a >< span class = "lineno" > 353</ span > *dMatTiltdTauY = (matProjZ * dMatRotXYdTauY) + (dMatProjZdTauY * matRotXY);</ div >
< div class = "line" >< a id = "l00354" name = "l00354" ></ a >< span class = "lineno" > 354</ span > }</ div >
< div class = "line" >< a id = "l00355" name = "l00355" ></ a >< span class = "lineno" > 355</ span > < span class = "keywordflow" > if</ span > (invMatTilt)</ div >
< div class = "line" >< a id = "l00356" name = "l00356" ></ a >< span class = "lineno" > 356</ span > {</ div >
< div class = "line" >< a id = "l00357" name = "l00357" ></ a >< span class = "lineno" > 357</ span > FLOAT inv = 1./matRotXY(2,2);</ div >
< div class = "line" >< a id = "l00358" name = "l00358" ></ a >< span class = "lineno" > 358</ span > cv::Matx< FLOAT, 3, 3> invMatProjZ = cv::Matx< FLOAT, 3, 3> (inv,0,inv*matRotXY(0,2),0,inv,inv*matRotXY(1,2),0,0,1);</ div >
< div class = "line" >< a id = "l00359" name = "l00359" ></ a >< span class = "lineno" > 359</ span > *invMatTilt = matRotXY.t()*invMatProjZ;</ div >
< div class = "line" >< a id = "l00360" name = "l00360" ></ a >< span class = "lineno" > 360</ span > }</ div >
< div class = "line" >< a id = "l00361" name = "l00361" ></ a >< span class = "lineno" > 361</ span > }</ div >
< div class = "line" >< a id = "l00362" name = "l00362" ></ a >< span class = "lineno" > 362</ span > </ div >
< div class = "line" >< a id = "l00363" name = "l00363" ></ a >< span class = "lineno" > 363</ span >< span class = "keywordtype" > void</ span > cvProjectPoints2Internal( < span class = "keyword" > const</ span > CvMat* objectPoints,</ div >
< div class = "line" >< a id = "l00364" name = "l00364" ></ a >< span class = "lineno" > 364</ span > < span class = "keyword" > const</ span > CvMat* r_vec,</ div >
< div class = "line" >< a id = "l00365" name = "l00365" ></ a >< span class = "lineno" > 365</ span > < span class = "keyword" > const</ span > CvMat* t_vec,</ div >
< div class = "line" >< a id = "l00366" name = "l00366" ></ a >< span class = "lineno" > 366</ span > < span class = "keyword" > const</ span > CvMat* A,</ div >
< div class = "line" >< a id = "l00367" name = "l00367" ></ a >< span class = "lineno" > 367</ span > < span class = "keyword" > const</ span > CvMat* distCoeffs,</ div >
< div class = "line" >< a id = "l00368" name = "l00368" ></ a >< span class = "lineno" > 368</ span > CvMat* imagePoints, CvMat* dpdr CV_DEFAULT(NULL),</ div >
< div class = "line" >< a id = "l00369" name = "l00369" ></ a >< span class = "lineno" > 369</ span > CvMat* dpdt CV_DEFAULT(NULL), CvMat* dpdf CV_DEFAULT(NULL),</ div >
< div class = "line" >< a id = "l00370" name = "l00370" ></ a >< span class = "lineno" > 370</ span > CvMat* dpdc CV_DEFAULT(NULL), CvMat* dpdk CV_DEFAULT(NULL),</ div >
< div class = "line" >< a id = "l00371" name = "l00371" ></ a >< span class = "lineno" > 371</ span > CvMat* dpdo CV_DEFAULT(NULL),</ div >
< div class = "line" >< a id = "l00372" name = "l00372" ></ a >< span class = "lineno" > 372</ span > < span class = "keywordtype" > double</ span > aspectRatio CV_DEFAULT(0) )</ div >
< div class = "line" >< a id = "l00373" name = "l00373" ></ a >< span class = "lineno" > 373</ span > {</ div >
< div class = "line" >< a id = "l00374" name = "l00374" ></ a >< span class = "lineno" > 374</ span > cv::Ptr< CvMat> matM, _m;</ div >
< div class = "line" >< a id = "l00375" name = "l00375" ></ a >< span class = "lineno" > 375</ span > cv::Ptr< CvMat> _dpdr, _dpdt, _dpdc, _dpdf, _dpdk;</ div >
< div class = "line" >< a id = "l00376" name = "l00376" ></ a >< span class = "lineno" > 376</ span > cv::Ptr< CvMat> _dpdo;</ div >
< div class = "line" >< a id = "l00377" name = "l00377" ></ a >< span class = "lineno" > 377</ span > </ div >
< div class = "line" >< a id = "l00378" name = "l00378" ></ a >< span class = "lineno" > 378</ span > < span class = "keywordtype" > int</ span > i, j, count;</ div >
< div class = "line" >< a id = "l00379" name = "l00379" ></ a >< span class = "lineno" > 379</ span > < span class = "keywordtype" > int</ span > calc_derivatives;</ div >
< div class = "line" >< a id = "l00380" name = "l00380" ></ a >< span class = "lineno" > 380</ span > < span class = "keyword" > const</ span > CvPoint3D64f* M;</ div >
< div class = "line" >< a id = "l00381" name = "l00381" ></ a >< span class = "lineno" > 381</ span > CvPoint2D64f* m;</ div >
< div class = "line" >< a id = "l00382" name = "l00382" ></ a >< span class = "lineno" > 382</ span > < span class = "keywordtype" > double</ span > r[3], R[9], dRdr[27], t[3], a[9], k[14] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, fx, fy, cx, cy;</ div >
< div class = "line" >< a id = "l00383" name = "l00383" ></ a >< span class = "lineno" > 383</ span > cv::Matx33d matTilt = cv::Matx33d::eye();</ div >
< div class = "line" >< a id = "l00384" name = "l00384" ></ a >< span class = "lineno" > 384</ span > cv::Matx33d dMatTiltdTauX(0,0,0,0,0,0,0,-1,0);</ div >
< div class = "line" >< a id = "l00385" name = "l00385" ></ a >< span class = "lineno" > 385</ span > cv::Matx33d dMatTiltdTauY(0,0,0,0,0,0,1,0,0);</ div >
< div class = "line" >< a id = "l00386" name = "l00386" ></ a >< span class = "lineno" > 386</ span > CvMat _r, _t, _a = cvMat( 3, 3, CV_64F, a ), _k;</ div >
< div class = "line" >< a id = "l00387" name = "l00387" ></ a >< span class = "lineno" > 387</ span > CvMat matR = cvMat( 3, 3, CV_64F, R ), _dRdr = cvMat( 3, 9, CV_64F, dRdr );</ div >
< div class = "line" >< a id = "l00388" name = "l00388" ></ a >< span class = "lineno" > 388</ span > < span class = "keywordtype" > double</ span > *dpdr_p = 0, *dpdt_p = 0, *dpdk_p = 0, *dpdf_p = 0, *dpdc_p = 0;</ div >
< div class = "line" >< a id = "l00389" name = "l00389" ></ a >< span class = "lineno" > 389</ span > < span class = "keywordtype" > double</ span > * dpdo_p = 0;</ div >
< div class = "line" >< a id = "l00390" name = "l00390" ></ a >< span class = "lineno" > 390</ span > < span class = "keywordtype" > int</ span > dpdr_step = 0, dpdt_step = 0, dpdk_step = 0, dpdf_step = 0, dpdc_step = 0;</ div >
< div class = "line" >< a id = "l00391" name = "l00391" ></ a >< span class = "lineno" > 391</ span > < span class = "keywordtype" > int</ span > dpdo_step = 0;</ div >
< div class = "line" >< a id = "l00392" name = "l00392" ></ a >< span class = "lineno" > 392</ span > < span class = "keywordtype" > bool</ span > fixedAspectRatio = aspectRatio > FLT_EPSILON;</ div >
< div class = "line" >< a id = "l00393" name = "l00393" ></ a >< span class = "lineno" > 393</ span > </ div >
< div class = "line" >< a id = "l00394" name = "l00394" ></ a >< span class = "lineno" > 394</ span > < span class = "keywordflow" > if</ span > ( !CV_IS_MAT(objectPoints) || !CV_IS_MAT(r_vec) ||</ div >
< div class = "line" >< a id = "l00395" name = "l00395" ></ a >< span class = "lineno" > 395</ span > !CV_IS_MAT(t_vec) || !CV_IS_MAT(A) ||</ div >
!CV_IS_MAT(imagePoints) )< div class = "line" >< a id = "l00397" name = "l00397" ></ a >< span class = "lineno" > 397</ span > CV_Error( CV_StsBadArg, < span class = "stringliteral" > " One of required arguments is not a valid matrix" </ span > );</ div >
< div class = "line" >< a id = "l00398" name = "l00398" ></ a >< span class = "lineno" > 398</ span > </ div >
< div class = "line" >< a id = "l00399" name = "l00399" ></ a >< span class = "lineno" > 399</ span > < span class = "keywordtype" > int</ span > total = objectPoints-> rows * objectPoints-> cols * CV_MAT_CN(objectPoints-> type);</ div >
< div class = "line" >< a id = "l00400" name = "l00400" ></ a >< span class = "lineno" > 400</ span > < span class = "keywordflow" > if</ span > (total % 3 != 0)</ div >
< div class = "line" >< a id = "l00401" name = "l00401" ></ a >< span class = "lineno" > 401</ span > {</ div >
< div class = "line" >< a id = "l00402" name = "l00402" ></ a >< span class = "lineno" > 402</ span > < span class = "comment" > //we have stopped support of homogeneous coordinates because it cause ambiguity in interpretation of the input data</ span ></ div >
< div class = "line" >< a id = "l00403" name = "l00403" ></ a >< span class = "lineno" > 403</ span > CV_Error( CV_StsBadArg, < span class = "stringliteral" > " Homogeneous coordinates are not supported" </ span > );</ div >
< div class = "line" >< a id = "l00404" name = "l00404" ></ a >< span class = "lineno" > 404</ span > }</ div >
< div class = "line" >< a id = "l00405" name = "l00405" ></ a >< span class = "lineno" > 405</ span > count = total / 3;</ div >
< div class = "line" >< a id = "l00406" name = "l00406" ></ a >< span class = "lineno" > 406</ span > </ div >
< div class = "line" >< a id = "l00407" name = "l00407" ></ a >< span class = "lineno" > 407</ span > < span class = "keywordflow" > if</ span > ( CV_IS_CONT_MAT(objectPoints-> type) && </ div >
< div class = "line" >< a id = "l00408" name = "l00408" ></ a >< span class = "lineno" > 408</ span > (CV_MAT_DEPTH(objectPoints-> type) == CV_32F || CV_MAT_DEPTH(objectPoints-> type) == CV_64F)&& </ div >
< div class = "line" >< a id = "l00409" name = "l00409" ></ a >< span class = "lineno" > 409</ span > ((objectPoints-> rows == 1 && CV_MAT_CN(objectPoints-> type) == 3) ||</ div >
< div class = "line" >< a id = "l00410" name = "l00410" ></ a >< span class = "lineno" > 410</ span > (objectPoints-> rows == count && CV_MAT_CN(objectPoints-> type)*objectPoints-> cols == 3) ||</ div >
< div class = "line" >< a id = "l00411" name = "l00411" ></ a >< span class = "lineno" > 411</ span > (objectPoints-> rows == 3 && CV_MAT_CN(objectPoints-> type) == 1 && objectPoints-> cols == count)))</ div >
< div class = "line" >< a id = "l00412" name = "l00412" ></ a >< span class = "lineno" > 412</ span > {</ div >
< div class = "line" >< a id = "l00413" name = "l00413" ></ a >< span class = "lineno" > 413</ span > matM.reset(cvCreateMat( objectPoints-> rows, objectPoints-> cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(objectPoints-> type)) ));</ div >
< div class = "line" >< a id = "l00414" name = "l00414" ></ a >< span class = "lineno" > 414</ span > cvConvert(objectPoints, matM);</ div >
< div class = "line" >< a id = "l00415" name = "l00415" ></ a >< span class = "lineno" > 415</ span > }</ div >
< div class = "line" >< a id = "l00416" name = "l00416" ></ a >< span class = "lineno" > 416</ span > < span class = "keywordflow" > else</ span ></ div >
< div class = "line" >< a id = "l00417" name = "l00417" ></ a >< span class = "lineno" > 417</ span > {</ div >
< div class = "line" >< a id = "l00418" name = "l00418" ></ a >< span class = "lineno" > 418</ span >< span class = "comment" > // matM = cvCreateMat( 1, count, CV_64FC3 );</ span ></ div >
< div class = "line" >< a id = "l00419" name = "l00419" ></ a >< span class = "lineno" > 419</ span >< span class = "comment" > // cvConvertPointsHomogeneous( objectPoints, matM );</ span ></ div >
< div class = "line" >< a id = "l00420" name = "l00420" ></ a >< span class = "lineno" > 420</ span > CV_Error( CV_StsBadArg, < span class = "stringliteral" > " Homogeneous coordinates are not supported" </ span > );</ div >
< div class = "line" >< a id = "l00421" name = "l00421" ></ a >< span class = "lineno" > 421</ span > }</ div >
< div class = "line" >< a id = "l00422" name = "l00422" ></ a >< span class = "lineno" > 422</ span > </ div >
< div class = "line" >< a id = "l00423" name = "l00423" ></ a >< span class = "lineno" > 423</ span > < span class = "keywordflow" > if</ span > ( CV_IS_CONT_MAT(imagePoints-> type) && </ div >
< div class = "line" >< a id = "l00424" name = "l00424" ></ a >< span class = "lineno" > 424</ span > (CV_MAT_DEPTH(imagePoints-> type) == CV_32F || CV_MAT_DEPTH(imagePoints-> type) == CV_64F) && </ div >
< div class = "line" >< a id = "l00425" name = "l00425" ></ a >< span class = "lineno" > 425</ span > ((imagePoints-> rows == 1 && CV_MAT_CN(imagePoints-> type) == 2) ||</ div >
< div class = "line" >< a id = "l00426" name = "l00426" ></ a >< span class = "lineno" > 426</ span > (imagePoints-> rows == count && CV_MAT_CN(imagePoints-> type)*imagePoints-> cols == 2) ||</ div >
< div class = "line" >< a id = "l00427" name = "l00427" ></ a >< span class = "lineno" > 427</ span > (imagePoints-> rows == 2 && CV_MAT_CN(imagePoints-> type) == 1 && imagePoints-> cols == count)))</ div >
< div class = "line" >< a id = "l00428" name = "l00428" ></ a >< span class = "lineno" > 428</ span > {</ div >
< div class = "line" >< a id = "l00429" name = "l00429" ></ a >< span class = "lineno" > 429</ span > _m.reset(cvCreateMat( imagePoints-> rows, imagePoints-> cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(imagePoints-> type)) ));</ div >
< div class = "line" >< a id = "l00430" name = "l00430" ></ a >< span class = "lineno" > 430</ span > cvConvert(imagePoints, _m);</ div >
< div class = "line" >< a id = "l00431" name = "l00431" ></ a >< span class = "lineno" > 431</ span > }</ div >
< div class = "line" >< a id = "l00432" name = "l00432" ></ a >< span class = "lineno" > 432</ span > < span class = "keywordflow" > else</ span ></ div >
< div class = "line" >< a id = "l00433" name = "l00433" ></ a >< span class = "lineno" > 433</ span > {</ div >
< div class = "line" >< a id = "l00434" name = "l00434" ></ a >< span class = "lineno" > 434</ span >< span class = "comment" > // _m = cvCreateMat( 1, count, CV_64FC2 );</ span ></ div >
< div class = "line" >< a id = "l00435" name = "l00435" ></ a >< span class = "lineno" > 435</ span > CV_Error( CV_StsBadArg, < span class = "stringliteral" > " Homogeneous coordinates are not supported" </ span > );</ div >
< div class = "line" >< a id = "l00436" name = "l00436" ></ a >< span class = "lineno" > 436</ span > }</ div >
< div class = "line" >< a id = "l00437" name = "l00437" ></ a >< span class = "lineno" > 437</ span > </ div >
< div class = "line" >< a id = "l00438" name = "l00438" ></ a >< span class = "lineno" > 438</ span > M = (CvPoint3D64f*)matM-> data.db;</ div >
< div class = "line" >< a id = "l00439" name = "l00439" ></ a >< span class = "lineno" > 439</ span > m = (CvPoint2D64f*)_m-> data.db;</ div >
< div class = "line" >< a id = "l00440" name = "l00440" ></ a >< span class = "lineno" > 440</ span > </ div >
< div class = "line" >< a id = "l00441" name = "l00441" ></ a >< span class = "lineno" > 441</ span > < span class = "keywordflow" > if</ span > ( (CV_MAT_DEPTH(r_vec-> type) != CV_64F && CV_MAT_DEPTH(r_vec-> type) != CV_32F) ||</ div >
< div class = "line" >< a id = "l00442" name = "l00442" ></ a >< span class = "lineno" > 442</ span > (((r_vec-> rows != 1 && r_vec-> cols != 1) ||</ div >
< div class = "line" >< a id = "l00443" name = "l00443" ></ a >< span class = "lineno" > 443</ span > r_vec-> rows*r_vec-> cols*CV_MAT_CN(r_vec-> type) != 3) && </ div >
< div class = "line" >< a id = "l00444" name = "l00444" ></ a >< span class = "lineno" > 444</ span > ((r_vec-> rows != 3 && r_vec-> cols != 3) || CV_MAT_CN(r_vec-> type) != 1)))</ div >
< div class = "line" >< a id = "l00445" name = "l00445" ></ a >< span class = "lineno" > 445</ span > CV_Error( CV_StsBadArg, < span class = "stringliteral" > " Rotation must be represented by 1x3 or 3x1 " </ span ></ div >
< div class = "line" >< a id = "l00446" name = "l00446" ></ a >< span class = "lineno" > 446</ span > < span class = "stringliteral" > " floating-point rotation vector, or 3x3 rotation matrix" </ span > );</ div >
< div class = "line" >< a id = "l00447" name = "l00447" ></ a >< span class = "lineno" > 447</ span > </ div >
< div class = "line" >< a id = "l00448" name = "l00448" ></ a >< span class = "lineno" > 448</ span > < span class = "keywordflow" > if</ span > ( r_vec-> rows == 3 && r_vec-> cols == 3 )</ div >
< div class = "line" >< a id = "l00449" name = "l00449" ></ a >< span class = "lineno" > 449</ span > {</ div >
< div class = "line" >< a id = "l00450" name = "l00450" ></ a >< span class = "lineno" > 450</ span > _r = cvMat( 3, 1, CV_64FC1, r );</ div >
< div class = "line" >< a id = "l00451" name = "l00451" ></ a >< span class = "lineno" > 451</ span > cvRodrigues2( r_vec, & _r );</ div >
< div class = "line" >< a id = "l00452" name = "l00452" ></ a >< span class = "lineno" > 452</ span > cvRodrigues2( & _r, & matR, & _dRdr );</ div >
< div class = "line" >< a id = "l00453" name = "l00453" ></ a >< span class = "lineno" > 453</ span > cvCopy( r_vec, & matR );</ div >
< div class = "line" >< a id = "l00454" name = "l00454" ></ a >< span class = "lineno" > 454</ span > }</ div >
< div class = "line" >< a id = "l00455" name = "l00455" ></ a >< span class = "lineno" > 455</ span > < span class = "keywordflow" > else</ span ></ div >
< div class = "line" >< a id = "l00456" name = "l00456" ></ a >< span class = "lineno" > 456</ span > {</ div >
< div class = "line" >< a id = "l00457" name = "l00457" ></ a >< span class = "lineno" > 457</ span > _r = cvMat( r_vec-> rows, r_vec-> cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(r_vec-> type)), r );</ div >
< div class = "line" >< a id = "l00458" name = "l00458" ></ a >< span class = "lineno" > 458</ span > cvConvert( r_vec, & _r );</ div >
< div class = "line" >< a id = "l00459" name = "l00459" ></ a >< span class = "lineno" > 459</ span > cvRodrigues2( & _r, & matR, & _dRdr );</ div >
< div class = "line" >< a id = "l00460" name = "l00460" ></ a >< span class = "lineno" > 460</ span > }</ div >
< div class = "line" >< a id = "l00461" name = "l00461" ></ a >< span class = "lineno" > 461</ span > </ div >
< div class = "line" >< a id = "l00462" name = "l00462" ></ a >< span class = "lineno" > 462</ span > < span class = "keywordflow" > if</ span > ( (CV_MAT_DEPTH(t_vec-> type) != CV_64F && CV_MAT_DEPTH(t_vec-> type) != CV_32F) ||</ div >
< div class = "line" >< a id = "l00463" name = "l00463" ></ a >< span class = "lineno" > 463</ span > (t_vec-> rows != 1 && t_vec-> cols != 1) ||</ div >
< div class = "line" >< a id = "l00464" name = "l00464" ></ a >< span class = "lineno" > 464</ span > t_vec-> rows*t_vec-> cols*CV_MAT_CN(t_vec-> type) != 3 )</ div >
< div class = "line" >< a id = "l00465" name = "l00465" ></ a >< span class = "lineno" > 465</ span > CV_Error( CV_StsBadArg,</ div >
< div class = "line" >< a id = "l00466" name = "l00466" ></ a >< span class = "lineno" > 466</ span > < span class = "stringliteral" > " Translation vector must be 1x3 or 3x1 floating-point vector" </ span > );</ div >
< div class = "line" >< a id = "l00467" name = "l00467" ></ a >< span class = "lineno" > 467</ span > </ div >
< div class = "line" >< a id = "l00468" name = "l00468" ></ a >< span class = "lineno" > 468</ span > _t = cvMat( t_vec-> rows, t_vec-> cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(t_vec-> type)), t );</ div >
< div class = "line" >< a id = "l00469" name = "l00469" ></ a >< span class = "lineno" > 469</ span > cvConvert( t_vec, & _t );</ div >
< div class = "line" >< a id = "l00470" name = "l00470" ></ a >< span class = "lineno" > 470</ span > </ div >
< div class = "line" >< a id = "l00471" name = "l00471" ></ a >< span class = "lineno" > 471</ span > < span class = "keywordflow" > if</ span > ( (CV_MAT_TYPE(A-> type) != CV_64FC1 && CV_MAT_TYPE(A-> type) != CV_32FC1) ||</ div >
< div class = "line" >< a id = "l00472" name = "l00472" ></ a >< span class = "lineno" > 472</ span > A-> rows != 3 || A-> cols != 3 )</ div >
< div class = "line" >< a id = "l00473" name = "l00473" ></ a >< span class = "lineno" > 473</ span > CV_Error( CV_StsBadArg, < span class = "stringliteral" > " Instrinsic parameters must be 3x3 floating-point matrix" </ span > );</ div >
< div class = "line" >< a id = "l00474" name = "l00474" ></ a >< span class = "lineno" > 474</ span > </ div >
< div class = "line" >< a id = "l00475" name = "l00475" ></ a >< span class = "lineno" > 475</ span > cvConvert( A, & _a );</ div >
< div class = "line" >< a id = "l00476" name = "l00476" ></ a >< span class = "lineno" > 476</ span > fx = a[0]; fy = a[4];</ div >
< div class = "line" >< a id = "l00477" name = "l00477" ></ a >< span class = "lineno" > 477</ span > cx = a[2]; cy = a[5];</ div >
< div class = "line" >< a id = "l00478" name = "l00478" ></ a >< span class = "lineno" > 478</ span > </ div >
< div class = "line" >< a id = "l00479" name = "l00479" ></ a >< span class = "lineno" > 479</ span > < span class = "keywordflow" > if</ span > ( fixedAspectRatio )</ div >
< div class = "line" >< a id = "l00480" name = "l00480" ></ a >< span class = "lineno" > 480</ span > fx = fy*aspectRatio;</ div >
< div class = "line" >< a id = "l00481" name = "l00481" ></ a >< span class = "lineno" > 481</ span > </ div >
< div class = "line" >< a id = "l00482" name = "l00482" ></ a >< span class = "lineno" > 482</ span > < span class = "keywordflow" > if</ span > ( distCoeffs )</ div >
< div class = "line" >< a id = "l00483" name = "l00483" ></ a >< span class = "lineno" > 483</ span > {</ div >
< div class = "line" >< a id = "l00484" name = "l00484" ></ a >< span class = "lineno" > 484</ span > < span class = "keywordflow" > if</ span > ( !CV_IS_MAT(distCoeffs) ||</ div >
< div class = "line" >< a id = "l00485" name = "l00485" ></ a >< span class = "lineno" > 485</ span > (CV_MAT_DEPTH(distCoeffs-> type) != CV_64F && </ div >
< div class = "line" >< a id = "l00486" name = "l00486" ></ a >< span class = "lineno" > 486</ span > CV_MAT_DEPTH(distCoeffs-> type) != CV_32F) ||</ div >
< div class = "line" >< a id = "l00487" name = "l00487" ></ a >< span class = "lineno" > 487</ span > (distCoeffs-> rows != 1 && distCoeffs-> cols != 1) ||</ div >
< div class = "line" >< a id = "l00488" name = "l00488" ></ a >< span class = "lineno" > 488</ span > (distCoeffs-> rows*distCoeffs-> cols*CV_MAT_CN(distCoeffs-> type) != 4 && </ div >
< div class = "line" >< a id = "l00489" name = "l00489" ></ a >< span class = "lineno" > 489</ span > distCoeffs-> rows*distCoeffs-> cols*CV_MAT_CN(distCoeffs-> type) != 5 && </ div >
< div class = "line" >< a id = "l00490" name = "l00490" ></ a >< span class = "lineno" > 490</ span > distCoeffs-> rows*distCoeffs-> cols*CV_MAT_CN(distCoeffs-> type) != 8 && </ div >
< div class = "line" >< a id = "l00491" name = "l00491" ></ a >< span class = "lineno" > 491</ span > distCoeffs-> rows*distCoeffs-> cols*CV_MAT_CN(distCoeffs-> type) != 12 && </ div >
< div class = "line" >< a id = "l00492" name = "l00492" ></ a >< span class = "lineno" > 492</ span > distCoeffs-> rows*distCoeffs-> cols*CV_MAT_CN(distCoeffs-> type) != 14) )</ div >
< div class = "line" >< a id = "l00493" name = "l00493" ></ a >< span class = "lineno" > 493</ span > CV_Error( CV_StsBadArg, < span class = "stringliteral" > " Distortion coefficients must be 1x4, 4x1, 1x5, 5x1, 1x8, 8x1, 1x12, 12x1, 1x14 or 14x1 floating-point vector" </ span > );</ div >
< div class = "line" >< a id = "l00494" name = "l00494" ></ a >< span class = "lineno" > 494</ span > </ div >
< div class = "line" >< a id = "l00495" name = "l00495" ></ a >< span class = "lineno" > 495</ span > _k = cvMat( distCoeffs-> rows, distCoeffs-> cols,</ div >
< div class = "line" >< a id = "l00496" name = "l00496" ></ a >< span class = "lineno" > 496</ span > CV_MAKETYPE(CV_64F,CV_MAT_CN(distCoeffs-> type)), k );</ div >
< div class = "line" >< a id = "l00497" name = "l00497" ></ a >< span class = "lineno" > 497</ span > cvConvert( distCoeffs, & _k );</ div >
< div class = "line" >< a id = "l00498" name = "l00498" ></ a >< span class = "lineno" > 498</ span > < span class = "keywordflow" > if</ span > (k[12] != 0 || k[13] != 0)</ div >
< div class = "line" >< a id = "l00499" name = "l00499" ></ a >< span class = "lineno" > 499</ span > {</ div >
< div class = "line" >< a id = "l00500" name = "l00500" ></ a >< span class = "lineno" > 500</ span > computeTiltProjectionMatrix(k[12], k[13],</ div >
< div class = "line" >< a id = "l00501" name = "l00501" ></ a >< span class = "lineno" > 501</ span > & matTilt, & dMatTiltdTauX, & dMatTiltdTauY);</ div >
< div class = "line" >< a id = "l00502" name = "l00502" ></ a >< span class = "lineno" > 502</ span > }</ div >
< div class = "line" >< a id = "l00503" name = "l00503" ></ a >< span class = "lineno" > 503</ span > }</ div >
< div class = "line" >< a id = "l00504" name = "l00504" ></ a >< span class = "lineno" > 504</ span > </ div >
< div class = "line" >< a id = "l00505" name = "l00505" ></ a >< span class = "lineno" > 505</ span > < span class = "keywordflow" > if</ span > ( dpdr )</ div >
< div class = "line" >< a id = "l00506" name = "l00506" ></ a >< span class = "lineno" > 506</ span > {</ div >
< div class = "line" >< a id = "l00507" name = "l00507" ></ a >< span class = "lineno" > 507</ span > < span class = "keywordflow" > if</ span > ( !CV_IS_MAT(dpdr) ||</ div >
< div class = "line" >< a id = "l00508" name = "l00508" ></ a >< span class = "lineno" > 508</ span > (CV_MAT_TYPE(dpdr-> type) != CV_32FC1 && </ div >
< div class = "line" >< a id = "l00509" name = "l00509" ></ a >< span class = "lineno" > 509</ span > CV_MAT_TYPE(dpdr-> type) != CV_64FC1) ||</ div >
< div class = "line" >< a id = "l00510" name = "l00510" ></ a >< span class = "lineno" > 510</ span > dpdr-> rows != count*2 || dpdr-> cols != 3 )</ div >
< div class = "line" >< a id = "l00511" name = "l00511" ></ a >< span class = "lineno" > 511</ span > CV_Error( CV_StsBadArg, < span class = "stringliteral" > " dp/drot must be 2Nx3 floating-point matrix" </ span > );</ div >
< div class = "line" >< a id = "l00512" name = "l00512" ></ a >< span class = "lineno" > 512</ span > </ div >
< div class = "line" >< a id = "l00513" name = "l00513" ></ a >< span class = "lineno" > 513</ span > < span class = "keywordflow" > if</ span > ( CV_MAT_TYPE(dpdr-> type) == CV_64FC1 )</ div >
< div class = "line" >< a id = "l00514" name = "l00514" ></ a >< span class = "lineno" > 514</ span > {</ div >
< div class = "line" >< a id = "l00515" name = "l00515" ></ a >< span class = "lineno" > 515</ span > _dpdr.reset(cvCloneMat(dpdr));</ div >
< div class = "line" >< a id = "l00516" name = "l00516" ></ a >< span class = "lineno" > 516</ span > }</ div >
< div class = "line" >< a id = "l00517" name = "l00517" ></ a >< span class = "lineno" > 517</ span > < span class = "keywordflow" > else</ span ></ div >
< div class = "line" >< a id = "l00518" name = "l00518" ></ a >< span class = "lineno" > 518</ span > _dpdr.reset(cvCreateMat( 2*count, 3, CV_64FC1 ));</ div >
< div class = "line" >< a id = "l00519" name = "l00519" ></ a >< span class = "lineno" > 519</ span > dpdr_p = _dpdr-> data.db;</ div >
< div class = "line" >< a id = "l00520" name = "l00520" ></ a >< span class = "lineno" > 520</ span > dpdr_step = _dpdr-> step/< span class = "keyword" > sizeof</ span > (dpdr_p[0]);</ div >
< div class = "line" >< a id = "l00521" name = "l00521" ></ a >< span class = "lineno" > 521</ span > }</ div >
< div class = "line" >< a id = "l00522" name = "l00522" ></ a >< span class = "lineno" > 522</ span > </ div >
< div class = "line" >< a id = "l00523" name = "l00523" ></ a >< span class = "lineno" > 523</ span > < span class = "keywordflow" > if</ span > ( dpdt )</ div >
< div class = "line" >< a id = "l00524" name = "l00524" ></ a >< span class = "lineno" > 524</ span > {</ div >
< div class = "line" >< a id = "l00525" name = "l00525" ></ a >< span class = "lineno" > 525</ span > < span class = "keywordflow" > if</ span > ( !CV_IS_MAT(dpdt) ||</ div >
< div class = "line" >< a id = "l00526" name = "l00526" ></ a >< span class = "lineno" > 526</ span > (CV_MAT_TYPE(dpdt-> type) != CV_32FC1 && </ div >
< div class = "line" >< a id = "l00527" name = "l00527" ></ a >< span class = "lineno" > 527</ span > CV_MAT_TYPE(dpdt-> type) != CV_64FC1) ||</ div >
< div class = "line" >< a id = "l00528" name = "l00528" ></ a >< span class = "lineno" > 528</ span > dpdt-> rows != count*2 || dpdt-> cols != 3 )</ div >
< div class = "line" >< a id = "l00529" name = "l00529" ></ a >< span class = "lineno" > 529</ span > CV_Error( CV_StsBadArg, < span class = "stringliteral" > " dp/dT must be 2Nx3 floating-point matrix" </ span > );</ div >
< div class = "line" >< a id = "l00530" name = "l00530" ></ a >< span class = "lineno" > 530</ span > </ div >
< div class = "line" >< a id = "l00531" name = "l00531" ></ a >< span class = "lineno" > 531</ span > < span class = "keywordflow" > if</ span > ( CV_MAT_TYPE(dpdt-> type) == CV_64FC1 )</ div >
< div class = "line" >< a id = "l00532" name = "l00532" ></ a >< span class = "lineno" > 532</ span > {</ div >
< div class = "line" >< a id = "l00533" name = "l00533" ></ a >< span class = "lineno" > 533</ span > _dpdt.reset(cvCloneMat(dpdt));</ div >
< div class = "line" >< a id = "l00534" name = "l00534" ></ a >< span class = "lineno" > 534</ span > }</ div >
< div class = "line" >< a id = "l00535" name = "l00535" ></ a >< span class = "lineno" > 535</ span > < span class = "keywordflow" > else</ span ></ div >
< div class = "line" >< a id = "l00536" name = "l00536" ></ a >< span class = "lineno" > 536</ span > _dpdt.reset(cvCreateMat( 2*count, 3, CV_64FC1 ));</ div >
< div class = "line" >< a id = "l00537" name = "l00537" ></ a >< span class = "lineno" > 537</ span > dpdt_p = _dpdt-> data.db;</ div >
< div class = "line" >< a id = "l00538" name = "l00538" ></ a >< span class = "lineno" > 538</ span > dpdt_step = _dpdt-> step/< span class = "keyword" > sizeof</ span > (dpdt_p[0]);</ div >
< div class = "line" >< a id = "l00539" name = "l00539" ></ a >< span class = "lineno" > 539</ span > }</ div >
< div class = "line" >< a id = "l00540" name = "l00540" ></ a >< span class = "lineno" > 540</ span > </ div >
< div class = "line" >< a id = "l00541" name = "l00541" ></ a >< span class = "lineno" > 541</ span > < span class = "keywordflow" > if</ span > ( dpdf )</ div >
< div class = "line" >< a id = "l00542" name = "l00542" ></ a >< span class = "lineno" > 542</ span > {</ div >
< div class = "line" >< a id = "l00543" name = "l00543" ></ a >< span class = "lineno" > 543</ span > < span class = "keywordflow" > if</ span > ( !CV_IS_MAT(dpdf) ||</ div >
< div class = "line" >< a id = "l00544" name = "l00544" ></ a >< span class = "lineno" > 544</ span > (CV_MAT_TYPE(dpdf-> type) != CV_32FC1 && CV_MAT_TYPE(dpdf-> type) != CV_64FC1) ||</ div >
< div class = "line" >< a id = "l00545" name = "l00545" ></ a >< span class = "lineno" > 545</ span > dpdf-> rows != count*2 || dpdf-> cols != 2 )</ div >
< div class = "line" >< a id = "l00546" name = "l00546" ></ a >< span class = "lineno" > 546</ span > CV_Error( CV_StsBadArg, < span class = "stringliteral" > " dp/df must be 2Nx2 floating-point matrix" </ span > );</ div >
< div class = "line" >< a id = "l00547" name = "l00547" ></ a >< span class = "lineno" > 547</ span > </ div >
< div class = "line" >< a id = "l00548" name = "l00548" ></ a >< span class = "lineno" > 548</ span > < span class = "keywordflow" > if</ span > ( CV_MAT_TYPE(dpdf-> type) == CV_64FC1 )</ div >
< div class = "line" >< a id = "l00549" name = "l00549" ></ a >< span class = "lineno" > 549</ span > {</ div >
< div class = "line" >< a id = "l00550" name = "l00550" ></ a >< span class = "lineno" > 550</ span > _dpdf.reset(cvCloneMat(dpdf));</ div >
< div class = "line" >< a id = "l00551" name = "l00551" ></ a >< span class = "lineno" > 551</ span > }</ div >
< div class = "line" >< a id = "l00552" name = "l00552" ></ a >< span class = "lineno" > 552</ span > < span class = "keywordflow" > else</ span ></ div >
< div class = "line" >< a id = "l00553" name = "l00553" ></ a >< span class = "lineno" > 553</ span > _dpdf.reset(cvCreateMat( 2*count, 2, CV_64FC1 ));</ div >
< div class = "line" >< a id = "l00554" name = "l00554" ></ a >< span class = "lineno" > 554</ span > dpdf_p = _dpdf-> data.db;</ div >
< div class = "line" >< a id = "l00555" name = "l00555" ></ a >< span class = "lineno" > 555</ span > dpdf_step = _dpdf-> step/< span class = "keyword" > sizeof</ span > (dpdf_p[0]);</ div >
< div class = "line" >< a id = "l00556" name = "l00556" ></ a >< span class = "lineno" > 556</ span > }</ div >
< div class = "line" >< a id = "l00557" name = "l00557" ></ a >< span class = "lineno" > 557</ span > </ div >
< div class = "line" >< a id = "l00558" name = "l00558" ></ a >< span class = "lineno" > 558</ span > < span class = "keywordflow" > if</ span > ( dpdc )</ div >
< div class = "line" >< a id = "l00559" name = "l00559" ></ a >< span class = "lineno" > 559</ span > {</ div >
< div class = "line" >< a id = "l00560" name = "l00560" ></ a >< span class = "lineno" > 560</ span > < span class = "keywordflow" > if</ span > ( !CV_IS_MAT(dpdc) ||</ div >
< div class = "line" >< a id = "l00561" name = "l00561" ></ a >< span class = "lineno" > 561</ span > (CV_MAT_TYPE(dpdc-> type) != CV_32FC1 && CV_MAT_TYPE(dpdc-> type) != CV_64FC1) ||</ div >
< div class = "line" >< a id = "l00562" name = "l00562" ></ a >< span class = "lineno" > 562</ span > dpdc-> rows != count*2 || dpdc-> cols != 2 )</ div >
< div class = "line" >< a id = "l00563" name = "l00563" ></ a >< span class = "lineno" > 563</ span > CV_Error( CV_StsBadArg, < span class = "stringliteral" > " dp/dc must be 2Nx2 floating-point matrix" </ span > );</ div >
< div class = "line" >< a id = "l00564" name = "l00564" ></ a >< span class = "lineno" > 564</ span > </ div >
< div class = "line" >< a id = "l00565" name = "l00565" ></ a >< span class = "lineno" > 565</ span > < span class = "keywordflow" > if</ span > ( CV_MAT_TYPE(dpdc-> type) == CV_64FC1 )</ div >
< div class = "line" >< a id = "l00566" name = "l00566" ></ a >< span class = "lineno" > 566</ span > {</ div >
< div class = "line" >< a id = "l00567" name = "l00567" ></ a >< span class = "lineno" > 567</ span > _dpdc.reset(cvCloneMat(dpdc));</ div >
< div class = "line" >< a id = "l00568" name = "l00568" ></ a >< span class = "lineno" > 568</ span > }</ div >
< div class = "line" >< a id = "l00569" name = "l00569" ></ a >< span class = "lineno" > 569</ span > < span class = "keywordflow" > else</ span ></ div >
< div class = "line" >< a id = "l00570" name = "l00570" ></ a >< span class = "lineno" > 570</ span > _dpdc.reset(cvCreateMat( 2*count, 2, CV_64FC1 ));</ div >
< div class = "line" >< a id = "l00571" name = "l00571" ></ a >< span class = "lineno" > 571</ span > dpdc_p = _dpdc-> data.db;</ div >
< div class = "line" >< a id = "l00572" name = "l00572" ></ a >< span class = "lineno" > 572</ span > dpdc_step = _dpdc-> step/< span class = "keyword" > sizeof</ span > (dpdc_p[0]);</ div >
< div class = "line" >< a id = "l00573" name = "l00573" ></ a >< span class = "lineno" > 573</ span > }</ div >
< div class = "line" >< a id = "l00574" name = "l00574" ></ a >< span class = "lineno" > 574</ span > </ div >
< div class = "line" >< a id = "l00575" name = "l00575" ></ a >< span class = "lineno" > 575</ span > < span class = "keywordflow" > if</ span > ( dpdk )</ div >
< div class = "line" >< a id = "l00576" name = "l00576" ></ a >< span class = "lineno" > 576</ span > {</ div >
< div class = "line" >< a id = "l00577" name = "l00577" ></ a >< span class = "lineno" > 577</ span > < span class = "keywordflow" > if</ span > ( !CV_IS_MAT(dpdk) ||</ div >
< div class = "line" >< a id = "l00578" name = "l00578" ></ a >< span class = "lineno" > 578</ span > (CV_MAT_TYPE(dpdk-> type) != CV_32FC1 && CV_MAT_TYPE(dpdk-> type) != CV_64FC1) ||</ div >
< div class = "line" >< a id = "l00579" name = "l00579" ></ a >< span class = "lineno" > 579</ span > dpdk-> rows != count*2 || (dpdk-> cols != 14 && dpdk-> cols != 12 && dpdk-> cols != 8 && dpdk-> cols != 5 && dpdk-> cols != 4 && dpdk-> cols != 2) )</ div >
< div class = "line" >< a id = "l00580" name = "l00580" ></ a >< span class = "lineno" > 580</ span > CV_Error( CV_StsBadArg, < span class = "stringliteral" > " dp/df must be 2Nx14, 2Nx12, 2Nx8, 2Nx5, 2Nx4 or 2Nx2 floating-point matrix" </ span > );</ div >
< div class = "line" >< a id = "l00581" name = "l00581" ></ a >< span class = "lineno" > 581</ span > </ div >
< div class = "line" >< a id = "l00582" name = "l00582" ></ a >< span class = "lineno" > 582</ span > < span class = "keywordflow" > if</ span > ( !distCoeffs )</ div >
< div class = "line" >< a id = "l00583" name = "l00583" ></ a >< span class = "lineno" > 583</ span > CV_Error( CV_StsNullPtr, < span class = "stringliteral" > " distCoeffs is NULL while dpdk is not" </ span > );</ div >
< div class = "line" >< a id = "l00584" name = "l00584" ></ a >< span class = "lineno" > 584</ span > </ div >
< div class = "line" >< a id = "l00585" name = "l00585" ></ a >< span class = "lineno" > 585</ span > < span class = "keywordflow" > if</ span > ( CV_MAT_TYPE(dpdk-> type) == CV_64FC1 )</ div >
< div class = "line" >< a id = "l00586" name = "l00586" ></ a >< span class = "lineno" > 586</ span > {</ div >
< div class = "line" >< a id = "l00587" name = "l00587" ></ a >< span class = "lineno" > 587</ span > _dpdk.reset(cvCloneMat(dpdk));</ div >
< div class = "line" >< a id = "l00588" name = "l00588" ></ a >< span class = "lineno" > 588</ span > }</ div >
< div class = "line" >< a id = "l00589" name = "l00589" ></ a >< span class = "lineno" > 589</ span > < span class = "keywordflow" > else</ span ></ div >
< div class = "line" >< a id = "l00590" name = "l00590" ></ a >< span class = "lineno" > 590</ span > _dpdk.reset(cvCreateMat( dpdk-> rows, dpdk-> cols, CV_64FC1 ));</ div >
< div class = "line" >< a id = "l00591" name = "l00591" ></ a >< span class = "lineno" > 591</ span > dpdk_p = _dpdk-> data.db;</ div >
< div class = "line" >< a id = "l00592" name = "l00592" ></ a >< span class = "lineno" > 592</ span > dpdk_step = _dpdk-> step/< span class = "keyword" > sizeof</ span > (dpdk_p[0]);</ div >
< div class = "line" >< a id = "l00593" name = "l00593" ></ a >< span class = "lineno" > 593</ span > }</ div >
< div class = "line" >< a id = "l00594" name = "l00594" ></ a >< span class = "lineno" > 594</ span > </ div >
< div class = "line" >< a id = "l00595" name = "l00595" ></ a >< span class = "lineno" > 595</ span > < span class = "keywordflow" > if</ span > ( dpdo )</ div >
< div class = "line" >< a id = "l00596" name = "l00596" ></ a >< span class = "lineno" > 596</ span > {</ div >
< div class = "line" >< a id = "l00597" name = "l00597" ></ a >< span class = "lineno" > 597</ span > < span class = "keywordflow" > if</ span > ( !CV_IS_MAT( dpdo ) || ( CV_MAT_TYPE( dpdo-> type ) != CV_32FC1</ div >
< div class = "line" >< a id = "l00598" name = "l00598" ></ a >< span class = "lineno" > 598</ span > && CV_MAT_TYPE( dpdo-> type ) != CV_64FC1 )</ div >
< div class = "line" >< a id = "l00599" name = "l00599" ></ a >< span class = "lineno" > 599</ span > || dpdo-> rows != count * 2 || dpdo-> cols != count * 3 )</ div >
< div class = "line" >< a id = "l00600" name = "l00600" ></ a >< span class = "lineno" > 600</ span > CV_Error( CV_StsBadArg, < span class = "stringliteral" > " dp/do must be 2Nx3N floating-point matrix" </ span > );</ div >
< div class = "line" >< a id = "l00601" name = "l00601" ></ a >< span class = "lineno" > 601</ span > </ div >
< div class = "line" >< a id = "l00602" name = "l00602" ></ a >< span class = "lineno" > 602</ span > < span class = "keywordflow" > if</ span > ( CV_MAT_TYPE( dpdo-> type ) == CV_64FC1 )</ div >
< div class = "line" >< a id = "l00603" name = "l00603" ></ a >< span class = "lineno" > 603</ span > {</ div >
< div class = "line" >< a id = "l00604" name = "l00604" ></ a >< span class = "lineno" > 604</ span > _dpdo.reset( cvCloneMat( dpdo ) );</ div >
< div class = "line" >< a id = "l00605" name = "l00605" ></ a >< span class = "lineno" > 605</ span > }</ div >
< div class = "line" >< a id = "l00606" name = "l00606" ></ a >< span class = "lineno" > 606</ span > < span class = "keywordflow" > else</ span ></ div >
< div class = "line" >< a id = "l00607" name = "l00607" ></ a >< span class = "lineno" > 607</ span > _dpdo.reset( cvCreateMat( 2 * count, 3 * count, CV_64FC1 ) );</ div >
< div class = "line" >< a id = "l00608" name = "l00608" ></ a >< span class = "lineno" > 608</ span > cvZero(_dpdo);</ div >
< div class = "line" >< a id = "l00609" name = "l00609" ></ a >< span class = "lineno" > 609</ span > dpdo_p = _dpdo-> data.db;</ div >
< div class = "line" >< a id = "l00610" name = "l00610" ></ a >< span class = "lineno" > 610</ span > dpdo_step = _dpdo-> step / < span class = "keyword" > sizeof</ span > ( dpdo_p[0] );</ div >
< div class = "line" >< a id = "l00611" name = "l00611" ></ a >< span class = "lineno" > 611</ span > }</ div >
< div class = "line" >< a id = "l00612" name = "l00612" ></ a >< span class = "lineno" > 612</ span > </ div >
< div class = "line" >< a id = "l00613" name = "l00613" ></ a >< span class = "lineno" > 613</ span > calc_derivatives = dpdr || dpdt || dpdf || dpdc || dpdk || dpdo;</ div >
< div class = "line" >< a id = "l00614" name = "l00614" ></ a >< span class = "lineno" > 614</ span > </ div >
< div class = "line" >< a id = "l00615" name = "l00615" ></ a >< span class = "lineno" > 615</ span > < span class = "keywordflow" > for</ span > ( i = 0; i < count; i++ )</ div >
< div class = "line" >< a id = "l00616" name = "l00616" ></ a >< span class = "lineno" > 616</ span > {</ div >
< div class = "line" >< a id = "l00617" name = "l00617" ></ a >< span class = "lineno" > 617</ span > < span class = "keywordtype" > double</ span > X = M[i].x, Y = M[i].y, Z = M[i].z;</ div >
< div class = "line" >< a id = "l00618" name = "l00618" ></ a >< span class = "lineno" > 618</ span > < span class = "keywordtype" > double</ span > x = R[0]*X + R[1]*Y + R[2]*Z + t[0];</ div >
< div class = "line" >< a id = "l00619" name = "l00619" ></ a >< span class = "lineno" > 619</ span > < span class = "keywordtype" > double</ span > y = R[3]*X + R[4]*Y + R[5]*Z + t[1];</ div >
< div class = "line" >< a id = "l00620" name = "l00620" ></ a >< span class = "lineno" > 620</ span > < span class = "keywordtype" > double</ span > z = R[6]*X + R[7]*Y + R[8]*Z + t[2];</ div >
< div class = "line" >< a id = "l00621" name = "l00621" ></ a >< span class = "lineno" > 621</ span > < span class = "keywordtype" > double</ span > r2, r4, r6, a1, a2, a3, cdist, icdist2;</ div >
< div class = "line" >< a id = "l00622" name = "l00622" ></ a >< span class = "lineno" > 622</ span > < span class = "keywordtype" > double</ span > xd, yd, xd0, yd0, invProj;</ div >
< div class = "line" >< a id = "l00623" name = "l00623" ></ a >< span class = "lineno" > 623</ span > cv::Vec3d vecTilt;</ div >
< div class = "line" >< a id = "l00624" name = "l00624" ></ a >< span class = "lineno" > 624</ span > cv::Vec3d dVecTilt;</ div >
< div class = "line" >< a id = "l00625" name = "l00625" ></ a >< span class = "lineno" > 625</ span > cv::Matx22d dMatTilt;</ div >
< div class = "line" >< a id = "l00626" name = "l00626" ></ a >< span class = "lineno" > 626</ span > cv::Vec2d dXdYd;</ div >
< div class = "line" >< a id = "l00627" name = "l00627" ></ a >< span class = "lineno" > 627</ span > </ div >
< div class = "line" >< a id = "l00628" name = "l00628" ></ a >< span class = "lineno" > 628</ span > < span class = "keywordtype" > double</ span > z0 = z;</ div >
< div class = "line" >< a id = "l00629" name = "l00629" ></ a >< span class = "lineno" > 629</ span > z = z ? 1./z : 1;</ div >
< div class = "line" >< a id = "l00630" name = "l00630" ></ a >< span class = "lineno" > 630</ span > x *= z; y *= z;</ div >
< div class = "line" >< a id = "l00631" name = "l00631" ></ a >< span class = "lineno" > 631</ span > </ div >
< div class = "line" >< a id = "l00632" name = "l00632" ></ a >< span class = "lineno" > 632</ span > r2 = x*x + y*y;</ div >
< div class = "line" >< a id = "l00633" name = "l00633" ></ a >< span class = "lineno" > 633</ span > r4 = r2*r2;</ div >
< div class = "line" >< a id = "l00634" name = "l00634" ></ a >< span class = "lineno" > 634</ span > r6 = r4*r2;</ div >
< div class = "line" >< a id = "l00635" name = "l00635" ></ a >< span class = "lineno" > 635</ span > a1 = 2*x*y;</ div >
< div class = "line" >< a id = "l00636" name = "l00636" ></ a >< span class = "lineno" > 636</ span > a2 = r2 + 2*x*x;</ div >
< div class = "line" >< a id = "l00637" name = "l00637" ></ a >< span class = "lineno" > 637</ span > a3 = r2 + 2*y*y;</ div >
< div class = "line" >< a id = "l00638" name = "l00638" ></ a >< span class = "lineno" > 638</ span > cdist = 1 + k[0]*r2 + k[1]*r4 + k[4]*r6;</ div >
< div class = "line" >< a id = "l00639" name = "l00639" ></ a >< span class = "lineno" > 639</ span > icdist2 = 1./(1 + k[5]*r2 + k[6]*r4 + k[7]*r6);</ div >
< div class = "line" >< a id = "l00640" name = "l00640" ></ a >< span class = "lineno" > 640</ span > xd0 = x*cdist*icdist2 + k[2]*a1 + k[3]*a2 + k[8]*r2+k[9]*r4;</ div >
< div class = "line" >< a id = "l00641" name = "l00641" ></ a >< span class = "lineno" > 641</ span > yd0 = y*cdist*icdist2 + k[2]*a3 + k[3]*a1 + k[10]*r2+k[11]*r4;</ div >
< div class = "line" >< a id = "l00642" name = "l00642" ></ a >< span class = "lineno" > 642</ span > </ div >
< div class = "line" >< a id = "l00643" name = "l00643" ></ a >< span class = "lineno" > 643</ span > < span class = "comment" > // additional distortion by projecting onto a tilt plane</ span ></ div >
< div class = "line" >< a id = "l00644" name = "l00644" ></ a >< span class = "lineno" > 644</ span > vecTilt = matTilt*cv::Vec3d(xd0, yd0, 1);</ div >
< div class = "line" >< a id = "l00645" name = "l00645" ></ a >< span class = "lineno" > 645</ span > invProj = vecTilt(2) ? 1./vecTilt(2) : 1;</ div >
< div class = "line" >< a id = "l00646" name = "l00646" ></ a >< span class = "lineno" > 646</ span > xd = invProj * vecTilt(0);</ div >
< div class = "line" >< a id = "l00647" name = "l00647" ></ a >< span class = "lineno" > 647</ span > yd = invProj * vecTilt(1);</ div >
< div class = "line" >< a id = "l00648" name = "l00648" ></ a >< span class = "lineno" > 648</ span > </ div >
< div class = "line" >< a id = "l00649" name = "l00649" ></ a >< span class = "lineno" > 649</ span > m[i].x = xd*fx + cx;</ div >
< div class = "line" >< a id = "l00650" name = "l00650" ></ a >< span class = "lineno" > 650</ span > m[i].y = yd*fy + cy;</ div >
< div class = "line" >< a id = "l00651" name = "l00651" ></ a >< span class = "lineno" > 651</ span > </ div >
< div class = "line" >< a id = "l00652" name = "l00652" ></ a >< span class = "lineno" > 652</ span > < span class = "keywordflow" > if</ span > ( calc_derivatives )</ div >
< div class = "line" >< a id = "l00653" name = "l00653" ></ a >< span class = "lineno" > 653</ span > {</ div >
< div class = "line" >< a id = "l00654" name = "l00654" ></ a >< span class = "lineno" > 654</ span > < span class = "keywordflow" > if</ span > ( dpdc_p )</ div >
< div class = "line" >< a id = "l00655" name = "l00655" ></ a >< span class = "lineno" > 655</ span > {</ div >
< div class = "line" >< a id = "l00656" name = "l00656" ></ a >< span class = "lineno" > 656</ span > dpdc_p[0] = 1; dpdc_p[1] = 0; < span class = "comment" > // dp_xdc_x; dp_xdc_y</ span ></ div >
< div class = "line" >< a id = "l00657" name = "l00657" ></ a >< span class = "lineno" > 657</ span > dpdc_p[dpdc_step] = 0;</ div >
< div class = "line" >< a id = "l00658" name = "l00658" ></ a >< span class = "lineno" > 658</ span > dpdc_p[dpdc_step+1] = 1;</ div >
< div class = "line" >< a id = "l00659" name = "l00659" ></ a >< span class = "lineno" > 659</ span > dpdc_p += dpdc_step*2;</ div >
< div class = "line" >< a id = "l00660" name = "l00660" ></ a >< span class = "lineno" > 660</ span > }</ div >
< div class = "line" >< a id = "l00661" name = "l00661" ></ a >< span class = "lineno" > 661</ span > </ div >
< div class = "line" >< a id = "l00662" name = "l00662" ></ a >< span class = "lineno" > 662</ span > < span class = "keywordflow" > if</ span > ( dpdf_p )</ div >
< div class = "line" >< a id = "l00663" name = "l00663" ></ a >< span class = "lineno" > 663</ span > {</ div >
< div class = "line" >< a id = "l00664" name = "l00664" ></ a >< span class = "lineno" > 664</ span > < span class = "keywordflow" > if</ span > ( fixedAspectRatio )</ div >
< div class = "line" >< a id = "l00665" name = "l00665" ></ a >< span class = "lineno" > 665</ span > {</ div >
< div class = "line" >< a id = "l00666" name = "l00666" ></ a >< span class = "lineno" > 666</ span > dpdf_p[0] = 0; dpdf_p[1] = xd*aspectRatio; < span class = "comment" > // dp_xdf_x; dp_xdf_y</ span ></ div >
< div class = "line" >< a id = "l00667" name = "l00667" ></ a >< span class = "lineno" > 667</ span > dpdf_p[dpdf_step] = 0;</ div >
< div class = "line" >< a id = "l00668" name = "l00668" ></ a >< span class = "lineno" > 668</ span > dpdf_p[dpdf_step+1] = yd;</ div >
< div class = "line" >< a id = "l00669" name = "l00669" ></ a >< span class = "lineno" > 669</ span > }</ div >
< div class = "line" >< a id = "l00670" name = "l00670" ></ a >< span class = "lineno" > 670</ span > < span class = "keywordflow" > else</ span ></ div >
< div class = "line" >< a id = "l00671" name = "l00671" ></ a >< span class = "lineno" > 671</ span > {</ div >
< div class = "line" >< a id = "l00672" name = "l00672" ></ a >< span class = "lineno" > 672</ span > dpdf_p[0] = xd; dpdf_p[1] = 0;</ div >
< div class = "line" >< a id = "l00673" name = "l00673" ></ a >< span class = "lineno" > 673</ span > dpdf_p[dpdf_step] = 0;</ div >
< div class = "line" >< a id = "l00674" name = "l00674" ></ a >< span class = "lineno" > 674</ span > dpdf_p[dpdf_step+1] = yd;</ div >
< div class = "line" >< a id = "l00675" name = "l00675" ></ a >< span class = "lineno" > 675</ span > }</ div >
< div class = "line" >< a id = "l00676" name = "l00676" ></ a >< span class = "lineno" > 676</ span > dpdf_p += dpdf_step*2;</ div >
< div class = "line" >< a id = "l00677" name = "l00677" ></ a >< span class = "lineno" > 677</ span > }</ div >
< div class = "line" >< a id = "l00678" name = "l00678" ></ a >< span class = "lineno" > 678</ span > < span class = "keywordflow" > for</ span > (< span class = "keywordtype" > int</ span > row = 0; row < 2; ++row)</ div >
< div class = "line" >< a id = "l00679" name = "l00679" ></ a >< span class = "lineno" > 679</ span > < span class = "keywordflow" > for</ span > (< span class = "keywordtype" > int</ span > col = 0; col < 2; ++col)</ div >
< div class = "line" >< a id = "l00680" name = "l00680" ></ a >< span class = "lineno" > 680</ span > dMatTilt(row,col) = matTilt(row,col)*vecTilt(2)</ div >
< div class = "line" >< a id = "l00681" name = "l00681" ></ a >< span class = "lineno" > 681</ span > - matTilt(2,col)*vecTilt(row);</ div >
< div class = "line" >< a id = "l00682" name = "l00682" ></ a >< span class = "lineno" > 682</ span > < span class = "keywordtype" > double</ span > invProjSquare = (invProj*invProj);</ div >
< div class = "line" >< a id = "l00683" name = "l00683" ></ a >< span class = "lineno" > 683</ span > dMatTilt *= invProjSquare;</ div >
< div class = "line" >< a id = "l00684" name = "l00684" ></ a >< span class = "lineno" > 684</ span > < span class = "keywordflow" > if</ span > ( dpdk_p )</ div >
< div class = "line" >< a id = "l00685" name = "l00685" ></ a >< span class = "lineno" > 685</ span > {</ div >
< div class = "line" >< a id = "l00686" name = "l00686" ></ a >< span class = "lineno" > 686</ span > dXdYd = dMatTilt*cv::Vec2d(x*icdist2*r2, y*icdist2*r2);</ div >
< div class = "line" >< a id = "l00687" name = "l00687" ></ a >< span class = "lineno" > 687</ span > dpdk_p[0] = fx*dXdYd(0);</ div >
< div class = "line" >< a id = "l00688" name = "l00688" ></ a >< span class = "lineno" > 688</ span > dpdk_p[dpdk_step] = fy*dXdYd(1);</ div >
< div class = "line" >< a id = "l00689" name = "l00689" ></ a >< span class = "lineno" > 689</ span > dXdYd = dMatTilt*cv::Vec2d(x*icdist2*r4, y*icdist2*r4);</ div >
< div class = "line" >< a id = "l00690" name = "l00690" ></ a >< span class = "lineno" > 690</ span > dpdk_p[1] = fx*dXdYd(0);</ div >
< div class = "line" >< a id = "l00691" name = "l00691" ></ a >< span class = "lineno" > 691</ span > dpdk_p[dpdk_step+1] = fy*dXdYd(1);</ div >
< div class = "line" >< a id = "l00692" name = "l00692" ></ a >< span class = "lineno" > 692</ span > < span class = "keywordflow" > if</ span > ( _dpdk-> cols > 2 )</ div >
< div class = "line" >< a id = "l00693" name = "l00693" ></ a >< span class = "lineno" > 693</ span > {</ div >
< div class = "line" >< a id = "l00694" name = "l00694" ></ a >< span class = "lineno" > 694</ span > dXdYd = dMatTilt*cv::Vec2d(a1, a3);</ div >
< div class = "line" >< a id = "l00695" name = "l00695" ></ a >< span class = "lineno" > 695</ span > dpdk_p[2] = fx*dXdYd(0);</ div >
< div class = "line" >< a id = "l00696" name = "l00696" ></ a >< span class = "lineno" > 696</ span > dpdk_p[dpdk_step+2] = fy*dXdYd(1);</ div >
< div class = "line" >< a id = "l00697" name = "l00697" ></ a >< span class = "lineno" > 697</ span > dXdYd = dMatTilt*cv::Vec2d(a2, a1);</ div >
< div class = "line" >< a id = "l00698" name = "l00698" ></ a >< span class = "lineno" > 698</ span > dpdk_p[3] = fx*dXdYd(0);</ div >
< div class = "line" >< a id = "l00699" name = "l00699" ></ a >< span class = "lineno" > 699</ span > dpdk_p[dpdk_step+3] = fy*dXdYd(1);</ div >
< div class = "line" >< a id = "l00700" name = "l00700" ></ a >< span class = "lineno" > 700</ span > < span class = "keywordflow" > if</ span > ( _dpdk-> cols > 4 )</ div >
< div class = "line" >< a id = "l00701" name = "l00701" ></ a >< span class = "lineno" > 701</ span > {</ div >
< div class = "line" >< a id = "l00702" name = "l00702" ></ a >< span class = "lineno" > 702</ span > dXdYd = dMatTilt*cv::Vec2d(x*icdist2*r6, y*icdist2*r6);</ div >
< div class = "line" >< a id = "l00703" name = "l00703" ></ a >< span class = "lineno" > 703</ span > dpdk_p[4] = fx*dXdYd(0);</ div >
< div class = "line" >< a id = "l00704" name = "l00704" ></ a >< span class = "lineno" > 704</ span > dpdk_p[dpdk_step+4] = fy*dXdYd(1);</ div >
< div class = "line" >< a id = "l00705" name = "l00705" ></ a >< span class = "lineno" > 705</ span > </ div >
< div class = "line" >< a id = "l00706" name = "l00706" ></ a >< span class = "lineno" > 706</ span > < span class = "keywordflow" > if</ span > ( _dpdk-> cols > 5 )</ div >
< div class = "line" >< a id = "l00707" name = "l00707" ></ a >< span class = "lineno" > 707</ span > {</ div >
< div class = "line" >< a id = "l00708" name = "l00708" ></ a >< span class = "lineno" > 708</ span > dXdYd = dMatTilt*cv::Vec2d(</ div >
< div class = "line" >< a id = "l00709" name = "l00709" ></ a >< span class = "lineno" > 709</ span > x*cdist*(-icdist2)*icdist2*r2, y*cdist*(-icdist2)*icdist2*r2);</ div >
< div class = "line" >< a id = "l00710" name = "l00710" ></ a >< span class = "lineno" > 710</ span > dpdk_p[5] = fx*dXdYd(0);</ div >
< div class = "line" >< a id = "l00711" name = "l00711" ></ a >< span class = "lineno" > 711</ span > dpdk_p[dpdk_step+5] = fy*dXdYd(1);</ div >
< div class = "line" >< a id = "l00712" name = "l00712" ></ a >< span class = "lineno" > 712</ span > dXdYd = dMatTilt*cv::Vec2d(</ div >
< div class = "line" >< a id = "l00713" name = "l00713" ></ a >< span class = "lineno" > 713</ span > x*cdist*(-icdist2)*icdist2*r4, y*cdist*(-icdist2)*icdist2*r4);</ div >
< div class = "line" >< a id = "l00714" name = "l00714" ></ a >< span class = "lineno" > 714</ span > dpdk_p[6] = fx*dXdYd(0);</ div >
< div class = "line" >< a id = "l00715" name = "l00715" ></ a >< span class = "lineno" > 715</ span > dpdk_p[dpdk_step+6] = fy*dXdYd(1);</ div >
< div class = "line" >< a id = "l00716" name = "l00716" ></ a >< span class = "lineno" > 716</ span > dXdYd = dMatTilt*cv::Vec2d(</ div >
< div class = "line" >< a id = "l00717" name = "l00717" ></ a >< span class = "lineno" > 717</ span > x*cdist*(-icdist2)*icdist2*r6, y*cdist*(-icdist2)*icdist2*r6);</ div >
< div class = "line" >< a id = "l00718" name = "l00718" ></ a >< span class = "lineno" > 718</ span > dpdk_p[7] = fx*dXdYd(0);</ div >
< div class = "line" >< a id = "l00719" name = "l00719" ></ a >< span class = "lineno" > 719</ span > dpdk_p[dpdk_step+7] = fy*dXdYd(1);</ div >
< div class = "line" >< a id = "l00720" name = "l00720" ></ a >< span class = "lineno" > 720</ span > < span class = "keywordflow" > if</ span > ( _dpdk-> cols > 8 )</ div >
< div class = "line" >< a id = "l00721" name = "l00721" ></ a >< span class = "lineno" > 721</ span > {</ div >
< div class = "line" >< a id = "l00722" name = "l00722" ></ a >< span class = "lineno" > 722</ span > dXdYd = dMatTilt*cv::Vec2d(r2, 0);</ div >
< div class = "line" >< a id = "l00723" name = "l00723" ></ a >< span class = "lineno" > 723</ span > dpdk_p[8] = fx*dXdYd(0); < span class = "comment" > //s1</ span ></ div >
< div class = "line" >< a id = "l00724" name = "l00724" ></ a >< span class = "lineno" > 724</ span > dpdk_p[dpdk_step+8] = fy*dXdYd(1); < span class = "comment" > //s1</ span ></ div >
< div class = "line" >< a id = "l00725" name = "l00725" ></ a >< span class = "lineno" > 725</ span > dXdYd = dMatTilt*cv::Vec2d(r4, 0);</ div >
< div class = "line" >< a id = "l00726" name = "l00726" ></ a >< span class = "lineno" > 726</ span > dpdk_p[9] = fx*dXdYd(0); < span class = "comment" > //s2</ span ></ div >
< div class = "line" >< a id = "l00727" name = "l00727" ></ a >< span class = "lineno" > 727</ span > dpdk_p[dpdk_step+9] = fy*dXdYd(1); < span class = "comment" > //s2</ span ></ div >
< div class = "line" >< a id = "l00728" name = "l00728" ></ a >< span class = "lineno" > 728</ span > dXdYd = dMatTilt*cv::Vec2d(0, r2);</ div >
< div class = "line" >< a id = "l00729" name = "l00729" ></ a >< span class = "lineno" > 729</ span > dpdk_p[10] = fx*dXdYd(0);< span class = "comment" > //s3</ span ></ div >
< div class = "line" >< a id = "l00730" name = "l00730" ></ a >< span class = "lineno" > 730</ span > dpdk_p[dpdk_step+10] = fy*dXdYd(1); < span class = "comment" > //s3</ span ></ div >
< div class = "line" >< a id = "l00731" name = "l00731" ></ a >< span class = "lineno" > 731</ span > dXdYd = dMatTilt*cv::Vec2d(0, r4);</ div >
< div class = "line" >< a id = "l00732" name = "l00732" ></ a >< span class = "lineno" > 732</ span > dpdk_p[11] = fx*dXdYd(0);< span class = "comment" > //s4</ span ></ div >
< div class = "line" >< a id = "l00733" name = "l00733" ></ a >< span class = "lineno" > 733</ span > dpdk_p[dpdk_step+11] = fy*dXdYd(1); < span class = "comment" > //s4</ span ></ div >
< div class = "line" >< a id = "l00734" name = "l00734" ></ a >< span class = "lineno" > 734</ span > < span class = "keywordflow" > if</ span > ( _dpdk-> cols > 12 )</ div >
< div class = "line" >< a id = "l00735" name = "l00735" ></ a >< span class = "lineno" > 735</ span > {</ div >
< div class = "line" >< a id = "l00736" name = "l00736" ></ a >< span class = "lineno" > 736</ span > dVecTilt = dMatTiltdTauX * cv::Vec3d(xd0, yd0, 1);</ div >
< div class = "line" >< a id = "l00737" name = "l00737" ></ a >< span class = "lineno" > 737</ span > dpdk_p[12] = fx * invProjSquare * (</ div >
< div class = "line" >< a id = "l00738" name = "l00738" ></ a >< span class = "lineno" > 738</ span > dVecTilt(0) * vecTilt(2) - dVecTilt(2) * vecTilt(0));</ div >
< div class = "line" >< a id = "l00739" name = "l00739" ></ a >< span class = "lineno" > 739</ span > dpdk_p[dpdk_step+12] = fy*invProjSquare * (</ div >
< div class = "line" >< a id = "l00740" name = "l00740" ></ a >< span class = "lineno" > 740</ span > dVecTilt(1) * vecTilt(2) - dVecTilt(2) * vecTilt(1));</ div >
< div class = "line" >< a id = "l00741" name = "l00741" ></ a >< span class = "lineno" > 741</ span > dVecTilt = dMatTiltdTauY * cv::Vec3d(xd0, yd0, 1);</ div >
< div class = "line" >< a id = "l00742" name = "l00742" ></ a >< span class = "lineno" > 742</ span > dpdk_p[13] = fx * invProjSquare * (</ div >
< div class = "line" >< a id = "l00743" name = "l00743" ></ a >< span class = "lineno" > 743</ span > dVecTilt(0) * vecTilt(2) - dVecTilt(2) * vecTilt(0));</ div >
< div class = "line" >< a id = "l00744" name = "l00744" ></ a >< span class = "lineno" > 744</ span > dpdk_p[dpdk_step+13] = fy * invProjSquare * (</ div >
< div class = "line" >< a id = "l00745" name = "l00745" ></ a >< span class = "lineno" > 745</ span > dVecTilt(1) * vecTilt(2) - dVecTilt(2) * vecTilt(1));</ div >
< div class = "line" >< a id = "l00746" name = "l00746" ></ a >< span class = "lineno" > 746</ span > }</ div >
< div class = "line" >< a id = "l00747" name = "l00747" ></ a >< span class = "lineno" > 747</ span > }</ div >
< div class = "line" >< a id = "l00748" name = "l00748" ></ a >< span class = "lineno" > 748</ span > }</ div >
< div class = "line" >< a id = "l00749" name = "l00749" ></ a >< span class = "lineno" > 749</ span > }</ div >
< div class = "line" >< a id = "l00750" name = "l00750" ></ a >< span class = "lineno" > 750</ span > }</ div >
< div class = "line" >< a id = "l00751" name = "l00751" ></ a >< span class = "lineno" > 751</ span > dpdk_p += dpdk_step*2;</ div >
< div class = "line" >< a id = "l00752" name = "l00752" ></ a >< span class = "lineno" > 752</ span > }</ div >
< div class = "line" >< a id = "l00753" name = "l00753" ></ a >< span class = "lineno" > 753</ span > </ div >
< div class = "line" >< a id = "l00754" name = "l00754" ></ a >< span class = "lineno" > 754</ span > < span class = "keywordflow" > if</ span > ( dpdt_p )</ div >
< div class = "line" >< a id = "l00755" name = "l00755" ></ a >< span class = "lineno" > 755</ span > {</ div >
< div class = "line" >< a id = "l00756" name = "l00756" ></ a >< span class = "lineno" > 756</ span > < span class = "keywordtype" > double</ span > dxdt[] = { z, 0, -x*z }, dydt[] = { 0, z, -y*z };</ div >
< div class = "line" >< a id = "l00757" name = "l00757" ></ a >< span class = "lineno" > 757</ span > < span class = "keywordflow" > for</ span > ( j = 0; j < 3; j++ )</ div >
< div class = "line" >< a id = "l00758" name = "l00758" ></ a >< span class = "lineno" > 758</ span > {</ div >
< div class = "line" >< a id = "l00759" name = "l00759" ></ a >< span class = "lineno" > 759</ span > < span class = "keywordtype" > double</ span > dr2dt = 2*x*dxdt[j] + 2*y*dydt[j];</ div >
< div class = "line" >< a id = "l00760" name = "l00760" ></ a >< span class = "lineno" > 760</ span > < span class = "keywordtype" > double</ span > dcdist_dt = k[0]*dr2dt + 2*k[1]*r2*dr2dt + 3*k[4]*r4*dr2dt;</ div >
< div class = "line" >< a id = "l00761" name = "l00761" ></ a >< span class = "lineno" > 761</ span > < span class = "keywordtype" > double</ span > dicdist2_dt = -icdist2*icdist2*(k[5]*dr2dt + 2*k[6]*r2*dr2dt + 3*k[7]*r4*dr2dt);</ div >
< div class = "line" >< a id = "l00762" name = "l00762" ></ a >< span class = "lineno" > 762</ span > < span class = "keywordtype" > double</ span > da1dt = 2*(x*dydt[j] + y*dxdt[j]);</ div >
< div class = "line" >< a id = "l00763" name = "l00763" ></ a >< span class = "lineno" > 763</ span > < span class = "keywordtype" > double</ span > dmxdt = (dxdt[j]*cdist*icdist2 + x*dcdist_dt*icdist2 + x*cdist*dicdist2_dt +</ div >
< div class = "line" >< a id = "l00764" name = "l00764" ></ a >< span class = "lineno" > 764</ span > k[2]*da1dt + k[3]*(dr2dt + 4*x*dxdt[j]) + k[8]*dr2dt + 2*r2*k[9]*dr2dt);</ div >
< div class = "line" >< a id = "l00765" name = "l00765" ></ a >< span class = "lineno" > 765</ span > < span class = "keywordtype" > double</ span > dmydt = (dydt[j]*cdist*icdist2 + y*dcdist_dt*icdist2 + y*cdist*dicdist2_dt +</ div >
< div class = "line" >< a id = "l00766" name = "l00766" ></ a >< span class = "lineno" > 766</ span > k[2]*(dr2dt + 4*y*dydt[j]) + k[3]*da1dt + k[10]*dr2dt + 2*r2*k[11]*dr2dt);</ div >
< div class = "line" >< a id = "l00767" name = "l00767" ></ a >< span class = "lineno" > 767</ span > dXdYd = dMatTilt*cv::Vec2d(dmxdt, dmydt);</ div >
< div class = "line" >< a id = "l00768" name = "l00768" ></ a >< span class = "lineno" > 768</ span > dpdt_p[j] = fx*dXdYd(0);</ div >
< div class = "line" >< a id = "l00769" name = "l00769" ></ a >< span class = "lineno" > 769</ span > dpdt_p[dpdt_step+j] = fy*dXdYd(1);</ div >
< div class = "line" >< a id = "l00770" name = "l00770" ></ a >< span class = "lineno" > 770</ span > }</ div >
< div class = "line" >< a id = "l00771" name = "l00771" ></ a >< span class = "lineno" > 771</ span > dpdt_p += dpdt_step*2;</ div >
< div class = "line" >< a id = "l00772" name = "l00772" ></ a >< span class = "lineno" > 772</ span > }</ div >
< div class = "line" >< a id = "l00773" name = "l00773" ></ a >< span class = "lineno" > 773</ span > </ div >
< div class = "line" >< a id = "l00774" name = "l00774" ></ a >< span class = "lineno" > 774</ span > < span class = "keywordflow" > if</ span > ( dpdr_p )</ div >
< div class = "line" >< a id = "l00775" name = "l00775" ></ a >< span class = "lineno" > 775</ span > {</ div >
< div class = "line" >< a id = "l00776" name = "l00776" ></ a >< span class = "lineno" > 776</ span > < span class = "keywordtype" > double</ span > dx0dr[] =</ div >
< div class = "line" >< a id = "l00777" name = "l00777" ></ a >< span class = "lineno" > 777</ span > {</ div >
< div class = "line" >< a id = "l00778" name = "l00778" ></ a >< span class = "lineno" > 778</ span > X*dRdr[0] + Y*dRdr[1] + Z*dRdr[2],</ div >
< div class = "line" >< a id = "l00779" name = "l00779" ></ a >< span class = "lineno" > 779</ span > X*dRdr[9] + Y*dRdr[10] + Z*dRdr[11],</ div >
< div class = "line" >< a id = "l00780" name = "l00780" ></ a >< span class = "lineno" > 780</ span > X*dRdr[18] + Y*dRdr[19] + Z*dRdr[20]</ div >
< div class = "line" >< a id = "l00781" name = "l00781" ></ a >< span class = "lineno" > 781</ span > };</ div >
< div class = "line" >< a id = "l00782" name = "l00782" ></ a >< span class = "lineno" > 782</ span > < span class = "keywordtype" > double</ span > dy0dr[] =</ div >
< div class = "line" >< a id = "l00783" name = "l00783" ></ a >< span class = "lineno" > 783</ span > {</ div >
< div class = "line" >< a id = "l00784" name = "l00784" ></ a >< span class = "lineno" > 784</ span > X*dRdr[3] + Y*dRdr[4] + Z*dRdr[5],</ div >
< div class = "line" >< a id = "l00785" name = "l00785" ></ a >< span class = "lineno" > 785</ span > X*dRdr[12] + Y*dRdr[13] + Z*dRdr[14],</ div >
< div class = "line" >< a id = "l00786" name = "l00786" ></ a >< span class = "lineno" > 786</ span > X*dRdr[21] + Y*dRdr[22] + Z*dRdr[23]</ div >
< div class = "line" >< a id = "l00787" name = "l00787" ></ a >< span class = "lineno" > 787</ span > };</ div >
< div class = "line" >< a id = "l00788" name = "l00788" ></ a >< span class = "lineno" > 788</ span > < span class = "keywordtype" > double</ span > dz0dr[] =</ div >
< div class = "line" >< a id = "l00789" name = "l00789" ></ a >< span class = "lineno" > 789</ span > {</ div >
< div class = "line" >< a id = "l00790" name = "l00790" ></ a >< span class = "lineno" > 790</ span > X*dRdr[6] + Y*dRdr[7] + Z*dRdr[8],</ div >
< div class = "line" >< a id = "l00791" name = "l00791" ></ a >< span class = "lineno" > 791</ span > X*dRdr[15] + Y*dRdr[16] + Z*dRdr[17],</ div >
< div class = "line" >< a id = "l00792" name = "l00792" ></ a >< span class = "lineno" > 792</ span > X*dRdr[24] + Y*dRdr[25] + Z*dRdr[26]</ div >
< div class = "line" >< a id = "l00793" name = "l00793" ></ a >< span class = "lineno" > 793</ span > };</ div >
< div class = "line" >< a id = "l00794" name = "l00794" ></ a >< span class = "lineno" > 794</ span > < span class = "keywordflow" > for</ span > ( j = 0; j < 3; j++ )</ div >
< div class = "line" >< a id = "l00795" name = "l00795" ></ a >< span class = "lineno" > 795</ span > {</ div >
< div class = "line" >< a id = "l00796" name = "l00796" ></ a >< span class = "lineno" > 796</ span > < span class = "keywordtype" > double</ span > dxdr = z*(dx0dr[j] - x*dz0dr[j]);</ div >
< div class = "line" >< a id = "l00797" name = "l00797" ></ a >< span class = "lineno" > 797</ span > < span class = "keywordtype" > double</ span > dydr = z*(dy0dr[j] - y*dz0dr[j]);</ div >
< div class = "line" >< a id = "l00798" name = "l00798" ></ a >< span class = "lineno" > 798</ span > < span class = "keywordtype" > double</ span > dr2dr = 2*x*dxdr + 2*y*dydr;</ div >
< div class = "line" >< a id = "l00799" name = "l00799" ></ a >< span class = "lineno" > 799</ span > < span class = "keywordtype" > double</ span > dcdist_dr = (k[0] + 2*k[1]*r2 + 3*k[4]*r4)*dr2dr;</ div >
< div class = "line" >< a id = "l00800" name = "l00800" ></ a >< span class = "lineno" > 800</ span > < span class = "keywordtype" > double</ span > dicdist2_dr = -icdist2*icdist2*(k[5] + 2*k[6]*r2 + 3*k[7]*r4)*dr2dr;</ div >
< div class = "line" >< a id = "l00801" name = "l00801" ></ a >< span class = "lineno" > 801</ span > < span class = "keywordtype" > double</ span > da1dr = 2*(x*dydr + y*dxdr);</ div >
< div class = "line" >< a id = "l00802" name = "l00802" ></ a >< span class = "lineno" > 802</ span > < span class = "keywordtype" > double</ span > dmxdr = (dxdr*cdist*icdist2 + x*dcdist_dr*icdist2 + x*cdist*dicdist2_dr +</ div >
< div class = "line" >< a id = "l00803" name = "l00803" ></ a >< span class = "lineno" > 803</ span > k[2]*da1dr + k[3]*(dr2dr + 4*x*dxdr) + (k[8] + 2*r2*k[9])*dr2dr);</ div >
< div class = "line" >< a id = "l00804" name = "l00804" ></ a >< span class = "lineno" > 804</ span > < span class = "keywordtype" > double</ span > dmydr = (dydr*cdist*icdist2 + y*dcdist_dr*icdist2 + y*cdist*dicdist2_dr +</ div >
< div class = "line" >< a id = "l00805" name = "l00805" ></ a >< span class = "lineno" > 805</ span > k[2]*(dr2dr + 4*y*dydr) + k[3]*da1dr + (k[10] + 2*r2*k[11])*dr2dr);</ div >
< div class = "line" >< a id = "l00806" name = "l00806" ></ a >< span class = "lineno" > 806</ span > dXdYd = dMatTilt*cv::Vec2d(dmxdr, dmydr);</ div >
< div class = "line" >< a id = "l00807" name = "l00807" ></ a >< span class = "lineno" > 807</ span > dpdr_p[j] = fx*dXdYd(0);</ div >
< div class = "line" >< a id = "l00808" name = "l00808" ></ a >< span class = "lineno" > 808</ span > dpdr_p[dpdr_step+j] = fy*dXdYd(1);</ div >
< div class = "line" >< a id = "l00809" name = "l00809" ></ a >< span class = "lineno" > 809</ span > }</ div >
< div class = "line" >< a id = "l00810" name = "l00810" ></ a >< span class = "lineno" > 810</ span > dpdr_p += dpdr_step*2;</ div >
< div class = "line" >< a id = "l00811" name = "l00811" ></ a >< span class = "lineno" > 811</ span > }</ div >
< div class = "line" >< a id = "l00812" name = "l00812" ></ a >< span class = "lineno" > 812</ span > </ div >
< div class = "line" >< a id = "l00813" name = "l00813" ></ a >< span class = "lineno" > 813</ span > < span class = "keywordflow" > if</ span > ( dpdo_p )</ div >
< div class = "line" >< a id = "l00814" name = "l00814" ></ a >< span class = "lineno" > 814</ span > {</ div >
< div class = "line" >< a id = "l00815" name = "l00815" ></ a >< span class = "lineno" > 815</ span > < span class = "keywordtype" > double</ span > dxdo[] = { z * ( R[0] - x * z * z0 * R[6] ),</ div >
< div class = "line" >< a id = "l00816" name = "l00816" ></ a >< span class = "lineno" > 816</ span > z * ( R[1] - x * z * z0 * R[7] ),</ div >
< div class = "line" >< a id = "l00817" name = "l00817" ></ a >< span class = "lineno" > 817</ span > z * ( R[2] - x * z * z0 * R[8] ) };</ div >
< div class = "line" >< a id = "l00818" name = "l00818" ></ a >< span class = "lineno" > 818</ span > < span class = "keywordtype" > double</ span > dydo[] = { z * ( R[3] - y * z * z0 * R[6] ),</ div >
< div class = "line" >< a id = "l00819" name = "l00819" ></ a >< span class = "lineno" > 819</ span > z * ( R[4] - y * z * z0 * R[7] ),</ div >
< div class = "line" >< a id = "l00820" name = "l00820" ></ a >< span class = "lineno" > 820</ span > z * ( R[5] - y * z * z0 * R[8] ) };</ div >
< div class = "line" >< a id = "l00821" name = "l00821" ></ a >< span class = "lineno" > 821</ span > < span class = "keywordflow" > for</ span > ( j = 0; j < 3; j++ )</ div >
< div class = "line" >< a id = "l00822" name = "l00822" ></ a >< span class = "lineno" > 822</ span > {</ div >
< div class = "line" >< a id = "l00823" name = "l00823" ></ a >< span class = "lineno" > 823</ span > < span class = "keywordtype" > double</ span > dr2do = 2 * x * dxdo[j] + 2 * y * dydo[j];</ div >
< div class = "line" >< a id = "l00824" name = "l00824" ></ a >< span class = "lineno" > 824</ span > < span class = "keywordtype" > double</ span > dr4do = 2 * r2 * dr2do;</ div >
< div class = "line" >< a id = "l00825" name = "l00825" ></ a >< span class = "lineno" > 825</ span > < span class = "keywordtype" > double</ span > dr6do = 3 * r4 * dr2do;</ div >
< div class = "line" >< a id = "l00826" name = "l00826" ></ a >< span class = "lineno" > 826</ span > < span class = "keywordtype" > double</ span > da1do = 2 * y * dxdo[j] + 2 * x * dydo[j];</ div >
< div class = "line" >< a id = "l00827" name = "l00827" ></ a >< span class = "lineno" > 827</ span > < span class = "keywordtype" > double</ span > da2do = dr2do + 4 * x * dxdo[j];</ div >
< div class = "line" >< a id = "l00828" name = "l00828" ></ a >< span class = "lineno" > 828</ span > < span class = "keywordtype" > double</ span > da3do = dr2do + 4 * y * dydo[j];</ div >
< div class = "line" >< a id = "l00829" name = "l00829" ></ a >< span class = "lineno" > 829</ span > < span class = "keywordtype" > double</ span > dcdist_do</ div >
< div class = "line" >< a id = "l00830" name = "l00830" ></ a >< span class = "lineno" > 830</ span > = k[0] * dr2do + k[1] * dr4do + k[4] * dr6do;</ div >
< div class = "line" >< a id = "l00831" name = "l00831" ></ a >< span class = "lineno" > 831</ span > < span class = "keywordtype" > double</ span > dicdist2_do = -icdist2 * icdist2</ div >
< div class = "line" >< a id = "l00832" name = "l00832" ></ a >< span class = "lineno" > 832</ span > * ( k[5] * dr2do + k[6] * dr4do + k[7] * dr6do );</ div >
< div class = "line" >< a id = "l00833" name = "l00833" ></ a >< span class = "lineno" > 833</ span > < span class = "keywordtype" > double</ span > dxd0_do = cdist * icdist2 * dxdo[j]</ div >
< div class = "line" >< a id = "l00834" name = "l00834" ></ a >< span class = "lineno" > 834</ span > + x * icdist2 * dcdist_do + x * cdist * dicdist2_do</ div >
< div class = "line" >< a id = "l00835" name = "l00835" ></ a >< span class = "lineno" > 835</ span > + k[2] * da1do + k[3] * da2do + k[8] * dr2do</ div >
< div class = "line" >< a id = "l00836" name = "l00836" ></ a >< span class = "lineno" > 836</ span > + k[9] * dr4do;</ div >
< div class = "line" >< a id = "l00837" name = "l00837" ></ a >< span class = "lineno" > 837</ span > < span class = "keywordtype" > double</ span > dyd0_do = cdist * icdist2 * dydo[j]</ div >
< div class = "line" >< a id = "l00838" name = "l00838" ></ a >< span class = "lineno" > 838</ span > + y * icdist2 * dcdist_do + y * cdist * dicdist2_do</ div >
< div class = "line" >< a id = "l00839" name = "l00839" ></ a >< span class = "lineno" > 839</ span > + k[2] * da3do + k[3] * da1do + k[10] * dr2do</ div >
< div class = "line" >< a id = "l00840" name = "l00840" ></ a >< span class = "lineno" > 840</ span > + k[11] * dr4do;</ div >
< div class = "line" >< a id = "l00841" name = "l00841" ></ a >< span class = "lineno" > 841</ span > dXdYd = dMatTilt * cv::Vec2d( dxd0_do, dyd0_do );</ div >
< div class = "line" >< a id = "l00842" name = "l00842" ></ a >< span class = "lineno" > 842</ span > dpdo_p[i * 3 + j] = fx * dXdYd( 0 );</ div >
< div class = "line" >< a id = "l00843" name = "l00843" ></ a >< span class = "lineno" > 843</ span > dpdo_p[dpdo_step + i * 3 + j] = fy * dXdYd( 1 );</ div >
< div class = "line" >< a id = "l00844" name = "l00844" ></ a >< span class = "lineno" > 844</ span > }</ div >
< div class = "line" >< a id = "l00845" name = "l00845" ></ a >< span class = "lineno" > 845</ span > dpdo_p += dpdo_step * 2;</ div >
< div class = "line" >< a id = "l00846" name = "l00846" ></ a >< span class = "lineno" > 846</ span > }</ div >
< div class = "line" >< a id = "l00847" name = "l00847" ></ a >< span class = "lineno" > 847</ span > }</ div >
< div class = "line" >< a id = "l00848" name = "l00848" ></ a >< span class = "lineno" > 848</ span > }</ div >
< div class = "line" >< a id = "l00849" name = "l00849" ></ a >< span class = "lineno" > 849</ span > </ div >
< div class = "line" >< a id = "l00850" name = "l00850" ></ a >< span class = "lineno" > 850</ span > < span class = "keywordflow" > if</ span > ( _m != imagePoints )</ div >
< div class = "line" >< a id = "l00851" name = "l00851" ></ a >< span class = "lineno" > 851</ span > cvConvert( _m, imagePoints );</ div >
< div class = "line" >< a id = "l00852" name = "l00852" ></ a >< span class = "lineno" > 852</ span > </ div >
< div class = "line" >< a id = "l00853" name = "l00853" ></ a >< span class = "lineno" > 853</ span > < span class = "keywordflow" > if</ span > ( _dpdr != dpdr )</ div >
< div class = "line" >< a id = "l00854" name = "l00854" ></ a >< span class = "lineno" > 854</ span > cvConvert( _dpdr, dpdr );</ div >
< div class = "line" >< a id = "l00855" name = "l00855" ></ a >< span class = "lineno" > 855</ span > </ div >
< div class = "line" >< a id = "l00856" name = "l00856" ></ a >< span class = "lineno" > 856</ span > < span class = "keywordflow" > if</ span > ( _dpdt != dpdt )</ div >
< div class = "line" >< a id = "l00857" name = "l00857" ></ a >< span class = "lineno" > 857</ span > cvConvert( _dpdt, dpdt );</ div >
< div class = "line" >< a id = "l00858" name = "l00858" ></ a >< span class = "lineno" > 858</ span > </ div >
< div class = "line" >< a id = "l00859" name = "l00859" ></ a >< span class = "lineno" > 859</ span > < span class = "keywordflow" > if</ span > ( _dpdf != dpdf )</ div >
< div class = "line" >< a id = "l00860" name = "l00860" ></ a >< span class = "lineno" > 860</ span > cvConvert( _dpdf, dpdf );</ div >
< div class = "line" >< a id = "l00861" name = "l00861" ></ a >< span class = "lineno" > 861</ span > </ div >
< div class = "line" >< a id = "l00862" name = "l00862" ></ a >< span class = "lineno" > 862</ span > < span class = "keywordflow" > if</ span > ( _dpdc != dpdc )</ div >
< div class = "line" >< a id = "l00863" name = "l00863" ></ a >< span class = "lineno" > 863</ span > cvConvert( _dpdc, dpdc );</ div >
< div class = "line" >< a id = "l00864" name = "l00864" ></ a >< span class = "lineno" > 864</ span > </ div >
< div class = "line" >< a id = "l00865" name = "l00865" ></ a >< span class = "lineno" > 865</ span > < span class = "keywordflow" > if</ span > ( _dpdk != dpdk )</ div >
< div class = "line" >< a id = "l00866" name = "l00866" ></ a >< span class = "lineno" > 866</ span > cvConvert( _dpdk, dpdk );</ div >
< div class = "line" >< a id = "l00867" name = "l00867" ></ a >< span class = "lineno" > 867</ span > </ div >
< div class = "line" >< a id = "l00868" name = "l00868" ></ a >< span class = "lineno" > 868</ span > < span class = "keywordflow" > if</ span > ( _dpdo != dpdo )</ div >
< div class = "line" >< a id = "l00869" name = "l00869" ></ a >< span class = "lineno" > 869</ span > cvConvert( _dpdo, dpdo );</ div >
< div class = "line" >< a id = "l00870" name = "l00870" ></ a >< span class = "lineno" > 870</ span > }</ div >
< div class = "line" >< a id = "l00871" name = "l00871" ></ a >< span class = "lineno" > 871</ span > </ div >
< div class = "line" >< a id = "l00872" name = "l00872" ></ a >< span class = "lineno" > 872</ span >< span class = "keywordtype" > void</ span > cvProjectPoints2( < span class = "keyword" > const</ span > CvMat* objectPoints,</ div >
< div class = "line" >< a id = "l00873" name = "l00873" ></ a >< span class = "lineno" > 873</ span > < span class = "keyword" > const</ span > CvMat* r_vec,</ div >
< div class = "line" >< a id = "l00874" name = "l00874" ></ a >< span class = "lineno" > 874</ span > < span class = "keyword" > const</ span > CvMat* t_vec,</ div >
< div class = "line" >< a id = "l00875" name = "l00875" ></ a >< span class = "lineno" > 875</ span > < span class = "keyword" > const</ span > CvMat* A,</ div >
< div class = "line" >< a id = "l00876" name = "l00876" ></ a >< span class = "lineno" > 876</ span > < span class = "keyword" > const</ span > CvMat* distCoeffs,</ div >
< div class = "line" >< a id = "l00877" name = "l00877" ></ a >< span class = "lineno" > 877</ span > CvMat* imagePoints, CvMat* dpdr CV_DEFAULT(NULL),</ div >
< div class = "line" >< a id = "l00878" name = "l00878" ></ a >< span class = "lineno" > 878</ span > CvMat* dpdt CV_DEFAULT(NULL), CvMat* dpdf CV_DEFAULT(NULL),</ div >
< div class = "line" >< a id = "l00879" name = "l00879" ></ a >< span class = "lineno" > 879</ span > CvMat* dpdc CV_DEFAULT(NULL), CvMat* dpdk CV_DEFAULT(NULL),</ div >
< div class = "line" >< a id = "l00880" name = "l00880" ></ a >< span class = "lineno" > 880</ span > < span class = "keywordtype" > double</ span > aspectRatio CV_DEFAULT(0))</ div >
< div class = "line" >< a id = "l00881" name = "l00881" ></ a >< span class = "lineno" > 881</ span > {</ div >
< div class = "line" >< a id = "l00882" name = "l00882" ></ a >< span class = "lineno" > 882</ span > cvProjectPoints2Internal( objectPoints, r_vec, t_vec, A, distCoeffs, imagePoints, dpdr, dpdt,</ div >
< div class = "line" >< a id = "l00883" name = "l00883" ></ a >< span class = "lineno" > 883</ span > dpdf, dpdc, dpdk, NULL, aspectRatio );</ div >
< div class = "line" >< a id = "l00884" name = "l00884" ></ a >< span class = "lineno" > 884</ span > }</ div >
< div class = "line" >< a id = "l00885" name = "l00885" ></ a >< span class = "lineno" > 885</ span > </ div >
< div class = "line" >< a id = "l00886" name = "l00886" ></ a >< span class = "lineno" > 886</ span >< span class = "keywordtype" > void</ span > cvConvertPointsHomogeneous( < span class = "keyword" > const</ span > CvMat* _src, CvMat* _dst )</ div >
< div class = "line" >< a id = "l00887" name = "l00887" ></ a >< span class = "lineno" > 887</ span > {</ div >
< div class = "line" >< a id = "l00888" name = "l00888" ></ a >< span class = "lineno" > 888</ span > cv::Mat src = cv::cvarrToMat(_src), dst = cv::cvarrToMat(_dst);</ div >
< div class = "line" >< a id = "l00889" name = "l00889" ></ a >< span class = "lineno" > 889</ span > < span class = "keyword" > const</ span > cv::Mat dst0 = dst;</ div >
< div class = "line" >< a id = "l00890" name = "l00890" ></ a >< span class = "lineno" > 890</ span > </ div >
< div class = "line" >< a id = "l00891" name = "l00891" ></ a >< span class = "lineno" > 891</ span > < span class = "keywordtype" > int</ span > d0 = src.channels() > 1 ? src.channels() : MIN(src.cols, src.rows);</ div >
< div class = "line" >< a id = "l00892" name = "l00892" ></ a >< span class = "lineno" > 892</ span > </ div >
< div class = "line" >< a id = "l00893" name = "l00893" ></ a >< span class = "lineno" > 893</ span > < span class = "keywordflow" > if</ span > ( src.channels() == 1 && src.cols > d0 )</ div >
< div class = "line" >< a id = "l00894" name = "l00894" ></ a >< span class = "lineno" > 894</ span > cv::transpose(src, src);</ div >
< div class = "line" >< a id = "l00895" name = "l00895" ></ a >< span class = "lineno" > 895</ span > </ div >
< div class = "line" >< a id = "l00896" name = "l00896" ></ a >< span class = "lineno" > 896</ span > < span class = "keywordtype" > int</ span > d1 = dst.channels() > 1 ? dst.channels() : MIN(dst.cols, dst.rows);</ div >
< div class = "line" >< a id = "l00897" name = "l00897" ></ a >< span class = "lineno" > 897</ span > </ div >
< div class = "line" >< a id = "l00898" name = "l00898" ></ a >< span class = "lineno" > 898</ span > < span class = "keywordflow" > if</ span > ( d0 == d1 )</ div >
< div class = "line" >< a id = "l00899" name = "l00899" ></ a >< span class = "lineno" > 899</ span > src.copyTo(dst);</ div >
< div class = "line" >< a id = "l00900" name = "l00900" ></ a >< span class = "lineno" > 900</ span > < span class = "keywordflow" > else</ span > < span class = "keywordflow" > if</ span > ( d0 < d1 )</ div >
< div class = "line" >< a id = "l00901" name = "l00901" ></ a >< span class = "lineno" > 901</ span > cv::convertPointsToHomogeneous(src, dst);</ div >
< div class = "line" >< a id = "l00902" name = "l00902" ></ a >< span class = "lineno" > 902</ span > < span class = "keywordflow" > else</ span ></ div >
< div class = "line" >< a id = "l00903" name = "l00903" ></ a >< span class = "lineno" > 903</ span > cv::convertPointsFromHomogeneous(src, dst);</ div >
< div class = "line" >< a id = "l00904" name = "l00904" ></ a >< span class = "lineno" > 904</ span > </ div >
< div class = "line" >< a id = "l00905" name = "l00905" ></ a >< span class = "lineno" > 905</ span > < span class = "keywordtype" > bool</ span > tflag = dst0.channels() == 1 && dst0.cols > d1;</ div >
< div class = "line" >< a id = "l00906" name = "l00906" ></ a >< span class = "lineno" > 906</ span > dst = dst.reshape(dst0.channels(), (tflag ? dst0.cols : dst0.rows));</ div >
< div class = "line" >< a id = "l00907" name = "l00907" ></ a >< span class = "lineno" > 907</ span > </ div >
< div class = "line" >< a id = "l00908" name = "l00908" ></ a >< span class = "lineno" > 908</ span > < span class = "keywordflow" > if</ span > ( tflag )</ div >
< div class = "line" >< a id = "l00909" name = "l00909" ></ a >< span class = "lineno" > 909</ span > {</ div >
< div class = "line" >< a id = "l00910" name = "l00910" ></ a >< span class = "lineno" > 910</ span > CV_Assert( dst.rows == dst0.cols && dst.cols == dst0.rows );</ div >
< div class = "line" >< a id = "l00911" name = "l00911" ></ a >< span class = "lineno" > 911</ span > < span class = "keywordflow" > if</ span > ( dst0.type() == dst.type() )</ div >
< div class = "line" >< a id = "l00912" name = "l00912" ></ a >< span class = "lineno" > 912</ span > transpose( dst, dst0 );</ div >
< div class = "line" >< a id = "l00913" name = "l00913" ></ a >< span class = "lineno" > 913</ span > < span class = "keywordflow" > else</ span ></ div >
< div class = "line" >< a id = "l00914" name = "l00914" ></ a >< span class = "lineno" > 914</ span > {</ div >
< div class = "line" >< a id = "l00915" name = "l00915" ></ a >< span class = "lineno" > 915</ span > transpose( dst, dst );</ div >
< div class = "line" >< a id = "l00916" name = "l00916" ></ a >< span class = "lineno" > 916</ span > dst.convertTo( dst0, dst0.type() );</ div >
< div class = "line" >< a id = "l00917" name = "l00917" ></ a >< span class = "lineno" > 917</ span > }</ div >
< div class = "line" >< a id = "l00918" name = "l00918" ></ a >< span class = "lineno" > 918</ span > }</ div >
< div class = "line" >< a id = "l00919" name = "l00919" ></ a >< span class = "lineno" > 919</ span > < span class = "keywordflow" > else</ span ></ div >
< div class = "line" >< a id = "l00920" name = "l00920" ></ a >< span class = "lineno" > 920</ span > {</ div >
< div class = "line" >< a id = "l00921" name = "l00921" ></ a >< span class = "lineno" > 921</ span > CV_Assert( dst.size() == dst0.size() );</ div >
< div class = "line" >< a id = "l00922" name = "l00922" ></ a >< span class = "lineno" > 922</ span > < span class = "keywordflow" > if</ span > ( dst.data != dst0.data )</ div >
< div class = "line" >< a id = "l00923" name = "l00923" ></ a >< span class = "lineno" > 923</ span > dst.convertTo(dst0, dst0.type());</ div >
< div class = "line" >< a id = "l00924" name = "l00924" ></ a >< span class = "lineno" > 924</ span > }</ div >
< div class = "line" >< a id = "l00925" name = "l00925" ></ a >< span class = "lineno" > 925</ span > }</ div >
< div class = "line" >< a id = "l00926" name = "l00926" ></ a >< span class = "lineno" > 926</ span > </ div >
< div class = "line" >< a id = "l00927" name = "l00927" ></ a >< span class = "lineno" > 927</ span >< span class = "preprocessor" > #endif </ span >< span class = "comment" > // OpenCV4</ span ></ div >
< div class = "line" >< a id = "l00928" name = "l00928" ></ a >< span class = "lineno" > 928</ span > </ div >
< div class = "line" >< a id = "l00929" name = "l00929" ></ a >< span class = "lineno" > 929</ span >< span class = "preprocessor" > #endif </ span >< span class = "comment" > // OpenCV3</ span ></ div >
< div class = "line" >< a id = "l00930" name = "l00930" ></ a >< span class = "lineno" > 930</ span > </ div >
< div class = "line" >< a id = "l00931" name = "l00931" ></ a >< span class = "lineno" > 931</ span >< span class = "keyword" > namespace </ span >< a class = "code hl_namespace" href = "namespacertabmap.html" > rtabmap</ a ></ div >
< div class = "line" >< a id = "l00932" name = "l00932" ></ a >< span class = "lineno" > 932</ span > {</ div >
< div class = "line" >< a id = "l00933" name = "l00933" ></ a >< span class = "lineno" > 933</ span > </ div >
< div class = "line" >< a id = "l00934" name = "l00934" ></ a >< span class = "lineno" > 934</ span >< span class = "keywordtype" > void</ span ></ div >
< div class = "line" >< a id = "l00935" name = "l00935" ></ a >< span class = "lineno" > 935</ span > icvGetRectanglesFisheye( < span class = "keyword" > const</ span > CvMat* cameraMatrix, < span class = "keyword" > const</ span > CvMat* distCoeffs,</ div >
< div class = "line" >< a id = "l00936" name = "l00936" ></ a >< span class = "lineno" > 936</ span > < span class = "keyword" > const</ span > CvMat* R, < span class = "keyword" > const</ span > CvMat* newCameraMatrix, CvSize imgSize,</ div >
< div class = "line" >< a id = "l00937" name = "l00937" ></ a >< span class = "lineno" > 937</ span > cv::Rect_< float>& inner, cv::Rect_< float>& outer )</ div >
< div class = "line" >< a id = "l00938" name = "l00938" ></ a >< span class = "lineno" > 938</ span > {</ div >
< div class = "line" >< a id = "l00939" name = "l00939" ></ a >< span class = "lineno" > 939</ span > < span class = "keyword" > const</ span > < span class = "keywordtype" > int</ span > N = 9;</ div >
< div class = "line" >< a id = "l00940" name = "l00940" ></ a >< span class = "lineno" > 940</ span > < span class = "keywordtype" > int</ span > x, y, k;</ div >
< div class = "line" >< a id = "l00941" name = "l00941" ></ a >< span class = "lineno" > 941</ span > cv::Mat _pts(1, N*N, CV_32FC2);</ div >
< div class = "line" >< a id = "l00942" name = "l00942" ></ a >< span class = "lineno" > 942</ span > CvPoint2D32f* pts = (CvPoint2D32f*)(_pts.data);</ div >
< div class = "line" >< a id = "l00943" name = "l00943" ></ a >< span class = "lineno" > 943</ span > </ div >
< div class = "line" >< a id = "l00944" name = "l00944" ></ a >< span class = "lineno" > 944</ span > < span class = "keywordflow" > for</ span > ( y = k = 0; y < N; y++ )</ div >
< div class = "line" >< a id = "l00945" name = "l00945" ></ a >< span class = "lineno" > 945</ span > < span class = "keywordflow" > for</ span > ( x = 0; x < N; x++ )</ div >
< div class = "line" >< a id = "l00946" name = "l00946" ></ a >< span class = "lineno" > 946</ span > pts[k++] = cvPoint2D32f((< span class = "keywordtype" > float</ span > )x*imgSize.width/(N-1),</ div >
< div class = "line" >< a id = "l00947" name = "l00947" ></ a >< span class = "lineno" > 947</ span > (< span class = "keywordtype" > float</ span > )y*imgSize.height/(N-1));</ div >
< div class = "line" >< a id = "l00948" name = "l00948" ></ a >< span class = "lineno" > 948</ span > </ div >
< div class = "line" >< a id = "l00949" name = "l00949" ></ a >< span class = "lineno" > 949</ span > cv::Mat cameraMatrixM(cameraMatrix-> rows, cameraMatrix-> cols, cameraMatrix-> type, cameraMatrix-> data.ptr);</ div >
< div class = "line" >< a id = "l00950" name = "l00950" ></ a >< span class = "lineno" > 950</ span > cv::Mat distCoeffsM(distCoeffs-> rows, distCoeffs-> cols, distCoeffs-> type, distCoeffs-> data.ptr);</ div >
< div class = "line" >< a id = "l00951" name = "l00951" ></ a >< span class = "lineno" > 951</ span > cv::Mat RM(R-> rows, R-> cols, R-> type, R-> data.ptr);</ div >
< div class = "line" >< a id = "l00952" name = "l00952" ></ a >< span class = "lineno" > 952</ span > cv::Mat newCameraMatrixM(newCameraMatrix-> rows, newCameraMatrix-> cols, newCameraMatrix-> type, newCameraMatrix-> data.ptr);</ div >
< div class = "line" >< a id = "l00953" name = "l00953" ></ a >< span class = "lineno" > 953</ span > cv::fisheye::undistortPoints(_pts, _pts, cameraMatrixM, distCoeffsM, RM, newCameraMatrixM);</ div >
< div class = "line" >< a id = "l00954" name = "l00954" ></ a >< span class = "lineno" > 954</ span > < span class = "keywordtype" > float</ span > iX0=-FLT_MAX, iX1=FLT_MAX, iY0=-FLT_MAX, iY1=FLT_MAX;</ div >
< div class = "line" >< a id = "l00955" name = "l00955" ></ a >< span class = "lineno" > 955</ span > < span class = "keywordtype" > float</ span > oX0=FLT_MAX, oX1=-FLT_MAX, oY0=FLT_MAX, oY1=-FLT_MAX;</ div >
< div class = "line" >< a id = "l00956" name = "l00956" ></ a >< span class = "lineno" > 956</ span > < span class = "comment" > // find the inscribed rectangle.</ span ></ div >
< div class = "line" >< a id = "l00957" name = "l00957" ></ a >< span class = "lineno" > 957</ span > < span class = "comment" > // the code will likely not work with extreme rotation matrices (R) (> 45%)</ span ></ div >
< div class = "line" >< a id = "l00958" name = "l00958" ></ a >< span class = "lineno" > 958</ span > < span class = "keywordflow" > for</ span > ( y = k = 0; y < N; y++ )</ div >
< div class = "line" >< a id = "l00959" name = "l00959" ></ a >< span class = "lineno" > 959</ span > < span class = "keywordflow" > for</ span > ( x = 0; x < N; x++ )</ div >
< div class = "line" >< a id = "l00960" name = "l00960" ></ a >< span class = "lineno" > 960</ span > {</ div >
< div class = "line" >< a id = "l00961" name = "l00961" ></ a >< span class = "lineno" > 961</ span > CvPoint2D32f p = pts[k++];</ div >
< div class = "line" >< a id = "l00962" name = "l00962" ></ a >< span class = "lineno" > 962</ span > oX0 = MIN(oX0, p.x);</ div >
< div class = "line" >< a id = "l00963" name = "l00963" ></ a >< span class = "lineno" > 963</ span > oX1 = MAX(oX1, p.x);</ div >
< div class = "line" >< a id = "l00964" name = "l00964" ></ a >< span class = "lineno" > 964</ span > oY0 = MIN(oY0, p.y);</ div >
< div class = "line" >< a id = "l00965" name = "l00965" ></ a >< span class = "lineno" > 965</ span > oY1 = MAX(oY1, p.y);</ div >
< div class = "line" >< a id = "l00966" name = "l00966" ></ a >< span class = "lineno" > 966</ span > </ div >
< div class = "line" >< a id = "l00967" name = "l00967" ></ a >< span class = "lineno" > 967</ span > < span class = "keywordflow" > if</ span > ( x == 0 )</ div >
< div class = "line" >< a id = "l00968" name = "l00968" ></ a >< span class = "lineno" > 968</ span > iX0 = MAX(iX0, p.x);</ div >
< div class = "line" >< a id = "l00969" name = "l00969" ></ a >< span class = "lineno" > 969</ span > < span class = "keywordflow" > if</ span > ( x == N-1 )</ div >
< div class = "line" >< a id = "l00970" name = "l00970" ></ a >< span class = "lineno" > 970</ span > iX1 = MIN(iX1, p.x);</ div >
< div class = "line" >< a id = "l00971" name = "l00971" ></ a >< span class = "lineno" > 971</ span > < span class = "keywordflow" > if</ span > ( y == 0 )</ div >
< div class = "line" >< a id = "l00972" name = "l00972" ></ a >< span class = "lineno" > 972</ span > iY0 = MAX(iY0, p.y);</ div >
< div class = "line" >< a id = "l00973" name = "l00973" ></ a >< span class = "lineno" > 973</ span > < span class = "keywordflow" > if</ span > ( y == N-1 )</ div >
< div class = "line" >< a id = "l00974" name = "l00974" ></ a >< span class = "lineno" > 974</ span > iY1 = MIN(iY1, p.y);</ div >
< div class = "line" >< a id = "l00975" name = "l00975" ></ a >< span class = "lineno" > 975</ span > }</ div >
< div class = "line" >< a id = "l00976" name = "l00976" ></ a >< span class = "lineno" > 976</ span > inner = cv::Rect_< float> (iX0, iY0, iX1-iX0, iY1-iY0);</ div >
< div class = "line" >< a id = "l00977" name = "l00977" ></ a >< span class = "lineno" > 977</ span > outer = cv::Rect_< float> (oX0, oY0, oX1-oX0, oY1-oY0);</ div >
< div class = "line" >< a id = "l00978" name = "l00978" ></ a >< span class = "lineno" > 978</ span > }</ div >
< div class = "line" >< a id = "l00979" name = "l00979" ></ a >< span class = "lineno" > 979</ span > </ div >
< div class = "line" >< a id = "l00980" name = "l00980" ></ a >< span class = "lineno" > 980</ span >< span class = "keywordtype" > void</ span > cvStereoRectifyFisheye( < span class = "keyword" > const</ span > CvMat* _cameraMatrix1, < span class = "keyword" > const</ span > CvMat* _cameraMatrix2,</ div >
< div class = "line" >< a id = "l00981" name = "l00981" ></ a >< span class = "lineno" > 981</ span > < span class = "keyword" > const</ span > CvMat* _distCoeffs1, < span class = "keyword" > const</ span > CvMat* _distCoeffs2,</ div >
< div class = "line" >< a id = "l00982" name = "l00982" ></ a >< span class = "lineno" > 982</ span > CvSize imageSize, < span class = "keyword" > const</ span > CvMat* matR, < span class = "keyword" > const</ span > CvMat* matT,</ div >
< div class = "line" >< a id = "l00983" name = "l00983" ></ a >< span class = "lineno" > 983</ span > CvMat* _R1, CvMat* _R2, CvMat* _P1, CvMat* _P2,</ div >
< div class = "line" >< a id = "l00984" name = "l00984" ></ a >< span class = "lineno" > 984</ span > CvMat* matQ, < span class = "keywordtype" > int</ span > flags, < span class = "keywordtype" > double</ span > alpha, CvSize newImgSize )</ div >
< div class = "line" >< a id = "l00985" name = "l00985" ></ a >< span class = "lineno" > 985</ span > {</ div >
< div class = "line" >< a id = "l00986" name = "l00986" ></ a >< span class = "lineno" > 986</ span > < span class = "keywordtype" > double</ span > _om[3], _t[3] = {0}, _uu[3]={0,0,0}, _r_r[3][3], _pp[3][4];</ div >
< div class = "line" >< a id = "l00987" name = "l00987" ></ a >< span class = "lineno" > 987</ span > < span class = "keywordtype" > double</ span > _ww[3], _wr[3][3], _z[3] = {0,0,0}, _ri[3][3], _w3[3];</ div >
< div class = "line" >< a id = "l00988" name = "l00988" ></ a >< span class = "lineno" > 988</ span > cv::Rect_< float> inner1, inner2, outer1, outer2;</ div >
< div class = "line" >< a id = "l00989" name = "l00989" ></ a >< span class = "lineno" > 989</ span > </ div >
< div class = "line" >< a id = "l00990" name = "l00990" ></ a >< span class = "lineno" > 990</ span > CvMat om = cvMat(3, 1, CV_64F, _om);</ div >
< div class = "line" >< a id = "l00991" name = "l00991" ></ a >< span class = "lineno" > 991</ span > CvMat t = cvMat(3, 1, CV_64F, _t);</ div >
< div class = "line" >< a id = "l00992" name = "l00992" ></ a >< span class = "lineno" > 992</ span > CvMat uu = cvMat(3, 1, CV_64F, _uu);</ div >
< div class = "line" >< a id = "l00993" name = "l00993" ></ a >< span class = "lineno" > 993</ span > CvMat r_r = cvMat(3, 3, CV_64F, _r_r);</ div >
< div class = "line" >< a id = "l00994" name = "l00994" ></ a >< span class = "lineno" > 994</ span > CvMat pp = cvMat(3, 4, CV_64F, _pp);</ div >
< div class = "line" >< a id = "l00995" name = "l00995" ></ a >< span class = "lineno" > 995</ span > CvMat ww = cvMat(3, 1, CV_64F, _ww); < span class = "comment" > // temps</ span ></ div >
< div class = "line" >< a id = "l00996" name = "l00996" ></ a >< span class = "lineno" > 996</ span > CvMat w3 = cvMat(3, 1, CV_64F, _w3); < span class = "comment" > // temps</ span ></ div >
< div class = "line" >< a id = "l00997" name = "l00997" ></ a >< span class = "lineno" > 997</ span > CvMat wR = cvMat(3, 3, CV_64F, _wr);</ div >
< div class = "line" >< a id = "l00998" name = "l00998" ></ a >< span class = "lineno" > 998</ span > CvMat Z = cvMat(3, 1, CV_64F, _z);</ div >
< div class = "line" >< a id = "l00999" name = "l00999" ></ a >< span class = "lineno" > 999</ span > CvMat Ri = cvMat(3, 3, CV_64F, _ri);</ div >
< div class = "line" >< a id = "l01000" name = "l01000" ></ a >< span class = "lineno" > 1000</ span > < span class = "keywordtype" > double</ span > nx = imageSize.width, ny = imageSize.height;</ div >
< div class = "line" >< a id = "l01001" name = "l01001" ></ a >< span class = "lineno" > 1001</ span > < span class = "keywordtype" > int</ span > i, k;</ div >
< div class = "line" >< a id = "l01002" name = "l01002" ></ a >< span class = "lineno" > 1002</ span > < span class = "keywordtype" > double</ span > nt, nw;</ div >
< div class = "line" >< a id = "l01003" name = "l01003" ></ a >< span class = "lineno" > 1003</ span > </ div >
< div class = "line" >< a id = "l01004" name = "l01004" ></ a >< span class = "lineno" > 1004</ span > < span class = "keywordflow" > if</ span > ( matR-> rows == 3 && matR-> cols == 3 )</ div >
< div class = "line" >< a id = "l01005" name = "l01005" ></ a >< span class = "lineno" > 1005</ span > cvRodrigues2(matR, & om); < span class = "comment" > // get vector rotation</ span ></ div >
< div class = "line" >< a id = "l01006" name = "l01006" ></ a >< span class = "lineno" > 1006</ span > < span class = "keywordflow" > else</ span ></ div >
< div class = "line" >< a id = "l01007" name = "l01007" ></ a >< span class = "lineno" > 1007</ span > cvConvert(matR, & om); < span class = "comment" > // it' s already a rotation vector</ span ></ div >
< div class = "line" >< a id = "l01008" name = "l01008" ></ a >< span class = "lineno" > 1008</ span > cvConvertScale(& om, & om, -0.5); < span class = "comment" > // get average rotation</ span ></ div >
< div class = "line" >< a id = "l01009" name = "l01009" ></ a >< span class = "lineno" > 1009</ span > cvRodrigues2(& om, & r_r); < span class = "comment" > // rotate cameras to same orientation by averaging</ span ></ div >
< div class = "line" >< a id = "l01010" name = "l01010" ></ a >< span class = "lineno" > 1010</ span > </ div >
< div class = "line" >< a id = "l01011" name = "l01011" ></ a >< span class = "lineno" > 1011</ span > cvMatMul(& r_r, matT, & t);</ div >
< div class = "line" >< a id = "l01012" name = "l01012" ></ a >< span class = "lineno" > 1012</ span > < span class = "keywordtype" > int</ span > idx = fabs(_t[0]) > fabs(_t[1]) ? 0 : 1;</ div >
< div class = "line" >< a id = "l01013" name = "l01013" ></ a >< span class = "lineno" > 1013</ span > < span class = "comment" > // if idx == 0</ span ></ div >
< div class = "line" >< a id = "l01014" name = "l01014" ></ a >< span class = "lineno" > 1014</ span > < span class = "comment" > // e1 = T / ||T||</ span ></ div >
< div class = "line" >< a id = "l01015" name = "l01015" ></ a >< span class = "lineno" > 1015</ span > < span class = "comment" > // e2 = e1 x [0,0,1]</ span ></ div >
< div class = "line" >< a id = "l01016" name = "l01016" ></ a >< span class = "lineno" > 1016</ span > </ div >
< div class = "line" >< a id = "l01017" name = "l01017" ></ a >< span class = "lineno" > 1017</ span > < span class = "comment" > // if idx == 1</ span ></ div >
< div class = "line" >< a id = "l01018" name = "l01018" ></ a >< span class = "lineno" > 1018</ span > < span class = "comment" > // e2 = T / ||T||</ span ></ div >
< div class = "line" >< a id = "l01019" name = "l01019" ></ a >< span class = "lineno" > 1019</ span > < span class = "comment" > // e1 = e2 x [0,0,1]</ span ></ div >
< div class = "line" >< a id = "l01020" name = "l01020" ></ a >< span class = "lineno" > 1020</ span > </ div >
< div class = "line" >< a id = "l01021" name = "l01021" ></ a >< span class = "lineno" > 1021</ span > < span class = "comment" > // e3 = e1 x e2</ span ></ div >
< div class = "line" >< a id = "l01022" name = "l01022" ></ a >< span class = "lineno" > 1022</ span > _uu[2] = 1;</ div >
< div class = "line" >< a id = "l01023" name = "l01023" ></ a >< span class = "lineno" > 1023</ span > cvCrossProduct(& uu, & t, & ww);</ div >
< div class = "line" >< a id = "l01024" name = "l01024" ></ a >< span class = "lineno" > 1024</ span > nt = cvNorm(& t, 0, CV_L2);</ div >
< div class = "line" >< a id = "l01025" name = "l01025" ></ a >< span class = "lineno" > 1025</ span > CV_Assert(fabs(nt) > 0);</ div >
< div class = "line" >< a id = "l01026" name = "l01026" ></ a >< span class = "lineno" > 1026</ span > nw = cvNorm(& ww, 0, CV_L2);</ div >
< div class = "line" >< a id = "l01027" name = "l01027" ></ a >< span class = "lineno" > 1027</ span > CV_Assert(fabs(nw) > 0);</ div >
< div class = "line" >< a id = "l01028" name = "l01028" ></ a >< span class = "lineno" > 1028</ span > cvConvertScale(& ww, & ww, 1 / nw);</ div >
< div class = "line" >< a id = "l01029" name = "l01029" ></ a >< span class = "lineno" > 1029</ span > cvCrossProduct(& t, & ww, & w3);</ div >
< div class = "line" >< a id = "l01030" name = "l01030" ></ a >< span class = "lineno" > 1030</ span > nw = cvNorm(& w3, 0, CV_L2);</ div >
< div class = "line" >< a id = "l01031" name = "l01031" ></ a >< span class = "lineno" > 1031</ span > CV_Assert(fabs(nw) > 0);</ div >
< div class = "line" >< a id = "l01032" name = "l01032" ></ a >< span class = "lineno" > 1032</ span > cvConvertScale(& w3, & w3, 1 / nw);</ div >
< div class = "line" >< a id = "l01033" name = "l01033" ></ a >< span class = "lineno" > 1033</ span > _uu[2] = 0;</ div >
< div class = "line" >< a id = "l01034" name = "l01034" ></ a >< span class = "lineno" > 1034</ span > < span class = "keywordflow" > for</ span > (i = 0; i < 3; ++i)</ div >
< div class = "line" >< a id = "l01035" name = "l01035" ></ a >< span class = "lineno" > 1035</ span > {</ div >
< div class = "line" >< a id = "l01036" name = "l01036" ></ a >< span class = "lineno" > 1036</ span > _wr[idx][i] = -_t[i] / nt;</ div >
< div class = "line" >< a id = "l01037" name = "l01037" ></ a >< span class = "lineno" > 1037</ span > _wr[idx ^ 1][i] = -_ww[i];</ div >
< div class = "line" >< a id = "l01038" name = "l01038" ></ a >< span class = "lineno" > 1038</ span > _wr[2][i] = _w3[i] * (1 - 2 * idx); < span class = "comment" > // if idx == 1 -> opposite direction</ span ></ div >
< div class = "line" >< a id = "l01039" name = "l01039" ></ a >< span class = "lineno" > 1039</ span > }</ div >
< div class = "line" >< a id = "l01040" name = "l01040" ></ a >< span class = "lineno" > 1040</ span > < span class = "comment" > // apply to both views</ span ></ div >
< div class = "line" >< a id = "l01041" name = "l01041" ></ a >< span class = "lineno" > 1041</ span > cvGEMM(& wR, & r_r, 1, 0, 0, & Ri, CV_GEMM_B_T);</ div >
< div class = "line" >< a id = "l01042" name = "l01042" ></ a >< span class = "lineno" > 1042</ span > cvConvert( & Ri, _R1 );</ div >
< div class = "line" >< a id = "l01043" name = "l01043" ></ a >< span class = "lineno" > 1043</ span > cvGEMM(& wR, & r_r, 1, 0, 0, & Ri, 0);</ div >
< div class = "line" >< a id = "l01044" name = "l01044" ></ a >< span class = "lineno" > 1044</ span > cvConvert( & Ri, _R2 );</ div >
< div class = "line" >< a id = "l01045" name = "l01045" ></ a >< span class = "lineno" > 1045</ span > cvMatMul(& Ri, matT, & t);</ div >
< div class = "line" >< a id = "l01046" name = "l01046" ></ a >< span class = "lineno" > 1046</ span > < span class = "comment" > // calculate projection/camera matrices</ span ></ div >
< div class = "line" >< a id = "l01047" name = "l01047" ></ a >< span class = "lineno" > 1047</ span > < span class = "comment" > // these contain the relevant rectified image internal params (fx, fy=fx, cx, cy)</ span ></ div >
< div class = "line" >< a id = "l01048" name = "l01048" ></ a >< span class = "lineno" > 1048</ span > < span class = "keywordtype" > double</ span > fc_new = DBL_MAX;</ div >
< div class = "line" >< a id = "l01049" name = "l01049" ></ a >< span class = "lineno" > 1049</ span > CvPoint2D64f cc_new[2] = {};</ div >
< div class = "line" >< a id = "l01050" name = "l01050" ></ a >< span class = "lineno" > 1050</ span > </ div >
< div class = "line" >< a id = "l01051" name = "l01051" ></ a >< span class = "lineno" > 1051</ span > newImgSize = newImgSize.width * newImgSize.height != 0 ? newImgSize : imageSize;</ div >
< div class = "line" >< a id = "l01052" name = "l01052" ></ a >< span class = "lineno" > 1052</ span > < span class = "keyword" > const</ span > < span class = "keywordtype" > double</ span > ratio_x = (double)newImgSize.width / imageSize.width / 2;</ div >
< div class = "line" >< a id = "l01053" name = "l01053" ></ a >< span class = "lineno" > 1053</ span > < span class = "keyword" > const</ span > < span class = "keywordtype" > double</ span > ratio_y = (double)newImgSize.height / imageSize.height / 2;</ div >
< div class = "line" >< a id = "l01054" name = "l01054" ></ a >< span class = "lineno" > 1054</ span > < span class = "keyword" > const</ span > < span class = "keywordtype" > double</ span > ratio = idx == 1 ? ratio_x : ratio_y;</ div >
< div class = "line" >< a id = "l01055" name = "l01055" ></ a >< span class = "lineno" > 1055</ span > fc_new = (cvmGet(_cameraMatrix1, idx ^ 1, idx ^ 1) + cvmGet(_cameraMatrix2, idx ^ 1, idx ^ 1)) * ratio;</ div >
< div class = "line" >< a id = "l01056" name = "l01056" ></ a >< span class = "lineno" > 1056</ span > < span class = "keywordflow" > for</ span > ( k = 0; k < 2; k++ )</ div >
< div class = "line" >< a id = "l01057" name = "l01057" ></ a >< span class = "lineno" > 1057</ span > {</ div >
< div class = "line" >< a id = "l01058" name = "l01058" ></ a >< span class = "lineno" > 1058</ span > < span class = "keyword" > const</ span > CvMat* A = k == 0 ? _cameraMatrix1 : _cameraMatrix2;</ div >
< div class = "line" >< a id = "l01059" name = "l01059" ></ a >< span class = "lineno" > 1059</ span > < span class = "keyword" > const</ span > CvMat* Dk = k == 0 ? _distCoeffs1 : _distCoeffs2;</ div >
< div class = "line" >< a id = "l01060" name = "l01060" ></ a >< span class = "lineno" > 1060</ span > CvPoint2D32f _pts[4] = {};</ div >
< div class = "line" >< a id = "l01061" name = "l01061" ></ a >< span class = "lineno" > 1061</ span > CvPoint3D32f _pts_3[4] = {};</ div >
< div class = "line" >< a id = "l01062" name = "l01062" ></ a >< span class = "lineno" > 1062</ span > CvMat pts = cvMat(1, 4, CV_32FC2, _pts);</ div >
< div class = "line" >< a id = "l01063" name = "l01063" ></ a >< span class = "lineno" > 1063</ span > CvMat pts_3 = cvMat(1, 4, CV_32FC3, _pts_3);</ div >
< div class = "line" >< a id = "l01064" name = "l01064" ></ a >< span class = "lineno" > 1064</ span > </ div >
< div class = "line" >< a id = "l01065" name = "l01065" ></ a >< span class = "lineno" > 1065</ span > < span class = "keywordflow" > for</ span > ( i = 0; i < 4; i++ )</ div >
< div class = "line" >< a id = "l01066" name = "l01066" ></ a >< span class = "lineno" > 1066</ span > {</ div >
< div class = "line" >< a id = "l01067" name = "l01067" ></ a >< span class = "lineno" > 1067</ span > < span class = "keywordtype" > int</ span > j = (i< 2) ? 0 : 1;</ div >
< div class = "line" >< a id = "l01068" name = "l01068" ></ a >< span class = "lineno" > 1068</ span > _pts[i].x = (float)((i % 2)*(nx));</ div >
< div class = "line" >< a id = "l01069" name = "l01069" ></ a >< span class = "lineno" > 1069</ span > _pts[i].y = (float)(j*(ny));</ div >
< div class = "line" >< a id = "l01070" name = "l01070" ></ a >< span class = "lineno" > 1070</ span > }</ div >
< div class = "line" >< a id = "l01071" name = "l01071" ></ a >< span class = "lineno" > 1071</ span > cv::Mat ptsM(pts.rows, pts.cols, pts.type, pts.data.ptr);</ div >
< div class = "line" >< a id = "l01072" name = "l01072" ></ a >< span class = "lineno" > 1072</ span > cv::Mat A_m(A-> rows, A-> cols, A-> type, A-> data.ptr);</ div >
< div class = "line" >< a id = "l01073" name = "l01073" ></ a >< span class = "lineno" > 1073</ span > cv::Mat Dk_m(Dk-> rows, Dk-> cols, Dk-> type, Dk-> data.ptr);</ div >
< div class = "line" >< a id = "l01074" name = "l01074" ></ a >< span class = "lineno" > 1074</ span > cv::fisheye::undistortPoints( ptsM, ptsM, A_m, Dk_m, cv::Mat(), cv::Mat() );</ div >
< div class = "line" >< a id = "l01075" name = "l01075" ></ a >< span class = "lineno" > 1075</ span > cvConvertPointsHomogeneous( & pts, & pts_3 );</ div >
< div class = "line" >< a id = "l01076" name = "l01076" ></ a >< span class = "lineno" > 1076</ span > </ div >
< div class = "line" >< a id = "l01077" name = "l01077" ></ a >< span class = "lineno" > 1077</ span > < span class = "comment" > //Change camera matrix to have cc=[0,0] and fc = fc_new</ span ></ div >
< div class = "line" >< a id = "l01078" name = "l01078" ></ a >< span class = "lineno" > 1078</ span > < span class = "keywordtype" > double</ span > _a_tmp[3][3];</ div >
< div class = "line" >< a id = "l01079" name = "l01079" ></ a >< span class = "lineno" > 1079</ span > CvMat A_tmp = cvMat(3, 3, CV_64F, _a_tmp);</ div >
< div class = "line" >< a id = "l01080" name = "l01080" ></ a >< span class = "lineno" > 1080</ span > _a_tmp[0][0]=fc_new;</ div >
< div class = "line" >< a id = "l01081" name = "l01081" ></ a >< span class = "lineno" > 1081</ span > _a_tmp[1][1]=fc_new;</ div >
< div class = "line" >< a id = "l01082" name = "l01082" ></ a >< span class = "lineno" > 1082</ span > _a_tmp[0][2]=0.0;</ div >
< div class = "line" >< a id = "l01083" name = "l01083" ></ a >< span class = "lineno" > 1083</ span > _a_tmp[1][2]=0.0;</ div >
< div class = "line" >< a id = "l01084" name = "l01084" ></ a >< span class = "lineno" > 1084</ span > </ div >
< div class = "line" >< a id = "l01085" name = "l01085" ></ a >< span class = "lineno" > 1085</ span > cvProjectPoints2( & pts_3, k == 0 ? _R1 : _R2, & Z, & A_tmp, 0, & pts );</ div >
< div class = "line" >< a id = "l01086" name = "l01086" ></ a >< span class = "lineno" > 1086</ span > CvScalar avg = cvAvg(& pts);</ div >
< div class = "line" >< a id = "l01087" name = "l01087" ></ a >< span class = "lineno" > 1087</ span > </ div >
< div class = "line" >< a id = "l01088" name = "l01088" ></ a >< span class = "lineno" > 1088</ span > cc_new[k].x = (nx)/2 - avg.val[0];</ div >
< div class = "line" >< a id = "l01089" name = "l01089" ></ a >< span class = "lineno" > 1089</ span > cc_new[k].y = (ny)/2 - avg.val[1];</ div >
< div class = "line" >< a id = "l01090" name = "l01090" ></ a >< span class = "lineno" > 1090</ span > }</ div >
< div class = "line" >< a id = "l01091" name = "l01091" ></ a >< span class = "lineno" > 1091</ span > </ div >
< div class = "line" >< a id = "l01092" name = "l01092" ></ a >< span class = "lineno" > 1092</ span > < span class = "comment" > // vertical focal length must be the same for both images to keep the epipolar constraint</ span ></ div >
< div class = "line" >< a id = "l01093" name = "l01093" ></ a >< span class = "lineno" > 1093</ span > < span class = "comment" > // (for horizontal epipolar lines -- TBD: check for vertical epipolar lines)</ span ></ div >
< div class = "line" >< a id = "l01094" name = "l01094" ></ a >< span class = "lineno" > 1094</ span > < span class = "comment" > // use fy for fx also, for simplicity</ span ></ div >
< div class = "line" >< a id = "l01095" name = "l01095" ></ a >< span class = "lineno" > 1095</ span > </ div >
< div class = "line" >< a id = "l01096" name = "l01096" ></ a >< span class = "lineno" > 1096</ span > < span class = "comment" > // For simplicity, set the principal points for both cameras to be the average</ span ></ div >
< div class = "line" >< a id = "l01097" name = "l01097" ></ a >< span class = "lineno" > 1097</ span > < span class = "comment" > // of the two principal points (either one of or both x- and y- coordinates)</ span ></ div >
< div class = "line" >< a id = "l01098" name = "l01098" ></ a >< span class = "lineno" > 1098</ span > < span class = "keywordflow" > if</ span > ( flags & cv::CALIB_ZERO_DISPARITY )</ div >
< div class = "line" >< a id = "l01099" name = "l01099" ></ a >< span class = "lineno" > 1099</ span > {</ div >
< div class = "line" >< a id = "l01100" name = "l01100" ></ a >< span class = "lineno" > 1100</ span > cc_new[0].x = cc_new[1].x = (cc_new[0].x + cc_new[1].x)*0.5;</ div >
< div class = "line" >< a id = "l01101" name = "l01101" ></ a >< span class = "lineno" > 1101</ span > cc_new[0].y = cc_new[1].y = (cc_new[0].y + cc_new[1].y)*0.5;</ div >
< div class = "line" >< a id = "l01102" name = "l01102" ></ a >< span class = "lineno" > 1102</ span > }</ div >
< div class = "line" >< a id = "l01103" name = "l01103" ></ a >< span class = "lineno" > 1103</ span > < span class = "keywordflow" > else</ span > < span class = "keywordflow" > if</ span > ( idx == 0 ) < span class = "comment" > // horizontal stereo</ span ></ div >
< div class = "line" >< a id = "l01104" name = "l01104" ></ a >< span class = "lineno" > 1104</ span > cc_new[0].y = cc_new[1].y = (cc_new[0].y + cc_new[1].y)*0.5;</ div >
< div class = "line" >< a id = "l01105" name = "l01105" ></ a >< span class = "lineno" > 1105</ span > < span class = "keywordflow" > else</ span > < span class = "comment" > // vertical stereo</ span ></ div >
< div class = "line" >< a id = "l01106" name = "l01106" ></ a >< span class = "lineno" > 1106</ span > cc_new[0].x = cc_new[1].x = (cc_new[0].x + cc_new[1].x)*0.5;</ div >
< div class = "line" >< a id = "l01107" name = "l01107" ></ a >< span class = "lineno" > 1107</ span > </ div >
< div class = "line" >< a id = "l01108" name = "l01108" ></ a >< span class = "lineno" > 1108</ span > cvZero( & pp );</ div >
< div class = "line" >< a id = "l01109" name = "l01109" ></ a >< span class = "lineno" > 1109</ span > _pp[0][0] = _pp[1][1] = fc_new;</ div >
< div class = "line" >< a id = "l01110" name = "l01110" ></ a >< span class = "lineno" > 1110</ span > _pp[0][2] = cc_new[0].x;</ div >
< div class = "line" >< a id = "l01111" name = "l01111" ></ a >< span class = "lineno" > 1111</ span > _pp[1][2] = cc_new[0].y;</ div >
< div class = "line" >< a id = "l01112" name = "l01112" ></ a >< span class = "lineno" > 1112</ span > _pp[2][2] = 1;</ div >
< div class = "line" >< a id = "l01113" name = "l01113" ></ a >< span class = "lineno" > 1113</ span > cvConvert(& pp, _P1);</ div >
< div class = "line" >< a id = "l01114" name = "l01114" ></ a >< span class = "lineno" > 1114</ span > </ div >
< div class = "line" >< a id = "l01115" name = "l01115" ></ a >< span class = "lineno" > 1115</ span > _pp[0][2] = cc_new[1].x;</ div >
< div class = "line" >< a id = "l01116" name = "l01116" ></ a >< span class = "lineno" > 1116</ span > _pp[1][2] = cc_new[1].y;</ div >
< div class = "line" >< a id = "l01117" name = "l01117" ></ a >< span class = "lineno" > 1117</ span > _pp[idx][3] = _t[idx]*fc_new; < span class = "comment" > // baseline * focal length</ span ></ div >
< div class = "line" >< a id = "l01118" name = "l01118" ></ a >< span class = "lineno" > 1118</ span > cvConvert(& pp, _P2);</ div >
< div class = "line" >< a id = "l01119" name = "l01119" ></ a >< span class = "lineno" > 1119</ span > </ div >
< div class = "line" >< a id = "l01120" name = "l01120" ></ a >< span class = "lineno" > 1120</ span > alpha = MIN(alpha, 1.);</ div >
< div class = "line" >< a id = "l01121" name = "l01121" ></ a >< span class = "lineno" > 1121</ span > </ div >
< div class = "line" >< a id = "l01122" name = "l01122" ></ a >< span class = "lineno" > 1122</ span > icvGetRectanglesFisheye( _cameraMatrix1, _distCoeffs1, _R1, _P1, imageSize, inner1, outer1 );</ div >
< div class = "line" >< a id = "l01123" name = "l01123" ></ a >< span class = "lineno" > 1123</ span > icvGetRectanglesFisheye( _cameraMatrix2, _distCoeffs2, _R2, _P2, imageSize, inner2, outer2 );</ div >
< div class = "line" >< a id = "l01124" name = "l01124" ></ a >< span class = "lineno" > 1124</ span > </ div >
< div class = "line" >< a id = "l01125" name = "l01125" ></ a >< span class = "lineno" > 1125</ span > {</ div >
< div class = "line" >< a id = "l01126" name = "l01126" ></ a >< span class = "lineno" > 1126</ span > newImgSize = newImgSize.width*newImgSize.height != 0 ? newImgSize : imageSize;</ div >
< div class = "line" >< a id = "l01127" name = "l01127" ></ a >< span class = "lineno" > 1127</ span > < span class = "keywordtype" > double</ span > cx1_0 = cc_new[0].x;</ div >
< div class = "line" >< a id = "l01128" name = "l01128" ></ a >< span class = "lineno" > 1128</ span > < span class = "keywordtype" > double</ span > cy1_0 = cc_new[0].y;</ div >
< div class = "line" >< a id = "l01129" name = "l01129" ></ a >< span class = "lineno" > 1129</ span > < span class = "keywordtype" > double</ span > cx2_0 = cc_new[1].x;</ div >
< div class = "line" >< a id = "l01130" name = "l01130" ></ a >< span class = "lineno" > 1130</ span > < span class = "keywordtype" > double</ span > cy2_0 = cc_new[1].y;</ div >
< div class = "line" >< a id = "l01131" name = "l01131" ></ a >< span class = "lineno" > 1131</ span > < span class = "keywordtype" > double</ span > cx1 = newImgSize.width*cx1_0/imageSize.width;</ div >
< div class = "line" >< a id = "l01132" name = "l01132" ></ a >< span class = "lineno" > 1132</ span > < span class = "keywordtype" > double</ span > cy1 = newImgSize.height*cy1_0/imageSize.height;</ div >
< div class = "line" >< a id = "l01133" name = "l01133" ></ a >< span class = "lineno" > 1133</ span > < span class = "keywordtype" > double</ span > cx2 = newImgSize.width*cx2_0/imageSize.width;</ div >
< div class = "line" >< a id = "l01134" name = "l01134" ></ a >< span class = "lineno" > 1134</ span > < span class = "keywordtype" > double</ span > cy2 = newImgSize.height*cy2_0/imageSize.height;</ div >
< div class = "line" >< a id = "l01135" name = "l01135" ></ a >< span class = "lineno" > 1135</ span > < span class = "keywordtype" > double</ span > s = 1.;</ div >
< div class = "line" >< a id = "l01136" name = "l01136" ></ a >< span class = "lineno" > 1136</ span > </ div >
< div class = "line" >< a id = "l01137" name = "l01137" ></ a >< span class = "lineno" > 1137</ span > < span class = "keywordflow" > if</ span > ( alpha > = 0 )</ div >
< div class = "line" >< a id = "l01138" name = "l01138" ></ a >< span class = "lineno" > 1138</ span > {</ div >
< div class = "line" >< a id = "l01139" name = "l01139" ></ a >< span class = "lineno" > 1139</ span > < span class = "keywordtype" > double</ span > s0 = std::max(std::max(std::max((< span class = "keywordtype" > double</ span > )cx1/(cx1_0 - inner1.x), (< span class = "keywordtype" > double</ span > )cy1/(cy1_0 - inner1.y)),</ div >
< div class = "line" >< a id = "l01140" name = "l01140" ></ a >< span class = "lineno" > 1140</ span > (< span class = "keywordtype" > double</ span > )(newImgSize.width - cx1)/(inner1.x + inner1.width - cx1_0)),</ div >
< div class = "line" >< a id = "l01141" name = "l01141" ></ a >< span class = "lineno" > 1141</ span > (< span class = "keywordtype" > double</ span > )(newImgSize.height - cy1)/(inner1.y + inner1.height - cy1_0));</ div >
< div class = "line" >< a id = "l01142" name = "l01142" ></ a >< span class = "lineno" > 1142</ span > s0 = std::max(std::max(std::max(std::max((< span class = "keywordtype" > double</ span > )cx2/(cx2_0 - inner2.x), (< span class = "keywordtype" > double</ span > )cy2/(cy2_0 - inner2.y)),</ div >
< div class = "line" >< a id = "l01143" name = "l01143" ></ a >< span class = "lineno" > 1143</ span > (< span class = "keywordtype" > double</ span > )(newImgSize.width - cx2)/(inner2.x + inner2.width - cx2_0)),</ div >
< div class = "line" >< a id = "l01144" name = "l01144" ></ a >< span class = "lineno" > 1144</ span > (< span class = "keywordtype" > double</ span > )(newImgSize.height - cy2)/(inner2.y + inner2.height - cy2_0)),</ div >
< div class = "line" >< a id = "l01145" name = "l01145" ></ a >< span class = "lineno" > 1145</ span > s0);</ div >
< div class = "line" >< a id = "l01146" name = "l01146" ></ a >< span class = "lineno" > 1146</ span > </ div >
< div class = "line" >< a id = "l01147" name = "l01147" ></ a >< span class = "lineno" > 1147</ span > < span class = "keywordtype" > double</ span > s1 = std::min(std::min(std::min((< span class = "keywordtype" > double</ span > )cx1/(cx1_0 - outer1.x), (< span class = "keywordtype" > double</ span > )cy1/(cy1_0 - outer1.y)),</ div >
< div class = "line" >< a id = "l01148" name = "l01148" ></ a >< span class = "lineno" > 1148</ span > (< span class = "keywordtype" > double</ span > )(newImgSize.width - cx1)/(outer1.x + outer1.width - cx1_0)),</ div >
< div class = "line" >< a id = "l01149" name = "l01149" ></ a >< span class = "lineno" > 1149</ span > (< span class = "keywordtype" > double</ span > )(newImgSize.height - cy1)/(outer1.y + outer1.height - cy1_0));</ div >
< div class = "line" >< a id = "l01150" name = "l01150" ></ a >< span class = "lineno" > 1150</ span > s1 = std::min(std::min(std::min(std::min((< span class = "keywordtype" > double</ span > )cx2/(cx2_0 - outer2.x), (< span class = "keywordtype" > double</ span > )cy2/(cy2_0 - outer2.y)),</ div >
< div class = "line" >< a id = "l01151" name = "l01151" ></ a >< span class = "lineno" > 1151</ span > (< span class = "keywordtype" > double</ span > )(newImgSize.width - cx2)/(outer2.x + outer2.width - cx2_0)),</ div >
< div class = "line" >< a id = "l01152" name = "l01152" ></ a >< span class = "lineno" > 1152</ span > (< span class = "keywordtype" > double</ span > )(newImgSize.height - cy2)/(outer2.y + outer2.height - cy2_0)),</ div >
< div class = "line" >< a id = "l01153" name = "l01153" ></ a >< span class = "lineno" > 1153</ span > s1);</ div >
< div class = "line" >< a id = "l01154" name = "l01154" ></ a >< span class = "lineno" > 1154</ span > </ div >
< div class = "line" >< a id = "l01155" name = "l01155" ></ a >< span class = "lineno" > 1155</ span > s = s0*(1 - alpha) + s1*alpha;</ div >
< div class = "line" >< a id = "l01156" name = "l01156" ></ a >< span class = "lineno" > 1156</ span > }</ div >
< div class = "line" >< a id = "l01157" name = "l01157" ></ a >< span class = "lineno" > 1157</ span > </ div >
< div class = "line" >< a id = "l01158" name = "l01158" ></ a >< span class = "lineno" > 1158</ span > fc_new *= s;</ div >
< div class = "line" >< a id = "l01159" name = "l01159" ></ a >< span class = "lineno" > 1159</ span > cc_new[0] = cvPoint2D64f(cx1, cy1);</ div >
< div class = "line" >< a id = "l01160" name = "l01160" ></ a >< span class = "lineno" > 1160</ span > cc_new[1] = cvPoint2D64f(cx2, cy2);</ div >
< div class = "line" >< a id = "l01161" name = "l01161" ></ a >< span class = "lineno" > 1161</ span > </ div >
< div class = "line" >< a id = "l01162" name = "l01162" ></ a >< span class = "lineno" > 1162</ span > cvmSet(_P1, 0, 0, fc_new);</ div >
< div class = "line" >< a id = "l01163" name = "l01163" ></ a >< span class = "lineno" > 1163</ span > cvmSet(_P1, 1, 1, fc_new);</ div >
< div class = "line" >< a id = "l01164" name = "l01164" ></ a >< span class = "lineno" > 1164</ span > cvmSet(_P1, 0, 2, cx1);</ div >
< div class = "line" >< a id = "l01165" name = "l01165" ></ a >< span class = "lineno" > 1165</ span > cvmSet(_P1, 1, 2, cy1);</ div >
< div class = "line" >< a id = "l01166" name = "l01166" ></ a >< span class = "lineno" > 1166</ span > </ div >
< div class = "line" >< a id = "l01167" name = "l01167" ></ a >< span class = "lineno" > 1167</ span > cvmSet(_P2, 0, 0, fc_new);</ div >
< div class = "line" >< a id = "l01168" name = "l01168" ></ a >< span class = "lineno" > 1168</ span > cvmSet(_P2, 1, 1, fc_new);</ div >
< div class = "line" >< a id = "l01169" name = "l01169" ></ a >< span class = "lineno" > 1169</ span > cvmSet(_P2, 0, 2, cx2);</ div >
< div class = "line" >< a id = "l01170" name = "l01170" ></ a >< span class = "lineno" > 1170</ span > cvmSet(_P2, 1, 2, cy2);</ div >
< div class = "line" >< a id = "l01171" name = "l01171" ></ a >< span class = "lineno" > 1171</ span > cvmSet(_P2, idx, 3, s*cvmGet(_P2, idx, 3));</ div >
< div class = "line" >< a id = "l01172" name = "l01172" ></ a >< span class = "lineno" > 1172</ span > </ div >
< div class = "line" >< a id = "l01173" name = "l01173" ></ a >< span class = "lineno" > 1173</ span > }</ div >
< div class = "line" >< a id = "l01174" name = "l01174" ></ a >< span class = "lineno" > 1174</ span > </ div >
< div class = "line" >< a id = "l01175" name = "l01175" ></ a >< span class = "lineno" > 1175</ span > < span class = "keywordflow" > if</ span > ( matQ )</ div >
< div class = "line" >< a id = "l01176" name = "l01176" ></ a >< span class = "lineno" > 1176</ span > {</ div >
< div class = "line" >< a id = "l01177" name = "l01177" ></ a >< span class = "lineno" > 1177</ span > < span class = "keywordtype" > double</ span > q[] =</ div >
< div class = "line" >< a id = "l01178" name = "l01178" ></ a >< span class = "lineno" > 1178</ span > {</ div >
< div class = "line" >< a id = "l01179" name = "l01179" ></ a >< span class = "lineno" > 1179</ span > 1, 0, 0, -cc_new[0].x,</ div >
< div class = "line" >< a id = "l01180" name = "l01180" ></ a >< span class = "lineno" > 1180</ span > 0, 1, 0, -cc_new[0].y,</ div >
< div class = "line" >< a id = "l01181" name = "l01181" ></ a >< span class = "lineno" > 1181</ span > 0, 0, 0, fc_new,</ div >
< div class = "line" >< a id = "l01182" name = "l01182" ></ a >< span class = "lineno" > 1182</ span > 0, 0, -1./_t[idx],</ div >
< div class = "line" >< a id = "l01183" name = "l01183" ></ a >< span class = "lineno" > 1183</ span > (idx == 0 ? cc_new[0].x - cc_new[1].x : cc_new[0].y - cc_new[1].y)/_t[idx]</ div >
< div class = "line" >< a id = "l01184" name = "l01184" ></ a >< span class = "lineno" > 1184</ span > };</ div >
< div class = "line" >< a id = "l01185" name = "l01185" ></ a >< span class = "lineno" > 1185</ span > CvMat Q = cvMat(4, 4, CV_64F, q);</ div >
< div class = "line" >< a id = "l01186" name = "l01186" ></ a >< span class = "lineno" > 1186</ span > cvConvert( & Q, matQ );</ div >
< div class = "line" >< a id = "l01187" name = "l01187" ></ a >< span class = "lineno" > 1187</ span > }</ div >
< div class = "line" >< a id = "l01188" name = "l01188" ></ a >< span class = "lineno" > 1188</ span > }</ div >
< div class = "line" >< a id = "l01189" name = "l01189" ></ a >< span class = "lineno" > 1189</ span > </ div >
< div class = "line" >< a id = "l01190" name = "l01190" ></ a >< span class = "lineno" > 1190</ span >< span class = "keywordtype" > void</ span > stereoRectifyFisheye( cv::InputArray _cameraMatrix1, cv::InputArray _distCoeffs1,</ div >
< div class = "line" >< a id = "l01191" name = "l01191" ></ a >< span class = "lineno" > 1191</ span > cv::InputArray _cameraMatrix2, cv::InputArray _distCoeffs2,</ div >
< div class = "line" >< a id = "l01192" name = "l01192" ></ a >< span class = "lineno" > 1192</ span > cv::Size imageSize, cv::InputArray _Rmat, cv::InputArray _Tmat,</ div >
< div class = "line" >< a id = "l01193" name = "l01193" ></ a >< span class = "lineno" > 1193</ span > cv::OutputArray _Rmat1, cv::OutputArray _Rmat2,</ div >
< div class = "line" >< a id = "l01194" name = "l01194" ></ a >< span class = "lineno" > 1194</ span > cv::OutputArray _Pmat1, cv::OutputArray _Pmat2,</ div >
< div class = "line" >< a id = "l01195" name = "l01195" ></ a >< span class = "lineno" > 1195</ span > cv::OutputArray _Qmat, < span class = "keywordtype" > int</ span > flags,</ div >
< div class = "line" >< a id = "l01196" name = "l01196" ></ a >< span class = "lineno" > 1196</ span > < span class = "keywordtype" > double</ span > alpha, cv::Size newImageSize)</ div >
< div class = "line" >< a id = "l01197" name = "l01197" ></ a >< span class = "lineno" > 1197</ span > {</ div >
< div class = "line" >< a id = "l01198" name = "l01198" ></ a >< span class = "lineno" > 1198</ span > cv::Mat cameraMatrix1 = _cameraMatrix1.getMat(), cameraMatrix2 = _cameraMatrix2.getMat();</ div >
< div class = "line" >< a id = "l01199" name = "l01199" ></ a >< span class = "lineno" > 1199</ span > cv::Mat distCoeffs1 = _distCoeffs1.getMat(), distCoeffs2 = _distCoeffs2.getMat();</ div >
< div class = "line" >< a id = "l01200" name = "l01200" ></ a >< span class = "lineno" > 1200</ span > cv::Mat Rmat = _Rmat.getMat(), Tmat = _Tmat.getMat();</ div >
< div class = "line" >< a id = "l01201" name = "l01201" ></ a >< span class = "lineno" > 1201</ span > </ div >
< div class = "line" >< a id = "l01202" name = "l01202" ></ a >< span class = "lineno" > 1202</ span >< span class = "preprocessor" > #if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION > = 3 && (CV_MINOR_VERSION> 4 || (CV_MINOR_VERSION> =4 && CV_SUBMINOR_VERSION> =4)))</ span ></ div >
< div class = "line" >< a id = "l01203" name = "l01203" ></ a >< span class = "lineno" > 1203</ span > CvMat c_cameraMatrix1 = cvMat(cameraMatrix1);</ div >
< div class = "line" >< a id = "l01204" name = "l01204" ></ a >< span class = "lineno" > 1204</ span > CvMat c_cameraMatrix2 = cvMat(cameraMatrix2);</ div >
< div class = "line" >< a id = "l01205" name = "l01205" ></ a >< span class = "lineno" > 1205</ span > CvMat c_distCoeffs1 = cvMat(distCoeffs1);</ div >
< div class = "line" >< a id = "l01206" name = "l01206" ></ a >< span class = "lineno" > 1206</ span > CvMat c_distCoeffs2 = cvMat(distCoeffs2);</ div >
< div class = "line" >< a id = "l01207" name = "l01207" ></ a >< span class = "lineno" > 1207</ span > CvMat c_R = cvMat(Rmat), c_T = cvMat(Tmat);</ div >
< div class = "line" >< a id = "l01208" name = "l01208" ></ a >< span class = "lineno" > 1208</ span >< span class = "preprocessor" > #else</ span ></ div >
< div class = "line" >< a id = "l01209" name = "l01209" ></ a >< span class = "lineno" > 1209</ span > CvMat c_cameraMatrix1 = CvMat(cameraMatrix1);</ div >
< div class = "line" >< a id = "l01210" name = "l01210" ></ a >< span class = "lineno" > 1210</ span > CvMat c_cameraMatrix2 = CvMat(cameraMatrix2);</ div >
< div class = "line" >< a id = "l01211" name = "l01211" ></ a >< span class = "lineno" > 1211</ span > CvMat c_distCoeffs1 = CvMat(distCoeffs1);</ div >
< div class = "line" >< a id = "l01212" name = "l01212" ></ a >< span class = "lineno" > 1212</ span > CvMat c_distCoeffs2 = CvMat(distCoeffs2);</ div >
< div class = "line" >< a id = "l01213" name = "l01213" ></ a >< span class = "lineno" > 1213</ span > CvMat c_R = CvMat(Rmat), c_T = CvMat(Tmat);</ div >
< div class = "line" >< a id = "l01214" name = "l01214" ></ a >< span class = "lineno" > 1214</ span >< span class = "preprocessor" > #endif</ span ></ div >
< div class = "line" >< a id = "l01215" name = "l01215" ></ a >< span class = "lineno" > 1215</ span > </ div >
< div class = "line" >< a id = "l01216" name = "l01216" ></ a >< span class = "lineno" > 1216</ span > < span class = "keywordtype" > int</ span > rtype = CV_64F;</ div >
< div class = "line" >< a id = "l01217" name = "l01217" ></ a >< span class = "lineno" > 1217</ span > _Rmat1.create(3, 3, rtype);</ div >
< div class = "line" >< a id = "l01218" name = "l01218" ></ a >< span class = "lineno" > 1218</ span > _Rmat2.create(3, 3, rtype);</ div >
< div class = "line" >< a id = "l01219" name = "l01219" ></ a >< span class = "lineno" > 1219</ span > _Pmat1.create(3, 4, rtype);</ div >
< div class = "line" >< a id = "l01220" name = "l01220" ></ a >< span class = "lineno" > 1220</ span > _Pmat2.create(3, 4, rtype);</ div >
< div class = "line" >< a id = "l01221" name = "l01221" ></ a >< span class = "lineno" > 1221</ span > cv::Mat R1 = _Rmat1.getMat(), R2 = _Rmat2.getMat(), P1 = _Pmat1.getMat(), P2 = _Pmat2.getMat(), Q;</ div >
< div class = "line" >< a id = "l01222" name = "l01222" ></ a >< span class = "lineno" > 1222</ span >< span class = "preprocessor" > #if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION > = 3 && (CV_MINOR_VERSION> 4 || (CV_MINOR_VERSION> =4 && CV_SUBMINOR_VERSION> =4)))</ span ></ div >
< div class = "line" >< a id = "l01223" name = "l01223" ></ a >< span class = "lineno" > 1223</ span > CvMat c_R1 = cvMat(R1), c_R2 = cvMat(R2), c_P1 = cvMat(P1), c_P2 = cvMat(P2);</ div >
< div class = "line" >< a id = "l01224" name = "l01224" ></ a >< span class = "lineno" > 1224</ span >< span class = "preprocessor" > #else</ span ></ div >
< div class = "line" >< a id = "l01225" name = "l01225" ></ a >< span class = "lineno" > 1225</ span > CvMat c_R1 = CvMat(R1), c_R2 = CvMat(R2), c_P1 = CvMat(P1), c_P2 = CvMat(P2);</ div >
< div class = "line" >< a id = "l01226" name = "l01226" ></ a >< span class = "lineno" > 1226</ span >< span class = "preprocessor" > #endif</ span ></ div >
< div class = "line" >< a id = "l01227" name = "l01227" ></ a >< span class = "lineno" > 1227</ span > CvMat c_Q, *p_Q = 0;</ div >
< div class = "line" >< a id = "l01228" name = "l01228" ></ a >< span class = "lineno" > 1228</ span > </ div >
< div class = "line" >< a id = "l01229" name = "l01229" ></ a >< span class = "lineno" > 1229</ span > < span class = "keywordflow" > if</ span > ( _Qmat.needed() )</ div >
< div class = "line" >< a id = "l01230" name = "l01230" ></ a >< span class = "lineno" > 1230</ span > {</ div >
< div class = "line" >< a id = "l01231" name = "l01231" ></ a >< span class = "lineno" > 1231</ span > _Qmat.create(4, 4, rtype);</ div >
< div class = "line" >< a id = "l01232" name = "l01232" ></ a >< span class = "lineno" > 1232</ span >< span class = "preprocessor" > #if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION > = 3 && (CV_MINOR_VERSION> 4 || (CV_MINOR_VERSION> =4 && CV_SUBMINOR_VERSION> =4)))</ span ></ div >
< div class = "line" >< a id = "l01233" name = "l01233" ></ a >< span class = "lineno" > 1233</ span > p_Q = & (c_Q = cvMat(Q = _Qmat.getMat()));</ div >
< div class = "line" >< a id = "l01234" name = "l01234" ></ a >< span class = "lineno" > 1234</ span >< span class = "preprocessor" > #else</ span ></ div >
< div class = "line" >< a id = "l01235" name = "l01235" ></ a >< span class = "lineno" > 1235</ span > p_Q = & (c_Q = CvMat(Q = _Qmat.getMat()));</ div >
< div class = "line" >< a id = "l01236" name = "l01236" ></ a >< span class = "lineno" > 1236</ span >< span class = "preprocessor" > #endif</ span ></ div >
< div class = "line" >< a id = "l01237" name = "l01237" ></ a >< span class = "lineno" > 1237</ span > }</ div >
< div class = "line" >< a id = "l01238" name = "l01238" ></ a >< span class = "lineno" > 1238</ span > </ div >
< div class = "line" >< a id = "l01239" name = "l01239" ></ a >< span class = "lineno" > 1239</ span > CvMat *p_distCoeffs1 = distCoeffs1.empty() ? NULL : & c_distCoeffs1;</ div >
< div class = "line" >< a id = "l01240" name = "l01240" ></ a >< span class = "lineno" > 1240</ span > CvMat *p_distCoeffs2 = distCoeffs2.empty() ? NULL : & c_distCoeffs2;</ div >
< div class = "line" >< a id = "l01241" name = "l01241" ></ a >< span class = "lineno" > 1241</ span > cvStereoRectifyFisheye( & c_cameraMatrix1, & c_cameraMatrix2, p_distCoeffs1, p_distCoeffs2,</ div >
< div class = "line" >< a id = "l01242" name = "l01242" ></ a >< span class = "lineno" > 1242</ span > #< span class = "keywordflow" > if</ span > CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION > = 3 && (CV_MINOR_VERSION> 4 || (CV_MINOR_VERSION> =4 && CV_SUBMINOR_VERSION> =4)))</ div >
< div class = "line" >< a id = "l01243" name = "l01243" ></ a >< span class = "lineno" > 1243</ span > cvSize(imageSize), & c_R, & c_T, & c_R1, & c_R2, & c_P1, & c_P2, p_Q, flags, alpha,</ div >
< div class = "line" >< a id = "l01244" name = "l01244" ></ a >< span class = "lineno" > 1244</ span > cvSize(newImageSize));</ div >
< div class = "line" >< a id = "l01245" name = "l01245" ></ a >< span class = "lineno" > 1245</ span >< span class = "preprocessor" > #else</ span ></ div >
< div class = "line" >< a id = "l01246" name = "l01246" ></ a >< span class = "lineno" > 1246</ span > CvSize(imageSize), & c_R, & c_T, & c_R1, & c_R2, & c_P1, & c_P2, p_Q, flags, alpha,</ div >
< div class = "line" >< a id = "l01247" name = "l01247" ></ a >< span class = "lineno" > 1247</ span > CvSize(newImageSize));</ div >
< div class = "line" >< a id = "l01248" name = "l01248" ></ a >< span class = "lineno" > 1248</ span >< span class = "preprocessor" > #endif</ span ></ div >
< div class = "line" >< a id = "l01249" name = "l01249" ></ a >< span class = "lineno" > 1249</ span > }</ div >
< div class = "line" >< a id = "l01250" name = "l01250" ></ a >< span class = "lineno" > 1250</ span > </ div >
< div class = "line" >< a id = "l01251" name = "l01251" ></ a >< span class = "lineno" > 1251</ span > }</ div >
< div class = "line" >< a id = "l01252" name = "l01252" ></ a >< span class = "lineno" > 1252</ span > </ div >
< div class = "line" >< a id = "l01253" name = "l01253" ></ a >< span class = "lineno" > 1253</ span > </ div >
< div class = "line" >< a id = "l01254" name = "l01254" ></ a >< span class = "lineno" > 1254</ span >< span class = "preprocessor" > #endif </ span >< span class = "comment" > /* CORELIB_SRC_OPENCV_STEREORECTIFYFISHEYE_H_ */</ span >< span class = "preprocessor" ></ span ></ div >
< div class = "ttc" id = "anamespacertabmap_html" >< div class = "ttname" >< a href = "namespacertabmap.html" > rtabmap</ a ></ div >< div class = "ttdef" >< b > Definition</ b > < a href = "BayesFilter_8h_source.html#l00039" > BayesFilter.h:39</ a ></ div ></ div >
</ div > <!-- fragment --> </ div > <!-- contents -->
2026-08-02 07:56:38 +00:00
</ div > <!-- doc-content -->
2026-08-02 01:36:11 +00:00
<!-- start footer part -->
2026-08-02 07:56:38 +00:00
< div id = "nav-path" class = "navpath" > <!-- id is needed for treeview function! -->
< ul >
< li class = "navelem" >< a class = "el" href = "dir_04d49acea3c42cab2afcd35cb2f2a48f.html" > corelib</ a ></ li >< li class = "navelem" >< a class = "el" href = "dir_e996b11fd4b3a747423759b16a8ba4a6.html" > include</ a ></ li >< li class = "navelem" >< a class = "el" href = "dir_ff01f7bc5399ccf9ff1cc91d1f45726a.html" > rtabmap</ a ></ li >< li class = "navelem" >< a class = "el" href = "dir_884419f21456e80aa47bc1d0c2ab0140.html" > core</ a ></ li >< li class = "navelem" >< a class = "el" href = "dir_7b07a0fe62d0fac6129c48e2cb6c7633.html" > stereo</ a ></ li >< li class = "navelem" >< b > stereoRectifyFisheye.h</ b ></ li >
< li class = "footer" > Generated by < a href = "https://www.doxygen.org/index.html" >< img class = "footer" src = "doxygen.svg" width = "104" height = "31" alt = "doxygen" /></ a > 1.9.8 </ li >
</ ul >
</ div >
2026-08-02 01:36:11 +00:00
</ body >
</ html >
