Files
rtabmap/preview/pr-1492/api/latest/stereoRectifyFisheye_8h_source.html
T

1374 lines
182 KiB
HTML

<!-- 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>
<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>
<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" />
<link href="doxygen-awesome.css" rel="stylesheet" type="text/css"/>
<link href="custom.css" rel="stylesheet" type="text/css"/>
<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>
<script type="text/javascript" src="doxygen-awesome-darkmode-toggle.js"></script>
<script type="text/javascript">DoxygenAwesomeDarkModeToggle.init();</script>
</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">&#160;0.23.10</span>
</div>
<div id="projectbrief">Real-Time Appearance-Based Mapping</div>
</td>
</tr>
</tbody>
</table>
</div>
<!-- end header part -->
<!-- Generated by Doxygen 1.9.8 -->
<script type="text/javascript">
/* @license magnet:?xt=urn:btih:d3d9a9a6595521f9666a5e94cc830dab83b65699&amp;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&amp;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&amp;dn=expat.txt MIT */
$(document).ready(function() { init_codefold(0); });
/* @license-end */
</script>
</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&amp;dn=expat.txt MIT */
$(document).ready(function(){initNavTree('stereoRectifyFisheye_8h_source.html',''); initResizable(); });
/* @license-end */
</script>
<div id="doc-content">
<!-- 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 &quot;AS IS&quot; 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 &lt;opencv2/core/version.hpp&gt;</span></div>
<div class="line"><a id="l00040" name="l00040"></a><span class="lineno"> 40</span><span class="preprocessor">#if CV_MAJOR_VERSION &gt;= 5</span></div>
<div class="line"><a id="l00041" name="l00041"></a><span class="lineno"> 41</span><span class="preprocessor">#error &quot;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 &#39;#if CV_MAJOR_VERSION &lt; 5&#39;.&quot;</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 &lt;opencv2/calib3d/calib3d.hpp&gt;</span></div>
<div class="line"><a id="l00045" name="l00045"></a><span class="lineno"> 45</span><span class="preprocessor">#if CV_MAJOR_VERSION &gt;= 3</span></div>
<div class="line"><a id="l00046" name="l00046"></a><span class="lineno"> 46</span><span class="preprocessor">#include &lt;opencv2/calib3d/calib3d_c.h&gt;</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 &gt;= 4</span></div>
<div class="line"><a id="l00049" name="l00049"></a><span class="lineno"> 49</span><span class="preprocessor">#include &lt;opencv2/core/core_c.h&gt;</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&#39;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">&quot;Input argument is not a valid matrix&quot;</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">&quot;The first output argument is not a valid matrix&quot;</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-&gt;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 &amp;&amp; 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">&quot;The matrices must have 32f or 64f data type&quot;</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">&quot;All the matrices must have the same data type&quot;</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">&quot;Jacobian is not a valid matrix&quot;</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-&gt;type) != 1 )</div>
<div class="line"><a id="l00081" name="l00081"></a><span class="lineno"> 81</span> CV_Error( CV_StsUnmatchedFormats, <span class="stringliteral">&quot;Jacobian must have 32fC1 or 64fC1 datatype&quot;</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-&gt;rows != 9 || jacobian-&gt;cols != 3) &amp;&amp;</div>
<div class="line"><a id="l00084" name="l00084"></a><span class="lineno"> 84</span> (jacobian-&gt;rows != 3 || jacobian-&gt;cols != 9))</div>
<div class="line"><a id="l00085" name="l00085"></a><span class="lineno"> 85</span> CV_Error( CV_StsBadSize, <span class="stringliteral">&quot;Jacobian must be 3x9 or 9x3&quot;</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-&gt;cols == 1 || src-&gt;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-&gt;rows &gt; 1 ? src-&gt;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-&gt;rows + src-&gt;cols*CV_MAT_CN(src-&gt;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">&quot;Input matrix must be 1x3, 3x1 or 3x3&quot;</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-&gt;rows != 3 || dst-&gt;cols != 3 || CV_MAT_CN(dst-&gt;type) != 1 )</div>
<div class="line"><a id="l00096" name="l00096"></a><span class="lineno"> 96</span> CV_Error( CV_StsBadSize, <span class="stringliteral">&quot;Output matrix must be 3x3, single-channel floating point matrix&quot;</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-&gt;data.fl[0];</div>
<div class="line"><a id="l00102" name="l00102"></a><span class="lineno"> 102</span> r.y = src-&gt;data.fl[step];</div>
<div class="line"><a id="l00103" name="l00103"></a><span class="lineno"> 103</span> r.z = src-&gt;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-&gt;data.db[0];</div>
<div class="line"><a id="l00108" name="l00108"></a><span class="lineno"> 108</span> r.y = src-&gt;data.db[step];</div>
<div class="line"><a id="l00109" name="l00109"></a><span class="lineno"> 109</span> r.z = src-&gt;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 &lt; 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-&gt;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 &lt; 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 &lt; 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-&gt;cols == 3 &amp;&amp; src-&gt;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-&gt;rows &gt; 1 ? dst-&gt;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-&gt;rows != 1 || dst-&gt;cols*CV_MAT_CN(dst-&gt;type) != 3) &amp;&amp;</div>
<div class="line"><a id="l00173" name="l00173"></a><span class="lineno"> 173</span> (dst-&gt;rows != 3 || dst-&gt;cols != 1 || CV_MAT_CN(dst-&gt;type) != 1))</div>
<div class="line"><a id="l00174" name="l00174"></a><span class="lineno"> 174</span> CV_Error( CV_StsBadSize, <span class="stringliteral">&quot;Output matrix must be 1x3 or 3x1&quot;</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 &gt; 1. ? 1. : c &lt; -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 &lt; 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 &gt; 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) &lt; 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) &lt; 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) &lt; fabs(r.y) &amp;&amp; fabs(r.x) &lt; fabs(r.z) &amp;&amp; (R(1, 2) &gt; 0) != (r.y*r.z &gt; 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 &gt; 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( &amp;_domegadvar2, &amp;_dvar2dvar, &amp;_t0 );</div>
<div class="line"><a id="l00269" name="l00269"></a><span class="lineno"> 269</span> cvMatMul( &amp;_t0, &amp;_dvardR, &amp;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-&gt;data.fl[0] = (float)r.x;</div>
<div class="line"><a id="l00284" name="l00284"></a><span class="lineno"> 284</span> dst-&gt;data.fl[step] = (float)r.y;</div>
<div class="line"><a id="l00285" name="l00285"></a><span class="lineno"> 285</span> dst-&gt;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-&gt;data.db[0] = r.x;</div>
<div class="line"><a id="l00290" name="l00290"></a><span class="lineno"> 290</span> dst-&gt;data.db[step] = r.y;</div>
<div class="line"><a id="l00291" name="l00291"></a><span class="lineno"> 291</span> dst-&gt;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-&gt;rows == matJ.rows )</div>
<div class="line"><a id="l00300" name="l00300"></a><span class="lineno"> 300</span> cvConvert( &amp;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( &amp;matJ, &amp;_Jf );</div>
<div class="line"><a id="l00306" name="l00306"></a><span class="lineno"> 306</span> cvTranspose( &amp;_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-&gt;rows == matJ.rows )</div>
<div class="line"><a id="l00310" name="l00310"></a><span class="lineno"> 310</span> cvCopy( &amp;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( &amp;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> &lt;<span class="keyword">typename</span> FLOAT&gt;</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&lt;FLOAT, 3, 3&gt;* matTilt = 0,</div>
<div class="line"><a id="l00322" name="l00322"></a><span class="lineno"> 322</span> cv::Matx&lt;FLOAT, 3, 3&gt;* dMatTiltdTauX = 0,</div>
<div class="line"><a id="l00323" name="l00323"></a><span class="lineno"> 323</span> cv::Matx&lt;FLOAT, 3, 3&gt;* dMatTiltdTauY = 0,</div>
<div class="line"><a id="l00324" name="l00324"></a><span class="lineno"> 324</span> cv::Matx&lt;FLOAT, 3, 3&gt;* 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&lt;FLOAT, 3, 3&gt; matRotX = cv::Matx&lt;FLOAT, 3, 3&gt;(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&lt;FLOAT, 3, 3&gt; matRotY = cv::Matx&lt;FLOAT, 3, 3&gt;(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&lt;FLOAT, 3, 3&gt; matRotXY = matRotY * matRotX;</div>
<div class="line"><a id="l00333" name="l00333"></a><span class="lineno"> 333</span> cv::Matx&lt;FLOAT, 3, 3&gt; matProjZ = cv::Matx&lt;FLOAT, 3, 3&gt;(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&lt;FLOAT, 3, 3&gt; dMatRotXYdTauX = matRotY * cv::Matx&lt;FLOAT, 3, 3&gt;(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&lt;FLOAT, 3, 3&gt; dMatProjZdTauX = cv::Matx&lt;FLOAT, 3, 3&gt;(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&lt;FLOAT, 3, 3&gt; dMatRotXYdTauY = cv::Matx&lt;FLOAT, 3, 3&gt;(-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&lt;FLOAT, 3, 3&gt; dMatProjZdTauY = cv::Matx&lt;FLOAT, 3, 3&gt;(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&lt;FLOAT, 3, 3&gt; invMatProjZ = cv::Matx&lt;FLOAT, 3, 3&gt;(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&lt;CvMat&gt; matM, _m;</div>
<div class="line"><a id="l00375" name="l00375"></a><span class="lineno"> 375</span> cv::Ptr&lt;CvMat&gt; _dpdr, _dpdt, _dpdc, _dpdf, _dpdk;</div>
<div class="line"><a id="l00376" name="l00376"></a><span class="lineno"> 376</span> cv::Ptr&lt;CvMat&gt; _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 &gt; 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">&quot;One of required arguments is not a valid matrix&quot;</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-&gt;rows * objectPoints-&gt;cols * CV_MAT_CN(objectPoints-&gt;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">&quot;Homogeneous coordinates are not supported&quot;</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-&gt;type) &amp;&amp;</div>
<div class="line"><a id="l00408" name="l00408"></a><span class="lineno"> 408</span> (CV_MAT_DEPTH(objectPoints-&gt;type) == CV_32F || CV_MAT_DEPTH(objectPoints-&gt;type) == CV_64F)&amp;&amp;</div>
<div class="line"><a id="l00409" name="l00409"></a><span class="lineno"> 409</span> ((objectPoints-&gt;rows == 1 &amp;&amp; CV_MAT_CN(objectPoints-&gt;type) == 3) ||</div>
<div class="line"><a id="l00410" name="l00410"></a><span class="lineno"> 410</span> (objectPoints-&gt;rows == count &amp;&amp; CV_MAT_CN(objectPoints-&gt;type)*objectPoints-&gt;cols == 3) ||</div>
<div class="line"><a id="l00411" name="l00411"></a><span class="lineno"> 411</span> (objectPoints-&gt;rows == 3 &amp;&amp; CV_MAT_CN(objectPoints-&gt;type) == 1 &amp;&amp; objectPoints-&gt;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-&gt;rows, objectPoints-&gt;cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(objectPoints-&gt;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">&quot;Homogeneous coordinates are not supported&quot;</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-&gt;type) &amp;&amp;</div>
<div class="line"><a id="l00424" name="l00424"></a><span class="lineno"> 424</span> (CV_MAT_DEPTH(imagePoints-&gt;type) == CV_32F || CV_MAT_DEPTH(imagePoints-&gt;type) == CV_64F) &amp;&amp;</div>
<div class="line"><a id="l00425" name="l00425"></a><span class="lineno"> 425</span> ((imagePoints-&gt;rows == 1 &amp;&amp; CV_MAT_CN(imagePoints-&gt;type) == 2) ||</div>
<div class="line"><a id="l00426" name="l00426"></a><span class="lineno"> 426</span> (imagePoints-&gt;rows == count &amp;&amp; CV_MAT_CN(imagePoints-&gt;type)*imagePoints-&gt;cols == 2) ||</div>
<div class="line"><a id="l00427" name="l00427"></a><span class="lineno"> 427</span> (imagePoints-&gt;rows == 2 &amp;&amp; CV_MAT_CN(imagePoints-&gt;type) == 1 &amp;&amp; imagePoints-&gt;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-&gt;rows, imagePoints-&gt;cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(imagePoints-&gt;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">&quot;Homogeneous coordinates are not supported&quot;</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-&gt;data.db;</div>
<div class="line"><a id="l00439" name="l00439"></a><span class="lineno"> 439</span> m = (CvPoint2D64f*)_m-&gt;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-&gt;type) != CV_64F &amp;&amp; CV_MAT_DEPTH(r_vec-&gt;type) != CV_32F) ||</div>
<div class="line"><a id="l00442" name="l00442"></a><span class="lineno"> 442</span> (((r_vec-&gt;rows != 1 &amp;&amp; r_vec-&gt;cols != 1) ||</div>
<div class="line"><a id="l00443" name="l00443"></a><span class="lineno"> 443</span> r_vec-&gt;rows*r_vec-&gt;cols*CV_MAT_CN(r_vec-&gt;type) != 3) &amp;&amp;</div>
<div class="line"><a id="l00444" name="l00444"></a><span class="lineno"> 444</span> ((r_vec-&gt;rows != 3 &amp;&amp; r_vec-&gt;cols != 3) || CV_MAT_CN(r_vec-&gt;type) != 1)))</div>
<div class="line"><a id="l00445" name="l00445"></a><span class="lineno"> 445</span> CV_Error( CV_StsBadArg, <span class="stringliteral">&quot;Rotation must be represented by 1x3 or 3x1 &quot;</span></div>
<div class="line"><a id="l00446" name="l00446"></a><span class="lineno"> 446</span> <span class="stringliteral">&quot;floating-point rotation vector, or 3x3 rotation matrix&quot;</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-&gt;rows == 3 &amp;&amp; r_vec-&gt;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, &amp;_r );</div>
<div class="line"><a id="l00452" name="l00452"></a><span class="lineno"> 452</span> cvRodrigues2( &amp;_r, &amp;matR, &amp;_dRdr );</div>
<div class="line"><a id="l00453" name="l00453"></a><span class="lineno"> 453</span> cvCopy( r_vec, &amp;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-&gt;rows, r_vec-&gt;cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(r_vec-&gt;type)), r );</div>
<div class="line"><a id="l00458" name="l00458"></a><span class="lineno"> 458</span> cvConvert( r_vec, &amp;_r );</div>
<div class="line"><a id="l00459" name="l00459"></a><span class="lineno"> 459</span> cvRodrigues2( &amp;_r, &amp;matR, &amp;_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-&gt;type) != CV_64F &amp;&amp; CV_MAT_DEPTH(t_vec-&gt;type) != CV_32F) ||</div>
<div class="line"><a id="l00463" name="l00463"></a><span class="lineno"> 463</span> (t_vec-&gt;rows != 1 &amp;&amp; t_vec-&gt;cols != 1) ||</div>
<div class="line"><a id="l00464" name="l00464"></a><span class="lineno"> 464</span> t_vec-&gt;rows*t_vec-&gt;cols*CV_MAT_CN(t_vec-&gt;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">&quot;Translation vector must be 1x3 or 3x1 floating-point vector&quot;</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-&gt;rows, t_vec-&gt;cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(t_vec-&gt;type)), t );</div>
<div class="line"><a id="l00469" name="l00469"></a><span class="lineno"> 469</span> cvConvert( t_vec, &amp;_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-&gt;type) != CV_64FC1 &amp;&amp; CV_MAT_TYPE(A-&gt;type) != CV_32FC1) ||</div>
<div class="line"><a id="l00472" name="l00472"></a><span class="lineno"> 472</span> A-&gt;rows != 3 || A-&gt;cols != 3 )</div>
<div class="line"><a id="l00473" name="l00473"></a><span class="lineno"> 473</span> CV_Error( CV_StsBadArg, <span class="stringliteral">&quot;Instrinsic parameters must be 3x3 floating-point matrix&quot;</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, &amp;_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-&gt;type) != CV_64F &amp;&amp;</div>
<div class="line"><a id="l00486" name="l00486"></a><span class="lineno"> 486</span> CV_MAT_DEPTH(distCoeffs-&gt;type) != CV_32F) ||</div>
<div class="line"><a id="l00487" name="l00487"></a><span class="lineno"> 487</span> (distCoeffs-&gt;rows != 1 &amp;&amp; distCoeffs-&gt;cols != 1) ||</div>
<div class="line"><a id="l00488" name="l00488"></a><span class="lineno"> 488</span> (distCoeffs-&gt;rows*distCoeffs-&gt;cols*CV_MAT_CN(distCoeffs-&gt;type) != 4 &amp;&amp;</div>
<div class="line"><a id="l00489" name="l00489"></a><span class="lineno"> 489</span> distCoeffs-&gt;rows*distCoeffs-&gt;cols*CV_MAT_CN(distCoeffs-&gt;type) != 5 &amp;&amp;</div>
<div class="line"><a id="l00490" name="l00490"></a><span class="lineno"> 490</span> distCoeffs-&gt;rows*distCoeffs-&gt;cols*CV_MAT_CN(distCoeffs-&gt;type) != 8 &amp;&amp;</div>
<div class="line"><a id="l00491" name="l00491"></a><span class="lineno"> 491</span> distCoeffs-&gt;rows*distCoeffs-&gt;cols*CV_MAT_CN(distCoeffs-&gt;type) != 12 &amp;&amp;</div>
<div class="line"><a id="l00492" name="l00492"></a><span class="lineno"> 492</span> distCoeffs-&gt;rows*distCoeffs-&gt;cols*CV_MAT_CN(distCoeffs-&gt;type) != 14) )</div>
<div class="line"><a id="l00493" name="l00493"></a><span class="lineno"> 493</span> CV_Error( CV_StsBadArg, <span class="stringliteral">&quot;Distortion coefficients must be 1x4, 4x1, 1x5, 5x1, 1x8, 8x1, 1x12, 12x1, 1x14 or 14x1 floating-point vector&quot;</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-&gt;rows, distCoeffs-&gt;cols,</div>
<div class="line"><a id="l00496" name="l00496"></a><span class="lineno"> 496</span> CV_MAKETYPE(CV_64F,CV_MAT_CN(distCoeffs-&gt;type)), k );</div>
<div class="line"><a id="l00497" name="l00497"></a><span class="lineno"> 497</span> cvConvert( distCoeffs, &amp;_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> &amp;matTilt, &amp;dMatTiltdTauX, &amp;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-&gt;type) != CV_32FC1 &amp;&amp;</div>
<div class="line"><a id="l00509" name="l00509"></a><span class="lineno"> 509</span> CV_MAT_TYPE(dpdr-&gt;type) != CV_64FC1) ||</div>
<div class="line"><a id="l00510" name="l00510"></a><span class="lineno"> 510</span> dpdr-&gt;rows != count*2 || dpdr-&gt;cols != 3 )</div>
<div class="line"><a id="l00511" name="l00511"></a><span class="lineno"> 511</span> CV_Error( CV_StsBadArg, <span class="stringliteral">&quot;dp/drot must be 2Nx3 floating-point matrix&quot;</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-&gt;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-&gt;data.db;</div>
<div class="line"><a id="l00520" name="l00520"></a><span class="lineno"> 520</span> dpdr_step = _dpdr-&gt;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-&gt;type) != CV_32FC1 &amp;&amp;</div>
<div class="line"><a id="l00527" name="l00527"></a><span class="lineno"> 527</span> CV_MAT_TYPE(dpdt-&gt;type) != CV_64FC1) ||</div>
<div class="line"><a id="l00528" name="l00528"></a><span class="lineno"> 528</span> dpdt-&gt;rows != count*2 || dpdt-&gt;cols != 3 )</div>
<div class="line"><a id="l00529" name="l00529"></a><span class="lineno"> 529</span> CV_Error( CV_StsBadArg, <span class="stringliteral">&quot;dp/dT must be 2Nx3 floating-point matrix&quot;</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-&gt;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-&gt;data.db;</div>
<div class="line"><a id="l00538" name="l00538"></a><span class="lineno"> 538</span> dpdt_step = _dpdt-&gt;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-&gt;type) != CV_32FC1 &amp;&amp; CV_MAT_TYPE(dpdf-&gt;type) != CV_64FC1) ||</div>
<div class="line"><a id="l00545" name="l00545"></a><span class="lineno"> 545</span> dpdf-&gt;rows != count*2 || dpdf-&gt;cols != 2 )</div>
<div class="line"><a id="l00546" name="l00546"></a><span class="lineno"> 546</span> CV_Error( CV_StsBadArg, <span class="stringliteral">&quot;dp/df must be 2Nx2 floating-point matrix&quot;</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-&gt;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-&gt;data.db;</div>
<div class="line"><a id="l00555" name="l00555"></a><span class="lineno"> 555</span> dpdf_step = _dpdf-&gt;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-&gt;type) != CV_32FC1 &amp;&amp; CV_MAT_TYPE(dpdc-&gt;type) != CV_64FC1) ||</div>
<div class="line"><a id="l00562" name="l00562"></a><span class="lineno"> 562</span> dpdc-&gt;rows != count*2 || dpdc-&gt;cols != 2 )</div>
<div class="line"><a id="l00563" name="l00563"></a><span class="lineno"> 563</span> CV_Error( CV_StsBadArg, <span class="stringliteral">&quot;dp/dc must be 2Nx2 floating-point matrix&quot;</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-&gt;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-&gt;data.db;</div>
<div class="line"><a id="l00572" name="l00572"></a><span class="lineno"> 572</span> dpdc_step = _dpdc-&gt;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-&gt;type) != CV_32FC1 &amp;&amp; CV_MAT_TYPE(dpdk-&gt;type) != CV_64FC1) ||</div>
<div class="line"><a id="l00579" name="l00579"></a><span class="lineno"> 579</span> dpdk-&gt;rows != count*2 || (dpdk-&gt;cols != 14 &amp;&amp; dpdk-&gt;cols != 12 &amp;&amp; dpdk-&gt;cols != 8 &amp;&amp; dpdk-&gt;cols != 5 &amp;&amp; dpdk-&gt;cols != 4 &amp;&amp; dpdk-&gt;cols != 2) )</div>
<div class="line"><a id="l00580" name="l00580"></a><span class="lineno"> 580</span> CV_Error( CV_StsBadArg, <span class="stringliteral">&quot;dp/df must be 2Nx14, 2Nx12, 2Nx8, 2Nx5, 2Nx4 or 2Nx2 floating-point matrix&quot;</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">&quot;distCoeffs is NULL while dpdk is not&quot;</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-&gt;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-&gt;rows, dpdk-&gt;cols, CV_64FC1 ));</div>
<div class="line"><a id="l00591" name="l00591"></a><span class="lineno"> 591</span> dpdk_p = _dpdk-&gt;data.db;</div>
<div class="line"><a id="l00592" name="l00592"></a><span class="lineno"> 592</span> dpdk_step = _dpdk-&gt;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-&gt;type ) != CV_32FC1</div>
<div class="line"><a id="l00598" name="l00598"></a><span class="lineno"> 598</span> &amp;&amp; CV_MAT_TYPE( dpdo-&gt;type ) != CV_64FC1 )</div>
<div class="line"><a id="l00599" name="l00599"></a><span class="lineno"> 599</span> || dpdo-&gt;rows != count * 2 || dpdo-&gt;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">&quot;dp/do must be 2Nx3N floating-point matrix&quot;</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-&gt;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-&gt;data.db;</div>
<div class="line"><a id="l00610" name="l00610"></a><span class="lineno"> 610</span> dpdo_step = _dpdo-&gt;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 &lt; 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 &lt; 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 &lt; 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-&gt;cols &gt; 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-&gt;cols &gt; 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-&gt;cols &gt; 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-&gt;cols &gt; 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-&gt;cols &gt; 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 &lt; 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 &lt; 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 &lt; 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() &gt; 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 &amp;&amp; src.cols &gt; 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() &gt; 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 &lt; 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 &amp;&amp; dst0.cols &gt; 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 &amp;&amp; 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_&lt;float&gt;&amp; inner, cv::Rect_&lt;float&gt;&amp; 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 &lt; 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 &lt; 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-&gt;rows, cameraMatrix-&gt;cols, cameraMatrix-&gt;type, cameraMatrix-&gt;data.ptr);</div>
<div class="line"><a id="l00950" name="l00950"></a><span class="lineno"> 950</span> cv::Mat distCoeffsM(distCoeffs-&gt;rows, distCoeffs-&gt;cols, distCoeffs-&gt;type, distCoeffs-&gt;data.ptr);</div>
<div class="line"><a id="l00951" name="l00951"></a><span class="lineno"> 951</span> cv::Mat RM(R-&gt;rows, R-&gt;cols, R-&gt;type, R-&gt;data.ptr);</div>
<div class="line"><a id="l00952" name="l00952"></a><span class="lineno"> 952</span> cv::Mat newCameraMatrixM(newCameraMatrix-&gt;rows, newCameraMatrix-&gt;cols, newCameraMatrix-&gt;type, newCameraMatrix-&gt;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) (&gt;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 &lt; 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 &lt; 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_&lt;float&gt;(iX0, iY0, iX1-iX0, iY1-iY0);</div>
<div class="line"><a id="l00977" name="l00977"></a><span class="lineno"> 977</span> outer = cv::Rect_&lt;float&gt;(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_&lt;float&gt; 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-&gt;rows == 3 &amp;&amp; matR-&gt;cols == 3 )</div>
<div class="line"><a id="l01005" name="l01005"></a><span class="lineno"> 1005</span> cvRodrigues2(matR, &amp;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, &amp;om); <span class="comment">// it&#39;s already a rotation vector</span></div>
<div class="line"><a id="l01008" name="l01008"></a><span class="lineno"> 1008</span> cvConvertScale(&amp;om, &amp;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(&amp;om, &amp;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(&amp;r_r, matT, &amp;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]) &gt; 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(&amp;uu, &amp;t, &amp;ww);</div>
<div class="line"><a id="l01024" name="l01024"></a><span class="lineno"> 1024</span> nt = cvNorm(&amp;t, 0, CV_L2);</div>
<div class="line"><a id="l01025" name="l01025"></a><span class="lineno"> 1025</span> CV_Assert(fabs(nt) &gt; 0);</div>
<div class="line"><a id="l01026" name="l01026"></a><span class="lineno"> 1026</span> nw = cvNorm(&amp;ww, 0, CV_L2);</div>
<div class="line"><a id="l01027" name="l01027"></a><span class="lineno"> 1027</span> CV_Assert(fabs(nw) &gt; 0);</div>
<div class="line"><a id="l01028" name="l01028"></a><span class="lineno"> 1028</span> cvConvertScale(&amp;ww, &amp;ww, 1 / nw);</div>
<div class="line"><a id="l01029" name="l01029"></a><span class="lineno"> 1029</span> cvCrossProduct(&amp;t, &amp;ww, &amp;w3);</div>
<div class="line"><a id="l01030" name="l01030"></a><span class="lineno"> 1030</span> nw = cvNorm(&amp;w3, 0, CV_L2);</div>
<div class="line"><a id="l01031" name="l01031"></a><span class="lineno"> 1031</span> CV_Assert(fabs(nw) &gt; 0);</div>
<div class="line"><a id="l01032" name="l01032"></a><span class="lineno"> 1032</span> cvConvertScale(&amp;w3, &amp;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 &lt; 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 -&gt; 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(&amp;wR, &amp;r_r, 1, 0, 0, &amp;Ri, CV_GEMM_B_T);</div>
<div class="line"><a id="l01042" name="l01042"></a><span class="lineno"> 1042</span> cvConvert( &amp;Ri, _R1 );</div>
<div class="line"><a id="l01043" name="l01043"></a><span class="lineno"> 1043</span> cvGEMM(&amp;wR, &amp;r_r, 1, 0, 0, &amp;Ri, 0);</div>
<div class="line"><a id="l01044" name="l01044"></a><span class="lineno"> 1044</span> cvConvert( &amp;Ri, _R2 );</div>
<div class="line"><a id="l01045" name="l01045"></a><span class="lineno"> 1045</span> cvMatMul(&amp;Ri, matT, &amp;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 &lt; 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 &lt; 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&lt;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-&gt;rows, A-&gt;cols, A-&gt;type, A-&gt;data.ptr);</div>
<div class="line"><a id="l01073" name="l01073"></a><span class="lineno"> 1073</span> cv::Mat Dk_m(Dk-&gt;rows, Dk-&gt;cols, Dk-&gt;type, Dk-&gt;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( &amp;pts, &amp;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( &amp;pts_3, k == 0 ? _R1 : _R2, &amp;Z, &amp;A_tmp, 0, &amp;pts );</div>
<div class="line"><a id="l01086" name="l01086"></a><span class="lineno"> 1086</span> CvScalar avg = cvAvg(&amp;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 &amp; 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( &amp;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(&amp;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(&amp;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 &gt;= 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( &amp;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 &gt; 3 || (CV_MAJOR_VERSION &gt;= 3 &amp;&amp; (CV_MINOR_VERSION&gt;4 || (CV_MINOR_VERSION&gt;=4 &amp;&amp; CV_SUBMINOR_VERSION&gt;=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 &gt; 3 || (CV_MAJOR_VERSION &gt;= 3 &amp;&amp; (CV_MINOR_VERSION&gt;4 || (CV_MINOR_VERSION&gt;=4 &amp;&amp; CV_SUBMINOR_VERSION&gt;=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 &gt; 3 || (CV_MAJOR_VERSION &gt;= 3 &amp;&amp; (CV_MINOR_VERSION&gt;4 || (CV_MINOR_VERSION&gt;=4 &amp;&amp; CV_SUBMINOR_VERSION&gt;=4)))</span></div>
<div class="line"><a id="l01233" name="l01233"></a><span class="lineno"> 1233</span> p_Q = &amp;(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 = &amp;(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 : &amp;c_distCoeffs1;</div>
<div class="line"><a id="l01240" name="l01240"></a><span class="lineno"> 1240</span> CvMat *p_distCoeffs2 = distCoeffs2.empty() ? NULL : &amp;c_distCoeffs2;</div>
<div class="line"><a id="l01241" name="l01241"></a><span class="lineno"> 1241</span> cvStereoRectifyFisheye( &amp;c_cameraMatrix1, &amp;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 &gt; 3 || (CV_MAJOR_VERSION &gt;= 3 &amp;&amp; (CV_MINOR_VERSION&gt;4 || (CV_MINOR_VERSION&gt;=4 &amp;&amp; CV_SUBMINOR_VERSION&gt;=4)))</div>
<div class="line"><a id="l01243" name="l01243"></a><span class="lineno"> 1243</span> cvSize(imageSize), &amp;c_R, &amp;c_T, &amp;c_R1, &amp;c_R2, &amp;c_P1, &amp;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), &amp;c_R, &amp;c_T, &amp;c_R1, &amp;c_R2, &amp;c_P1, &amp;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 -->
</div><!-- doc-content -->
<!-- start footer part -->
<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>
</body>
</html>