<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN">
<html><head><meta http-equiv="Content-Type" content="text/html;charset=iso-8859-1">
<title>CCamModel.h Source File</title>
<link href="doxygen.css" rel="stylesheet" type="text/css">
<link href="tabs.css" rel="stylesheet" type="text/css">
</head><body>
<div align="left"><a href="http://www.mrpt.org/">Main MRPT website</a> > <b>C++ reference</b> </div>
<div align="right">
<a href="index.html"><img border="0" src="mrpt_logo.png" alt="MRPT logo"></a>
</div>
<!-- Generated by Doxygen 1.7.5 -->
<script type="text/javascript">
var searchBox = new SearchBox("searchBox", "search",false,'Search');
</script>
<div id="navrow1" class="tabs">
<ul class="tablist">
<li><a href="index.html"><span>Main Page</span></a></li>
<li><a href="pages.html"><span>Related Pages</span></a></li>
<li><a href="modules.html"><span>Modules</span></a></li>
<li><a href="namespaces.html"><span>Namespaces</span></a></li>
<li><a href="annotated.html"><span>Classes</span></a></li>
<li class="current"><a href="files.html"><span>Files</span></a></li>
<li>
<div id="MSearchBox" class="MSearchBoxInactive">
<div class="left">
<form id="FSearchBox" action="search.php" method="get">
<img id="MSearchSelect" src="search/mag.png" alt=""/>
<input type="text" id="MSearchField" name="query" value="Search" size="20" accesskey="S"
onfocus="searchBox.OnSearchFieldFocus(true)"
onblur="searchBox.OnSearchFieldFocus(false)"/>
</form>
</div><div class="right"></div>
</div>
</li>
</ul>
</div>
<div id="navrow2" class="tabs2">
<ul class="tablist">
<li><a href="files.html"><span>File List</span></a></li>
<li><a href="globals.html"><span>File Members</span></a></li>
</ul>
</div>
<div class="header">
<div class="headertitle">
<div class="title">CCamModel.h</div> </div>
</div>
<div class="contents">
<a href="_c_cam_model_8h.html">Go to the documentation of this file.</a><div class="fragment"><pre class="fragment"><a name="l00001"></a>00001 <span class="comment">/* +---------------------------------------------------------------------------+</span>
<a name="l00002"></a>00002 <span class="comment"> | The Mobile Robot Programming Toolkit (MRPT) C++ library |</span>
<a name="l00003"></a>00003 <span class="comment"> | |</span>
<a name="l00004"></a>00004 <span class="comment"> | http://www.mrpt.org/ |</span>
<a name="l00005"></a>00005 <span class="comment"> | |</span>
<a name="l00006"></a>00006 <span class="comment"> | Copyright (C) 2005-2011 University of Malaga |</span>
<a name="l00007"></a>00007 <span class="comment"> | |</span>
<a name="l00008"></a>00008 <span class="comment"> | This software was written by the Machine Perception and Intelligent |</span>
<a name="l00009"></a>00009 <span class="comment"> | Robotics Lab, University of Malaga (Spain). |</span>
<a name="l00010"></a>00010 <span class="comment"> | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> |</span>
<a name="l00011"></a>00011 <span class="comment"> | |</span>
<a name="l00012"></a>00012 <span class="comment"> | This file is part of the MRPT project. |</span>
<a name="l00013"></a>00013 <span class="comment"> | |</span>
<a name="l00014"></a>00014 <span class="comment"> | MRPT is free software: you can redistribute it and/or modify |</span>
<a name="l00015"></a>00015 <span class="comment"> | it under the terms of the GNU General Public License as published by |</span>
<a name="l00016"></a>00016 <span class="comment"> | the Free Software Foundation, either version 3 of the License, or |</span>
<a name="l00017"></a>00017 <span class="comment"> | (at your option) any later version. |</span>
<a name="l00018"></a>00018 <span class="comment"> | |</span>
<a name="l00019"></a>00019 <span class="comment"> | MRPT is distributed in the hope that it will be useful, |</span>
<a name="l00020"></a>00020 <span class="comment"> | but WITHOUT ANY WARRANTY; without even the implied warranty of |</span>
<a name="l00021"></a>00021 <span class="comment"> | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |</span>
<a name="l00022"></a>00022 <span class="comment"> | GNU General Public License for more details. |</span>
<a name="l00023"></a>00023 <span class="comment"> | |</span>
<a name="l00024"></a>00024 <span class="comment"> | You should have received a copy of the GNU General Public License |</span>
<a name="l00025"></a>00025 <span class="comment"> | along with MRPT. If not, see <http://www.gnu.org/licenses/>. |</span>
<a name="l00026"></a>00026 <span class="comment"> | |</span>
<a name="l00027"></a>00027 <span class="comment"> +---------------------------------------------------------------------------+ */</span>
<a name="l00028"></a>00028 <span class="preprocessor">#ifndef CCamModel_H</span>
<a name="l00029"></a>00029 <span class="preprocessor"></span><span class="preprocessor">#define CCamModel_H</span>
<a name="l00030"></a>00030 <span class="preprocessor"></span>
<a name="l00031"></a>00031 <span class="preprocessor">#include <<a class="code" href="_t_camera_8h.html">mrpt/utils/TCamera.h</a>></span>
<a name="l00032"></a>00032 <span class="preprocessor">#include <<a class="code" href="os_8h.html">mrpt/system/os.h</a>></span>
<a name="l00033"></a>00033 <span class="preprocessor">#include <<a class="code" href="vision_2include_2mrpt_2vision_2utils_8h.html">mrpt/vision/utils.h</a>></span>
<a name="l00034"></a>00034 <span class="preprocessor">#include <<a class="code" href="lightweight__geom__data_8h.html">mrpt/math/lightweight_geom_data.h</a>></span>
<a name="l00035"></a>00035
<a name="l00036"></a>00036 <span class="keyword">namespace </span>mrpt
<a name="l00037"></a>00037 {
<a name="l00038"></a>00038 <span class="keyword">namespace </span>vision
<a name="l00039"></a>00039 {<span class="comment"></span>
<a name="l00040"></a>00040 <span class="comment"> /** This class represent a pinhole camera model for Monocular SLAM and implements some associated Jacobians</span>
<a name="l00041"></a>00041 <span class="comment"> *</span>
<a name="l00042"></a>00042 <span class="comment"> * The camera parameters are accessible in the public member CCamModel::cam</span>
<a name="l00043"></a>00043 <span class="comment"> *</span>
<a name="l00044"></a>00044 <span class="comment"> * - Versions:</span>
<a name="l00045"></a>00045 <span class="comment"> * - First version: By Antonio J. Ortiz de Galistea.</span>
<a name="l00046"></a>00046 <span class="comment"> * - 2009-2010: Rewritten by various authors.</span>
<a name="l00047"></a>00047 <span class="comment"> *</span>
<a name="l00048"></a>00048 <span class="comment"> * \sa mrpt::utils::TCamera, CMonoSlam, the application <a href="http://www.mrpt.org/Application:camera-calib-gui" >camera-calib-gui</a> for calibrating a camera</span>
<a name="l00049"></a>00049 <span class="comment"> * \ingroup mrpt_vision_grp </span>
<a name="l00050"></a>00050 <span class="comment"> */</span>
<a name="l00051"></a>00051 <span class="keyword">class </span><a class="code" href="vision_2include_2mrpt_2vision_2link__pragmas_8h.html#aa4a17fb846aac61f4e57b17b6f784559">VISION_IMPEXP</a> CCamModel : <span class="keyword">public</span> mrpt::utils::CLoadableOptions
<a name="l00052"></a>00052 {
<a name="l00053"></a>00053 <span class="keyword">public</span>:
<a name="l00054"></a><a class="code" href="classmrpt_1_1vision_1_1_c_cam_model.html#a6816942b827143fc7de3b7dff2cabe50">00054</a> mrpt<a class="code" href="classmrpt_1_1utils_1_1_t_camera.html" title="Structure to hold the parameters of a pinhole camera model.">::utils::TCamera</a> <a class="code" href="classmrpt_1_1vision_1_1_c_cam_model.html#a6816942b827143fc7de3b7dff2cabe50" title="The parameters of a camera.">cam</a>; <span class="comment">//!< The parameters of a camera</span>
<a name="l00055"></a>00055 <span class="comment"></span><span class="comment"></span>
<a name="l00056"></a>00056 <span class="comment"> /** Default Constructor */</span>
<a name="l00057"></a>00057 <a class="code" href="classmrpt_1_1vision_1_1_c_cam_model.html" title="This class represent a pinhole camera model for Monocular SLAM and implements some associated Jacobia...">CCamModel</a>();
<a name="l00058"></a>00058 <span class="comment"></span>
<a name="l00059"></a>00059 <span class="comment"> /** This method loads the options from a ".ini"-like file or memory-stored string list.</span>
<a name="l00060"></a>00060 <span class="comment"> */</span>
<a name="l00061"></a>00061 <span class="keywordtype">void</span> loadFromConfigFile(
<a name="l00062"></a>00062 <span class="keyword">const</span> <a class="code" href="classmrpt_1_1utils_1_1_c_config_file_base.html" title="This class allows loading and storing values and vectors of different types from a configuration text...">mrpt::utils::CConfigFileBase</a> &source,
<a name="l00063"></a>00063 <span class="keyword">const</span> <a class="code" href="classstd_1_1string.html" title="STL class.">std::string</a> &section);
<a name="l00064"></a>00064 <span class="comment"></span>
<a name="l00065"></a>00065 <span class="comment"> /** This method displays clearly all the contents of the structure in textual form, sending it to a CStream. */</span>
<a name="l00066"></a>00066 <span class="keywordtype">void</span> dumpToTextStream( <a class="code" href="classmrpt_1_1utils_1_1_c_stream.html" title="This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...">CStream</a> &out) <span class="keyword">const</span>;
<a name="l00067"></a>00067 <span class="comment"></span>
<a name="l00068"></a>00068 <span class="comment"> /** Constructor from a ini file</span>
<a name="l00069"></a>00069 <span class="comment"> */</span>
<a name="l00070"></a>00070 <a class="code" href="classmrpt_1_1vision_1_1_c_cam_model.html" title="This class represent a pinhole camera model for Monocular SLAM and implements some associated Jacobia...">CCamModel</a>(<span class="keyword">const</span> <a class="code" href="classmrpt_1_1utils_1_1_c_config_file_base.html" title="This class allows loading and storing values and vectors of different types from a configuration text...">mrpt::utils::CConfigFileBase</a> &cfgIni);
<a name="l00071"></a>00071 <span class="comment"></span>
<a name="l00072"></a>00072 <span class="comment"> /** Jacobian for undistortion the image coordinates */</span>
<a name="l00073"></a>00073 <span class="keywordtype">void</span> jacob_undistor_fm(<span class="keyword">const</span> <a class="code" href="structmrpt_1_1utils_1_1_t_pixel_coordf.html" title="A pair (x,y) of pixel coordinates (subpixel resolution).">mrpt::vision::TPixelCoordf</a> &uvd, <a class="code" href="classmrpt_1_1math_1_1_c_matrix_template_numeric.html" title="A matrix of dynamic size.">math::CMatrixDouble</a> &J_undist);
<a name="l00074"></a>00074 <span class="comment"></span>
<a name="l00075"></a>00075 <span class="comment"> /** Calculate the image coordinates undistorted</span>
<a name="l00076"></a>00076 <span class="comment"> */</span>
<a name="l00077"></a>00077 <span class="keywordtype">void</span> jacob_undistor(<span class="keyword">const</span> <a class="code" href="structmrpt_1_1utils_1_1_t_pixel_coordf.html" title="A pair (x,y) of pixel coordinates (subpixel resolution).">mrpt::vision::TPixelCoordf</a> &p, <a class="code" href="classmrpt_1_1math_1_1_c_matrix_template_numeric.html" title="A matrix of dynamic size.">mrpt::math::CMatrixDouble</a> &J_undist );
<a name="l00078"></a>00078 <span class="comment"></span>
<a name="l00079"></a>00079 <span class="comment"> /** Return the pixel position distorted by the camera</span>
<a name="l00080"></a>00080 <span class="comment"> */</span>
<a name="l00081"></a>00081 <span class="keywordtype">void</span> distort_a_point(<span class="keyword">const</span> <a class="code" href="structmrpt_1_1utils_1_1_t_pixel_coordf.html" title="A pair (x,y) of pixel coordinates (subpixel resolution).">mrpt::vision::TPixelCoordf</a> &p, <a class="code" href="structmrpt_1_1utils_1_1_t_pixel_coordf.html" title="A pair (x,y) of pixel coordinates (subpixel resolution).">mrpt::vision::TPixelCoordf</a> &distorted_p);
<a name="l00082"></a>00082 <span class="comment"></span>
<a name="l00083"></a>00083 <span class="comment"> /** Return the pixel position undistorted by the camera</span>
<a name="l00084"></a>00084 <span class="comment"> * The input values 'col' and 'row' will be replace for the new values (undistorted)</span>
<a name="l00085"></a>00085 <span class="comment"> */</span>
<a name="l00086"></a>00086 <span class="keywordtype">void</span> <a class="code" href="group__mrpt__vision__grp.html#gaec6f784e2f55e5476d108ace5182ef5f" title="Undistort one point given by its pixel coordinates and the camera parameters.">undistort_point</a>(<span class="keyword">const</span> <a class="code" href="structmrpt_1_1utils_1_1_t_pixel_coordf.html" title="A pair (x,y) of pixel coordinates (subpixel resolution).">mrpt::vision::TPixelCoordf</a> &p, <a class="code" href="structmrpt_1_1utils_1_1_t_pixel_coordf.html" title="A pair (x,y) of pixel coordinates (subpixel resolution).">mrpt::vision::TPixelCoordf</a> &undistorted_p);
<a name="l00087"></a>00087 <span class="comment"></span>
<a name="l00088"></a>00088 <span class="comment"> /** Return the (distorted) pixel position of a 3D point given in coordinates relative to the camera (+Z pointing forward, +X to the right)</span>
<a name="l00089"></a>00089 <span class="comment"> * \sa unproject_3D_point</span>
<a name="l00090"></a>00090 <span class="comment"> */</span>
<a name="l00091"></a>00091 <span class="keywordtype">void</span> project_3D_point(<span class="keyword">const</span> <a class="code" href="structmrpt_1_1math_1_1_t_point3_d.html" title="Lightweight 3D point.">mrpt::math::TPoint3D</a> &p3D, <a class="code" href="structmrpt_1_1utils_1_1_t_pixel_coordf.html" title="A pair (x,y) of pixel coordinates (subpixel resolution).">mrpt::vision::TPixelCoordf</a> &distorted_p) <span class="keyword">const</span>;
<a name="l00092"></a>00092 <span class="comment"></span>
<a name="l00093"></a>00093 <span class="comment"> /** Return the 3D location of a point (at a fixed distance z=1), for the given (distorted) pixel position</span>
<a name="l00094"></a>00094 <span class="comment"> * \sa project_3D_point</span>
<a name="l00095"></a>00095 <span class="comment"> * \note Of course, there is a depth ambiguity, so the returned 3D point must be considered a direction from the camera focus, or a vector, rather than a meaninful physical point.</span>
<a name="l00096"></a>00096 <span class="comment"> */</span>
<a name="l00097"></a>00097 <span class="keywordtype">void</span> unproject_3D_point(<span class="keyword">const</span> <a class="code" href="structmrpt_1_1utils_1_1_t_pixel_coordf.html" title="A pair (x,y) of pixel coordinates (subpixel resolution).">mrpt::vision::TPixelCoordf</a> &distorted_p, <a class="code" href="structmrpt_1_1math_1_1_t_point3_d.html" title="Lightweight 3D point.">mrpt::math::TPoint3D</a> &p3D) <span class="keyword">const</span>;
<a name="l00098"></a>00098 <span class="comment"></span>
<a name="l00099"></a>00099 <span class="comment"> /** Jacobian of the projection of 3D points (with distortion), as done in project_3D_point \f$ \frac{\partial h}{\partial y} \f$, evaluated at the point p3D (read below the full explanation)</span>
<a name="l00100"></a>00100 <span class="comment"></span>
<a name="l00101"></a>00101 <span class="comment"> We define \f$ h = (h_x ~ h_y) \f$ as the projected point in pixels (origin at the top-left corner),</span>
<a name="l00102"></a>00102 <span class="comment"> and \f$ y=( y_x ~ y_y ~ y_z ) \f$ as the 3D point in space, in coordinates relative to the camera (+Z pointing forwards).</span>
<a name="l00103"></a>00103 <span class="comment"></span>
<a name="l00104"></a>00104 <span class="comment"> Then this method computes the 2x3 Jacobian:</span>
<a name="l00105"></a>00105 <span class="comment"></span>
<a name="l00106"></a>00106 <span class="comment"> \f[</span>
<a name="l00107"></a>00107 <span class="comment"> \frac{\partial h}{\partial y} = \frac{\partial h}{\partial u} \frac{\partial u}{\partial y}</span>
<a name="l00108"></a>00108 <span class="comment"> \f]</span>
<a name="l00109"></a>00109 <span class="comment"></span>
<a name="l00110"></a>00110 <span class="comment"> With:</span>
<a name="l00111"></a>00111 <span class="comment"></span>
<a name="l00112"></a>00112 <span class="comment"> \f[</span>
<a name="l00113"></a>00113 <span class="comment"> \frac{\partial u}{\partial y} =</span>
<a name="l00114"></a>00114 <span class="comment"> \left( \begin{array}{ccc}</span>
<a name="l00115"></a>00115 <span class="comment"> \frac{f_x}{y_z} & 0 & - y \frac{f_x}{y_z^2} \\</span>
<a name="l00116"></a>00116 <span class="comment"> 0 & \frac{f_y}{y_z} & - y \frac{f_y}{y_z^2} \\</span>
<a name="l00117"></a>00117 <span class="comment"> \end{array} \right)</span>
<a name="l00118"></a>00118 <span class="comment"> \f]</span>
<a name="l00119"></a>00119 <span class="comment"></span>
<a name="l00120"></a>00120 <span class="comment"> where \f$ f_x, f_y \f$ is the focal length in units of pixel sizes in x and y, respectively.</span>
<a name="l00121"></a>00121 <span class="comment"> And, if we define:</span>
<a name="l00122"></a>00122 <span class="comment"></span>
<a name="l00123"></a>00123 <span class="comment"> \f[</span>
<a name="l00124"></a>00124 <span class="comment"> f = 1+ 2 k_1 (u_x^2+u_y^2)</span>
<a name="l00125"></a>00125 <span class="comment"> \f]</span>
<a name="l00126"></a>00126 <span class="comment"></span>
<a name="l00127"></a>00127 <span class="comment"> then:</span>
<a name="l00128"></a>00128 <span class="comment"></span>
<a name="l00129"></a>00129 <span class="comment"> \f[</span>
<a name="l00130"></a>00130 <span class="comment"> \frac{\partial h}{\partial u} =</span>
<a name="l00131"></a>00131 <span class="comment"> \left( \begin{array}{cc}</span>
<a name="l00132"></a>00132 <span class="comment"> \frac{ 1+2 k_1 u_y^2 }{f^{3/2}} & -\frac{2 u_x u_y k_1 }{f^{3/2}} \\</span>
<a name="l00133"></a>00133 <span class="comment"> -\frac{2 u_x u_y k_1 }{f^{3/2}} & \frac{ 1+2 k_1 u_x^2 }{f^{3/2}}</span>
<a name="l00134"></a>00134 <span class="comment"> \end{array} \right)</span>
<a name="l00135"></a>00135 <span class="comment"> \f]</span>
<a name="l00136"></a>00136 <span class="comment"></span>
<a name="l00137"></a>00137 <span class="comment"> \note JLBC: Added in March, 2009. Should be equivalent to Davison's WideCamera::ProjectionJacobian</span>
<a name="l00138"></a>00138 <span class="comment"> \sa project_3D_point</span>
<a name="l00139"></a>00139 <span class="comment"> */</span>
<a name="l00140"></a>00140 <span class="keywordtype">void</span> jacobian_project_with_distortion(<span class="keyword">const</span> <a class="code" href="structmrpt_1_1math_1_1_t_point3_d.html" title="Lightweight 3D point.">mrpt::math::TPoint3D</a> &p3D, <a class="code" href="classmrpt_1_1math_1_1_c_matrix_template_numeric.html" title="A matrix of dynamic size.">math::CMatrixDouble</a> & dh_dy ) <span class="keyword">const</span>;
<a name="l00141"></a>00141
<a name="l00142"></a>00142 <span class="comment"></span>
<a name="l00143"></a>00143 <span class="comment"> /** Jacobian of the unprojection of a pixel (with distortion) back into a 3D point, as done in unproject_3D_point \f$ \frac{\partial y}{\partial h} \f$, evaluated at the pixel p</span>
<a name="l00144"></a>00144 <span class="comment"> \note JLBC: Added in March, 2009. Should be equivalent to Davison's WideCamera::UnprojectionJacobian</span>
<a name="l00145"></a>00145 <span class="comment"> \sa unproject_3D_point</span>
<a name="l00146"></a>00146 <span class="comment"> */</span>
<a name="l00147"></a>00147 <span class="keywordtype">void</span> jacobian_unproject_with_distortion(<span class="keyword">const</span> <a class="code" href="structmrpt_1_1utils_1_1_t_pixel_coordf.html" title="A pair (x,y) of pixel coordinates (subpixel resolution).">mrpt::vision::TPixelCoordf</a> &p, <a class="code" href="classmrpt_1_1math_1_1_c_matrix_template_numeric.html" title="A matrix of dynamic size.">math::CMatrixDouble</a> & dy_dh ) <span class="keyword">const</span>;
<a name="l00148"></a>00148
<a name="l00149"></a>00149 <span class="keyword">template</span><<span class="keyword">typename</span> T> <span class="keyword">struct </span><a class="code" href="structmrpt_1_1vision_1_1_c_cam_model_1_1_camera_temp_variables.html">CameraTempVariables</a> {
<a name="l00150"></a><a class="code" href="structmrpt_1_1vision_1_1_c_cam_model_1_1_camera_temp_variables.html#a077e14e40429fa945fc82c580cabb8eb">00150</a> T x_,<a class="code" href="structmrpt_1_1vision_1_1_c_cam_model_1_1_camera_temp_variables.html#a077e14e40429fa945fc82c580cabb8eb">y_</a>;
<a name="l00151"></a><a class="code" href="structmrpt_1_1vision_1_1_c_cam_model_1_1_camera_temp_variables.html#ac4f22383c646046c6199970e85c2057d">00151</a> T x_2,<a class="code" href="structmrpt_1_1vision_1_1_c_cam_model_1_1_camera_temp_variables.html#ac4f22383c646046c6199970e85c2057d">y_2</a>;
<a name="l00152"></a><a class="code" href="structmrpt_1_1vision_1_1_c_cam_model_1_1_camera_temp_variables.html#afb65ce4c7c795cf4db03ef4e7b3b7cf7">00152</a> T <a class="code" href="structmrpt_1_1vision_1_1_c_cam_model_1_1_camera_temp_variables.html#afb65ce4c7c795cf4db03ef4e7b3b7cf7">R</a>;
<a name="l00153"></a><a class="code" href="structmrpt_1_1vision_1_1_c_cam_model_1_1_camera_temp_variables.html#a9bb3825ef0a9eba27298c2e04a289b84">00153</a> T <a class="code" href="structmrpt_1_1vision_1_1_c_cam_model_1_1_camera_temp_variables.html#a9bb3825ef0a9eba27298c2e04a289b84">K</a>;
<a name="l00154"></a><a class="code" href="structmrpt_1_1vision_1_1_c_cam_model_1_1_camera_temp_variables.html#a19c86903484333f9602222573ba077c8">00154</a> T x__,<a class="code" href="structmrpt_1_1vision_1_1_c_cam_model_1_1_camera_temp_variables.html#a19c86903484333f9602222573ba077c8">y__</a>;
<a name="l00155"></a>00155 };
<a name="l00156"></a><a class="code" href="classmrpt_1_1vision_1_1_c_cam_model.html#ab914ab4628377de3c857634a7136b206">00156</a> <span class="keyword">template</span><<span class="keyword">typename</span> T,<span class="keyword">typename</span> POINT> <span class="keywordtype">void</span> <a class="code" href="classmrpt_1_1vision_1_1_c_cam_model.html#ab914ab4628377de3c857634a7136b206">getTemporaryVariablesForTransform</a>(<span class="keyword">const</span> POINT &p,<a class="code" href="structmrpt_1_1vision_1_1_c_cam_model_1_1_camera_temp_variables.html">CameraTempVariables<T></a> &v)<span class="keyword"> const </span>{
<a name="l00157"></a>00157 v.<a class="code" href="structmrpt_1_1vision_1_1_c_cam_model_1_1_camera_temp_variables.html#ab014693073f8c0a72a558ec4e4dbbde8">x_</a>=p[1]/p[0];
<a name="l00158"></a>00158 v.<a class="code" href="structmrpt_1_1vision_1_1_c_cam_model_1_1_camera_temp_variables.html#a077e14e40429fa945fc82c580cabb8eb">y_</a>=p[2]/p[0];
<a name="l00159"></a>00159 v.<a class="code" href="structmrpt_1_1vision_1_1_c_cam_model_1_1_camera_temp_variables.html#a9eacc0d556a0184294f3b11ecd9afd74">x_2</a>=<a class="code" href="namespacemrpt_1_1utils.html#a67cb05bb8ad4e725875a7ee54b7042ae" title="Inline function for the square of a number.">square</a>(v.<a class="code" href="structmrpt_1_1vision_1_1_c_cam_model_1_1_camera_temp_variables.html#ab014693073f8c0a72a558ec4e4dbbde8">x_</a>);
<a name="l00160"></a>00160 v.<a class="code" href="structmrpt_1_1vision_1_1_c_cam_model_1_1_camera_temp_variables.html#ac4f22383c646046c6199970e85c2057d">y_2</a>=<a class="code" href="namespacemrpt_1_1utils.html#a67cb05bb8ad4e725875a7ee54b7042ae" title="Inline function for the square of a number.">square</a>(v.<a class="code" href="structmrpt_1_1vision_1_1_c_cam_model_1_1_camera_temp_variables.html#a077e14e40429fa945fc82c580cabb8eb">y_</a>);
<a name="l00161"></a>00161 v.<a class="code" href="structmrpt_1_1vision_1_1_c_cam_model_1_1_camera_temp_variables.html#afb65ce4c7c795cf4db03ef4e7b3b7cf7">R</a>=v.<a class="code" href="structmrpt_1_1vision_1_1_c_cam_model_1_1_camera_temp_variables.html#a9eacc0d556a0184294f3b11ecd9afd74">x_2</a>+v.<a class="code" href="structmrpt_1_1vision_1_1_c_cam_model_1_1_camera_temp_variables.html#ac4f22383c646046c6199970e85c2057d">y_2</a>;
<a name="l00162"></a>00162 v.<a class="code" href="structmrpt_1_1vision_1_1_c_cam_model_1_1_camera_temp_variables.html#a9bb3825ef0a9eba27298c2e04a289b84">K</a>=1+v.<a class="code" href="structmrpt_1_1vision_1_1_c_cam_model_1_1_camera_temp_variables.html#afb65ce4c7c795cf4db03ef4e7b3b7cf7">R</a>*(cam.k1()+v.<a class="code" href="structmrpt_1_1vision_1_1_c_cam_model_1_1_camera_temp_variables.html#afb65ce4c7c795cf4db03ef4e7b3b7cf7">R</a>*(cam.k2()+v.<a class="code" href="structmrpt_1_1vision_1_1_c_cam_model_1_1_camera_temp_variables.html#afb65ce4c7c795cf4db03ef4e7b3b7cf7">R</a>*cam.k3()));
<a name="l00163"></a>00163 T xy=v.<a class="code" href="structmrpt_1_1vision_1_1_c_cam_model_1_1_camera_temp_variables.html#ab014693073f8c0a72a558ec4e4dbbde8">x_</a>*v.<a class="code" href="structmrpt_1_1vision_1_1_c_cam_model_1_1_camera_temp_variables.html#a077e14e40429fa945fc82c580cabb8eb">y_</a>,p1=cam.p1(),p2=cam.p2();
<a name="l00164"></a>00164 v.<a class="code" href="structmrpt_1_1vision_1_1_c_cam_model_1_1_camera_temp_variables.html#a8280ea7a589f98b8b640df6377babab2">x__</a>=v.<a class="code" href="structmrpt_1_1vision_1_1_c_cam_model_1_1_camera_temp_variables.html#ab014693073f8c0a72a558ec4e4dbbde8">x_</a>*v.<a class="code" href="structmrpt_1_1vision_1_1_c_cam_model_1_1_camera_temp_variables.html#a9bb3825ef0a9eba27298c2e04a289b84">K</a>+2*p1*xy+p2*(3*v.<a class="code" href="structmrpt_1_1vision_1_1_c_cam_model_1_1_camera_temp_variables.html#a9eacc0d556a0184294f3b11ecd9afd74">x_2</a>+v.<a class="code" href="structmrpt_1_1vision_1_1_c_cam_model_1_1_camera_temp_variables.html#ac4f22383c646046c6199970e85c2057d">y_2</a>);
<a name="l00165"></a>00165 v.<a class="code" href="structmrpt_1_1vision_1_1_c_cam_model_1_1_camera_temp_variables.html#a19c86903484333f9602222573ba077c8">y__</a>=v.<a class="code" href="structmrpt_1_1vision_1_1_c_cam_model_1_1_camera_temp_variables.html#a077e14e40429fa945fc82c580cabb8eb">y_</a>*v.<a class="code" href="structmrpt_1_1vision_1_1_c_cam_model_1_1_camera_temp_variables.html#a9bb3825ef0a9eba27298c2e04a289b84">K</a>+p1*(v.<a class="code" href="structmrpt_1_1vision_1_1_c_cam_model_1_1_camera_temp_variables.html#a9eacc0d556a0184294f3b11ecd9afd74">x_2</a>+3*v.<a class="code" href="structmrpt_1_1vision_1_1_c_cam_model_1_1_camera_temp_variables.html#ac4f22383c646046c6199970e85c2057d">y_2</a>)+2*p2*xy;
<a name="l00166"></a>00166 }
<a name="l00167"></a>00167
<a name="l00168"></a><a class="code" href="classmrpt_1_1vision_1_1_c_cam_model.html#a07aa2cb171327074ad3cb76582a73754">00168</a> <span class="keyword">template</span><<span class="keyword">typename</span> T,<span class="keyword">typename</span> POINT,<span class="keyword">typename</span> PIXEL> <span class="keyword">inline</span> <span class="keywordtype">void</span> <a class="code" href="classmrpt_1_1vision_1_1_c_cam_model.html#a07aa2cb171327074ad3cb76582a73754">getFullProjection</a>(<span class="keyword">const</span> POINT &pIn,PIXEL &pOut)<span class="keyword"> const </span>{
<a name="l00169"></a>00169 <a class="code" href="structmrpt_1_1vision_1_1_c_cam_model_1_1_camera_temp_variables.html">CameraTempVariables<T></a> tmp;
<a name="l00170"></a>00170 getTemporaryVariablesForTransform(pIn,tmp);
<a name="l00171"></a>00171 getFullProjectionT(tmp,pOut);
<a name="l00172"></a>00172 }
<a name="l00173"></a>00173
<a name="l00174"></a><a class="code" href="classmrpt_1_1vision_1_1_c_cam_model.html#aa441286f9758c0a034d23c7078a4a777">00174</a> <span class="keyword">template</span><<span class="keyword">typename</span> T,<span class="keyword">typename</span> PIXEL> <span class="keyword">inline</span> <span class="keywordtype">void</span> <a class="code" href="classmrpt_1_1vision_1_1_c_cam_model.html#aa441286f9758c0a034d23c7078a4a777">getFullProjectionT</a>(<span class="keyword">const</span> <a class="code" href="structmrpt_1_1vision_1_1_c_cam_model_1_1_camera_temp_variables.html">CameraTempVariables<T></a> &tmp,PIXEL &pOut)<span class="keyword"> const </span>{
<a name="l00175"></a>00175 pOut[0]=cam.fx()*tmp.<a class="code" href="structmrpt_1_1vision_1_1_c_cam_model_1_1_camera_temp_variables.html#a8280ea7a589f98b8b640df6377babab2">x__</a>+cam.cx();
<a name="l00176"></a>00176 pOut[1]=cam.fy()*tmp.<a class="code" href="structmrpt_1_1vision_1_1_c_cam_model_1_1_camera_temp_variables.html#a19c86903484333f9602222573ba077c8">y__</a>+cam.cy();
<a name="l00177"></a>00177 }
<a name="l00178"></a>00178
<a name="l00179"></a><a class="code" href="classmrpt_1_1vision_1_1_c_cam_model.html#a1bc967a04b265249c56070b178ea9b15">00179</a> <span class="keyword">template</span><<span class="keyword">typename</span> T,<span class="keyword">typename</span> POINT,<span class="keyword">typename</span> MATRIX> <span class="keyword">inline</span> <span class="keywordtype">void</span> <a class="code" href="classmrpt_1_1vision_1_1_c_cam_model.html#a1bc967a04b265249c56070b178ea9b15">getFullJacobian</a>(<span class="keyword">const</span> POINT &pIn,MATRIX &mOut)<span class="keyword"> const </span>{
<a name="l00180"></a>00180 <a class="code" href="structmrpt_1_1vision_1_1_c_cam_model_1_1_camera_temp_variables.html">CameraTempVariables<T></a> tmp;
<a name="l00181"></a>00181 getTemporaryVariablesForTransform(pIn,tmp);
<a name="l00182"></a>00182 getFullJacobianT(pIn,tmp,mOut);
<a name="l00183"></a>00183 }
<a name="l00184"></a>00184
<a name="l00185"></a><a class="code" href="classmrpt_1_1vision_1_1_c_cam_model.html#a842faddd778d61f88c1f6f8bf684e88e">00185</a> <span class="keyword">template</span><<span class="keyword">typename</span> T,<span class="keyword">typename</span> POINT,<span class="keyword">typename</span> MATRIX> <span class="keywordtype">void</span> <a class="code" href="classmrpt_1_1vision_1_1_c_cam_model.html#a842faddd778d61f88c1f6f8bf684e88e">getFullJacobianT</a>(<span class="keyword">const</span> POINT &pIn,<span class="keyword">const</span> <a class="code" href="structmrpt_1_1vision_1_1_c_cam_model_1_1_camera_temp_variables.html">CameraTempVariables<T></a> &tmp,MATRIX &mOut)<span class="keyword"> const </span>{
<a name="l00186"></a>00186 T x_=1/pIn[0];
<a name="l00187"></a>00187 T x_2=<a class="code" href="namespacemrpt_1_1utils.html#a67cb05bb8ad4e725875a7ee54b7042ae" title="Inline function for the square of a number.">square</a>(x_);
<a name="l00188"></a>00188 <span class="comment">//First two jacobians...</span>
<a name="l00189"></a>00189 <a class="code" href="classmrpt_1_1math_1_1_c_matrix_fixed_numeric.html" title="A numeric matrix of compile-time fixed size.">CMatrixFixedNumeric<T,3,3></a> J21;
<a name="l00190"></a>00190 T tmpK=2*(cam.k1()+tmp.<a class="code" href="structmrpt_1_1vision_1_1_c_cam_model_1_1_camera_temp_variables.html#afb65ce4c7c795cf4db03ef4e7b3b7cf7">R</a>*(2*cam.k2()+3*tmp.<a class="code" href="structmrpt_1_1vision_1_1_c_cam_model_1_1_camera_temp_variables.html#afb65ce4c7c795cf4db03ef4e7b3b7cf7">R</a>*cam.k3()));
<a name="l00191"></a>00191 T tmpKx=tmpK*tmp.<a class="code" href="structmrpt_1_1vision_1_1_c_cam_model_1_1_camera_temp_variables.html#ab014693073f8c0a72a558ec4e4dbbde8">x_</a>;
<a name="l00192"></a>00192 T tmpKy=tmpK*tmp.<a class="code" href="structmrpt_1_1vision_1_1_c_cam_model_1_1_camera_temp_variables.html#a077e14e40429fa945fc82c580cabb8eb">y_</a>;
<a name="l00193"></a>00193 T yx2=-pIn[1]*x_2;
<a name="l00194"></a>00194 T zx2=-pIn[2]*x_2;
<a name="l00195"></a>00195 J21.set_unsafe(0,0,yx2);
<a name="l00196"></a>00196 J21.set_unsafe(0,1,x_);
<a name="l00197"></a>00197 J21.set_unsafe(0,2,0);
<a name="l00198"></a>00198 J21.set_unsafe(1,0,zx2);
<a name="l00199"></a>00199 J21.set_unsafe(1,1,0);
<a name="l00200"></a>00200 J21.set_unsafe(1,2,x_);
<a name="l00201"></a>00201 J21.set_unsafe(2,0,tmpKx*yx2+tmpKy*zx2);
<a name="l00202"></a>00202 J21.set_unsafe(2,1,tmpKx*x_);
<a name="l00203"></a>00203 J21.set_unsafe(2,2,tmpKy*x_);
<a name="l00204"></a>00204 <span class="comment">//Last two jacobians...</span>
<a name="l00205"></a>00205 T pxpy=2*(cam.p1()*tmp.<a class="code" href="structmrpt_1_1vision_1_1_c_cam_model_1_1_camera_temp_variables.html#ab014693073f8c0a72a558ec4e4dbbde8">x_</a>+cam.p2()*tmp.<a class="code" href="structmrpt_1_1vision_1_1_c_cam_model_1_1_camera_temp_variables.html#a077e14e40429fa945fc82c580cabb8eb">y_</a>);
<a name="l00206"></a>00206 T p1y=cam.p1()*tmp.<a class="code" href="structmrpt_1_1vision_1_1_c_cam_model_1_1_camera_temp_variables.html#a077e14e40429fa945fc82c580cabb8eb">y_</a>;
<a name="l00207"></a>00207 T p2x=cam.p2()*tmp.<a class="code" href="structmrpt_1_1vision_1_1_c_cam_model_1_1_camera_temp_variables.html#ab014693073f8c0a72a558ec4e4dbbde8">x_</a>;
<a name="l00208"></a>00208 <a class="code" href="classmrpt_1_1math_1_1_c_matrix_fixed_numeric.html" title="A numeric matrix of compile-time fixed size.">CMatrixFixedNumeric<T,2,3></a> J43;
<a name="l00209"></a>00209 T fx=cam.fx(),fy=cam.fy();
<a name="l00210"></a>00210 J43.set_unsafe(0,0,fx*(tmp.<a class="code" href="structmrpt_1_1vision_1_1_c_cam_model_1_1_camera_temp_variables.html#a9bb3825ef0a9eba27298c2e04a289b84">K</a>+2*p1y+6*p2x));
<a name="l00211"></a>00211 J43.set_unsafe(0,1,fx*pxpy);
<a name="l00212"></a>00212 J43.set_unsafe(0,2,fx*tmp.<a class="code" href="structmrpt_1_1vision_1_1_c_cam_model_1_1_camera_temp_variables.html#ab014693073f8c0a72a558ec4e4dbbde8">x_</a>);
<a name="l00213"></a>00213 J43.set_unsafe(1,0,fy*pxpy);
<a name="l00214"></a>00214 J43.set_unsafe(1,1,fy*(tmp.<a class="code" href="structmrpt_1_1vision_1_1_c_cam_model_1_1_camera_temp_variables.html#a9bb3825ef0a9eba27298c2e04a289b84">K</a>+6*p1y+2*p2x));
<a name="l00215"></a>00215 J43.set_unsafe(1,2,fy*tmp.<a class="code" href="structmrpt_1_1vision_1_1_c_cam_model_1_1_camera_temp_variables.html#a077e14e40429fa945fc82c580cabb8eb">y_</a>);
<a name="l00216"></a>00216 mOut.multiply(J43,J21);
<a name="l00217"></a>00217 <span class="comment">//cout<<"J21:\n"<<J21<<"\nJ43:\n"<<J43<<"\nmOut:\n"<<mOut;</span>
<a name="l00218"></a>00218 }
<a name="l00219"></a>00219 <span class="keyword">private</span>:
<a name="l00220"></a>00220 <span class="comment">//These functions are little tricks to avoid multiple initialization.</span>
<a name="l00221"></a>00221 <span class="comment">//They are intended to initialize the common parts of the jacobians just once,</span>
<a name="l00222"></a>00222 <span class="comment">//and not in each iteration.</span>
<a name="l00223"></a>00223 <span class="comment">//They are mostly useless outside the scope of this function.</span>
<a name="l00224"></a><a class="code" href="classmrpt_1_1vision_1_1_c_cam_model.html#acc285978efc02edd77a5571f50cf21cb">00224</a> <a class="code" href="classmrpt_1_1math_1_1_c_matrix_fixed_numeric.html">CMatrixFixedNumeric<double,2,2></a> <a class="code" href="classmrpt_1_1vision_1_1_c_cam_model.html#acc285978efc02edd77a5571f50cf21cb">firstInverseJacobian</a>()<span class="keyword"> const </span>{
<a name="l00225"></a>00225 <a class="code" href="classmrpt_1_1math_1_1_c_matrix_fixed_numeric.html">CMatrixFixedNumeric<double,2,2></a> res;
<a name="l00226"></a>00226 res.set_unsafe(0,1,0);
<a name="l00227"></a>00227 res.set_unsafe(1,0,0);
<a name="l00228"></a>00228 <span class="keywordflow">return</span> res;
<a name="l00229"></a>00229 }
<a name="l00230"></a><a class="code" href="classmrpt_1_1vision_1_1_c_cam_model.html#aca310dda5e61a8a8a46bcf4ff32d8dda">00230</a> <a class="code" href="classmrpt_1_1math_1_1_c_matrix_fixed_numeric.html" title="A numeric matrix of compile-time fixed size.">CMatrixFixedNumeric<double,4,2></a> <a class="code" href="classmrpt_1_1vision_1_1_c_cam_model.html#aca310dda5e61a8a8a46bcf4ff32d8dda">secondInverseJacobian</a>()<span class="keyword"> const </span>{
<a name="l00231"></a>00231 <a class="code" href="classmrpt_1_1math_1_1_c_matrix_fixed_numeric.html" title="A numeric matrix of compile-time fixed size.">CMatrixFixedNumeric<double,4,2></a> res;
<a name="l00232"></a>00232 res.set_unsafe(0,0,1);
<a name="l00233"></a>00233 res.set_unsafe(0,1,0);
<a name="l00234"></a>00234 res.set_unsafe(1,0,0);
<a name="l00235"></a>00235 res.set_unsafe(1,1,1);
<a name="l00236"></a>00236 <span class="keywordflow">return</span> res;
<a name="l00237"></a>00237 }
<a name="l00238"></a><a class="code" href="classmrpt_1_1vision_1_1_c_cam_model.html#a3d472fbd80a0dff3c8a2f1e6052dc98c">00238</a> <a class="code" href="classmrpt_1_1math_1_1_c_matrix_fixed_numeric.html" title="A numeric matrix of compile-time fixed size.">CMatrixFixedNumeric<double,3,4></a> <a class="code" href="classmrpt_1_1vision_1_1_c_cam_model.html#a3d472fbd80a0dff3c8a2f1e6052dc98c">thirdInverseJacobian</a>()<span class="keyword"> const </span>{
<a name="l00239"></a>00239 <a class="code" href="classmrpt_1_1math_1_1_c_matrix_fixed_numeric.html" title="A numeric matrix of compile-time fixed size.">CMatrixFixedNumeric<double,3,4></a> res;
<a name="l00240"></a>00240 res.set_unsafe(0,1,0);
<a name="l00241"></a>00241 res.set_unsafe(0,2,0);
<a name="l00242"></a>00242 res.set_unsafe(1,0,0);
<a name="l00243"></a>00243 res.set_unsafe(1,2,0);
<a name="l00244"></a>00244 res.set_unsafe(2,0,0);
<a name="l00245"></a>00245 res.set_unsafe(2,1,0);
<a name="l00246"></a>00246 res.set_unsafe(2,2,1);
<a name="l00247"></a>00247 res.set_unsafe(2,3,0);
<a name="l00248"></a>00248 <span class="keywordflow">return</span> res;
<a name="l00249"></a>00249 }
<a name="l00250"></a>00250 <span class="keyword">public</span>:
<a name="l00251"></a><a class="code" href="classmrpt_1_1vision_1_1_c_cam_model.html#aa7689e25c2dc657a50d73a7fad413d0d">00251</a> <span class="keyword">template</span><<span class="keyword">typename</span> POINTIN,<span class="keyword">typename</span> POINTOUT,<span class="keyword">typename</span> MAT22> <span class="keywordtype">void</span> <a class="code" href="classmrpt_1_1vision_1_1_c_cam_model.html#aa7689e25c2dc657a50d73a7fad413d0d">getFullInverseModelWithJacobian</a>(<span class="keyword">const</span> POINTIN &pIn,POINTOUT &pOut,MAT22 &jOut)<span class="keyword"> const </span>{
<a name="l00252"></a>00252 <span class="comment">//Temporary variables (well, there are some more, but these are the basics)</span>
<a name="l00253"></a>00253 <span class="comment">//WARNING!: this shortcut to avoid repeated initialization makes the method somewhat</span>
<a name="l00254"></a>00254 <span class="comment">//faster, but makes it incapable of being used in more than one thread</span>
<a name="l00255"></a>00255 <span class="comment">//simultaneously!</span>
<a name="l00256"></a>00256 <span class="keyword">static</span> <a class="code" href="classmrpt_1_1math_1_1_c_matrix_fixed_numeric.html">CMatrixFixedNumeric<double,2,2></a> J1(firstInverseJacobian());
<a name="l00257"></a>00257 <span class="keyword">static</span> <a class="code" href="classmrpt_1_1math_1_1_c_matrix_fixed_numeric.html" title="A numeric matrix of compile-time fixed size.">CMatrixFixedNumeric<double,4,2></a> J2(secondInverseJacobian());
<a name="l00258"></a>00258 <span class="keyword">static</span> <a class="code" href="classmrpt_1_1math_1_1_c_matrix_fixed_numeric.html" title="A numeric matrix of compile-time fixed size.">CMatrixFixedNumeric<double,3,4></a> J3(thirdInverseJacobian());
<a name="l00259"></a>00259 <span class="keyword">static</span> <a class="code" href="classmrpt_1_1math_1_1_c_matrix_fixed_numeric.html" title="A numeric matrix of compile-time fixed size.">CMatrixFixedNumeric<double,2,3></a> J4; <span class="comment">//This is not initialized in a special way, although declaring it</span>
<a name="l00260"></a>00260 <a class="code" href="classmrpt_1_1math_1_1_c_array.html" title="A STL container (as wrapper) for arrays of constant size defined at compile time - Users will most li...">CArray<double,4></a> tmp1;
<a name="l00261"></a>00261 <a class="code" href="classmrpt_1_1math_1_1_c_array.html" title="A STL container (as wrapper) for arrays of constant size defined at compile time - Users will most li...">CArray<double,2></a> tmp2; <span class="comment">//This would be a CArray<double,3>, but to avoid copying, we let "R2" lie in tmp1.</span>
<a name="l00262"></a>00262 <span class="comment">//Camera Parameters</span>
<a name="l00263"></a>00263 <span class="keywordtype">double</span> cx=cam.cx(),cy=cam.cy(),ifx=1/cam.fx(),ify=1/cam.fy();
<a name="l00264"></a>00264 <span class="keywordtype">double</span> K1=cam.k1(),K2=cam.k2(),p1=cam.p1(),p2=cam.p2(),K3=cam.k3();
<a name="l00265"></a>00265 <span class="comment">//First step: intrinsic matrix.</span>
<a name="l00266"></a>00266 tmp1[0]=(pIn[0]-cx)*ifx;
<a name="l00267"></a>00267 tmp1[1]=(pIn[1]-cy)*ify;
<a name="l00268"></a>00268 J1.set_unsafe(0,0,ifx);
<a name="l00269"></a>00269 J1.set_unsafe(1,1,ify);
<a name="l00270"></a>00270 <span class="comment">//Second step: adding temporary variables, related to the distortion.</span>
<a name="l00271"></a>00271 tmp1[2]=<a class="code" href="namespacemrpt_1_1utils.html#a67cb05bb8ad4e725875a7ee54b7042ae" title="Inline function for the square of a number.">square</a>(tmp1[0])+<a class="code" href="namespacemrpt_1_1utils.html#a67cb05bb8ad4e725875a7ee54b7042ae" title="Inline function for the square of a number.">square</a>(tmp1[1]);
<a name="l00272"></a>00272 <span class="keywordtype">double</span> sK1=<a class="code" href="namespacemrpt_1_1utils.html#a67cb05bb8ad4e725875a7ee54b7042ae" title="Inline function for the square of a number.">square</a>(K1);
<a name="l00273"></a>00273 <span class="keywordtype">double</span> K12=sK1-K2;
<a name="l00274"></a>00274 <span class="keywordtype">double</span> K123=-K1*sK1+2*K1*K2-K3; <span class="comment">//-K1³+2K1K2-K3</span>
<a name="l00275"></a>00275 <span class="comment">//tmp1[3]=1-K1*tmp1[2]+K12*square(tmp1[2]);</span>
<a name="l00276"></a>00276 tmp1[3]=1+tmp1[2]*(-K1+tmp1[2]*(K12+tmp1[2]*K123));
<a name="l00277"></a>00277 J2.set_unsafe(2,0,2*tmp1[0]);
<a name="l00278"></a>00278 J2.set_unsafe(2,1,2*tmp1[1]);
<a name="l00279"></a>00279 <span class="keywordtype">double</span> jTemp=-2*K1+4*tmp1[2]*K12+6*<a class="code" href="namespacemrpt_1_1utils.html#a67cb05bb8ad4e725875a7ee54b7042ae" title="Inline function for the square of a number.">square</a>(tmp1[2])*K123;
<a name="l00280"></a>00280 J2.set_unsafe(3,0,tmp1[0]*jTemp);
<a name="l00281"></a>00281 J2.set_unsafe(3,1,tmp1[1]*jTemp);
<a name="l00282"></a>00282 <span class="comment">//Third step: radial distortion. Really simple, since most work has been done in the previous step.</span>
<a name="l00283"></a>00283 tmp2[0]=tmp1[0]*tmp1[3];
<a name="l00284"></a>00284 tmp2[1]=tmp1[1]*tmp1[3];
<a name="l00285"></a>00285 J3.set_unsafe(0,0,tmp1[3]);
<a name="l00286"></a>00286 J3.set_unsafe(0,3,tmp1[0]);
<a name="l00287"></a>00287 J3.set_unsafe(1,1,tmp1[3]);
<a name="l00288"></a>00288 J3.set_unsafe(1,3,tmp1[1]);
<a name="l00289"></a>00289 <span class="comment">//Fourth step: tangential distorion. A little more complicated, but not much more.</span>
<a name="l00290"></a>00290 <span class="keywordtype">double</span> prod=tmp2[0]*tmp2[1];
<a name="l00291"></a>00291 <span class="comment">//References to tmp1[2] are not errors! That element is "R2".</span>
<a name="l00292"></a>00292 pOut[0]=tmp2[0]-p1*prod-p2*(tmp1[2]+2*<a class="code" href="namespacemrpt_1_1utils.html#a67cb05bb8ad4e725875a7ee54b7042ae" title="Inline function for the square of a number.">square</a>(tmp2[0]));
<a name="l00293"></a>00293 pOut[1]=tmp2[1]-p1*(tmp1[2]+2*<a class="code" href="namespacemrpt_1_1utils.html#a67cb05bb8ad4e725875a7ee54b7042ae" title="Inline function for the square of a number.">square</a>(tmp2[1]))-p2*prod;
<a name="l00294"></a>00294 J4.set_unsafe(0,0,1-p1*tmp2[1]-4*p2*tmp2[0]);
<a name="l00295"></a>00295 J4.set_unsafe(0,1,-p1*tmp2[0]);
<a name="l00296"></a>00296 J4.set_unsafe(0,2,-p2);
<a name="l00297"></a>00297 J4.set_unsafe(1,0,-p2*tmp2[1]);
<a name="l00298"></a>00298 J4.set_unsafe(1,1,1-4*p1*tmp2[1]-p2*tmp2[0]);
<a name="l00299"></a>00299 J4.set_unsafe(1,2,-p1);
<a name="l00300"></a>00300 <span class="comment">//As fast as possible, and without more temporaries, let the jacobian be J4*J3*J2*J1;</span>
<a name="l00301"></a>00301 jOut.multiply_ABC(J4,J3,J2); <span class="comment">//Note that using the other order is not possible due to matrix sizes (jOut may, and most probably will, be fixed).</span>
<a name="l00302"></a>00302 jOut.multiply(jOut,J1);
<a name="l00303"></a>00303 }
<a name="l00304"></a>00304
<a name="l00305"></a>00305 }; <span class="comment">// end class</span>
<a name="l00306"></a>00306
<a name="l00307"></a>00307 } <span class="comment">// end namespace</span>
<a name="l00308"></a>00308 } <span class="comment">// end namespace</span>
<a name="l00309"></a>00309 <span class="preprocessor">#endif //__CCamModel_H</span>
</pre></div></div>
</div>
<br><hr><br> <table border="0" width="100%"> <tr> <td> Page generated by <a href="http://www.doxygen.org" target="_blank">Doxygen 1.7.5</a> for MRPT 0.9.5 SVN: at Sun Sep 25 17:20:18 UTC 2011</td><td></td> <td width="100"> </td> <td width="150"> </td></tr> </table> </body></html>
