<!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>KDTreeCapable.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">KDTreeCapable.h</div> </div>
</div>
<div class="contents">
<a href="_k_d_tree_capable_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 KDTreeCapable_H</span>
<a name="l00029"></a>00029 <span class="preprocessor"></span><span class="preprocessor">#define KDTreeCapable_H</span>
<a name="l00030"></a>00030 <span class="preprocessor"></span>
<a name="l00031"></a>00031 <span class="preprocessor">#include <<a class="code" href="utils__defs_8h.html">mrpt/utils/utils_defs.h</a>></span>
<a name="l00032"></a>00032
<a name="l00033"></a>00033 <span class="comment">// nanoflann library:</span>
<a name="l00034"></a>00034 <span class="preprocessor">#include <<a class="code" href="nanoflann_8hpp.html">mrpt/otherlibs/nanoflann/nanoflann.hpp</a>></span>
<a name="l00035"></a>00035 <span class="preprocessor">#include <<a class="code" href="lightweight__geom__data_8h.html">mrpt/math/lightweight_geom_data.h</a>></span>
<a name="l00036"></a>00036
<a name="l00037"></a>00037 <span class="keyword">namespace </span>mrpt
<a name="l00038"></a>00038 {
<a name="l00039"></a>00039 <span class="keyword">namespace </span>math
<a name="l00040"></a>00040 {<span class="comment"></span>
<a name="l00041"></a>00041 <span class="comment"> /** \addtogroup kdtree_grp KD-Trees</span>
<a name="l00042"></a>00042 <span class="comment"> * \ingroup mrpt_base_grp</span>
<a name="l00043"></a>00043 <span class="comment"> * @{ */</span>
<a name="l00044"></a>00044
<a name="l00045"></a>00045 <span class="comment"></span>
<a name="l00046"></a>00046 <span class="comment"> /** A generic adaptor class for providing Approximate Nearest Neighbors (ANN) (via the nanoflann library) to MRPT classses.</span>
<a name="l00047"></a>00047 <span class="comment"> * This makes use of the CRTP design pattern.</span>
<a name="l00048"></a>00048 <span class="comment"> *</span>
<a name="l00049"></a>00049 <span class="comment"> * Derived classes must be aware of the need to call "kdtree_mark_as_outdated()" when the data points</span>
<a name="l00050"></a>00050 <span class="comment"> * change to mark the cached KD-tree (an "index") as invalid, and also implement the following interface</span>
<a name="l00051"></a>00051 <span class="comment"> * (note that these are not virtual functions due to the usage of CRTP):</span>
<a name="l00052"></a>00052 <span class="comment"> *</span>
<a name="l00053"></a>00053 <span class="comment"> * \code</span>
<a name="l00054"></a>00054 <span class="comment"> * // Must return the number of data points</span>
<a name="l00055"></a>00055 <span class="comment"> * inline size_t kdtree_get_point_count() const { ... }</span>
<a name="l00056"></a>00056 <span class="comment"> *</span>
<a name="l00057"></a>00057 <span class="comment"> * // Returns the distance between the vector "p1[0:size-1]" and the data point with index "idx_p2" stored in the class:</span>
<a name="l00058"></a>00058 <span class="comment"> * inline float kdtree_distance(const float *p1, const size_t idx_p2,size_t size) const { ... }</span>
<a name="l00059"></a>00059 <span class="comment"> *</span>
<a name="l00060"></a>00060 <span class="comment"> * // Returns the dim'th component of the idx'th point in the class:</span>
<a name="l00061"></a>00061 <span class="comment"> * inline num_t kdtree_get_pt(const size_t idx, int dim) const { ... }</span>
<a name="l00062"></a>00062 <span class="comment"> *</span>
<a name="l00063"></a>00063 <span class="comment"> * // Optional bounding-box computation: return false to default to a standard bbox computation loop.</span>
<a name="l00064"></a>00064 <span class="comment"> * // Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it again.</span>
<a name="l00065"></a>00065 <span class="comment"> * // Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds)</span>
<a name="l00066"></a>00066 <span class="comment"> * template <class BBOX></span>
<a name="l00067"></a>00067 <span class="comment"> * bool kdtree_get_bbox(BBOX &bb) const</span>
<a name="l00068"></a>00068 <span class="comment"> * {</span>
<a name="l00069"></a>00069 <span class="comment"> * bb[0].low = ...; bb[0].high = ...; // "x" limits</span>
<a name="l00070"></a>00070 <span class="comment"> * return true;</span>
<a name="l00071"></a>00071 <span class="comment"> * }</span>
<a name="l00072"></a>00072 <span class="comment"> *</span>
<a name="l00073"></a>00073 <span class="comment"> * \endcode</span>
<a name="l00074"></a>00074 <span class="comment"> *</span>
<a name="l00075"></a>00075 <span class="comment"> * The KD-tree index will be built on demand only upon call of any of the query methods provided by this class.</span>
<a name="l00076"></a>00076 <span class="comment"> *</span>
<a name="l00077"></a>00077 <span class="comment"> * Notice that there is only ONE internal cached KD-tree, so if a method to query a 2D point is called,</span>
<a name="l00078"></a>00078 <span class="comment"> * then another method for 3D points, then again the 2D method, three KD-trees will be built. So, try</span>
<a name="l00079"></a>00079 <span class="comment"> * to group all the calls for a given dimensionality together or build different class instances for</span>
<a name="l00080"></a>00080 <span class="comment"> * queries of each dimensionality, etc.</span>
<a name="l00081"></a>00081 <span class="comment"> *</span>
<a name="l00082"></a>00082 <span class="comment"> * \sa See some of the derived classes for example implementations. See also the documentation of nanoflann</span>
<a name="l00083"></a>00083 <span class="comment"> * \ingroup mrpt_base_grp</span>
<a name="l00084"></a>00084 <span class="comment"> */</span>
<a name="l00085"></a>00085 <span class="keyword">template</span> <<span class="keyword">class</span> Derived, <span class="keyword">typename</span> num_t = <span class="keywordtype">float</span>, <span class="keyword">typename</span> metric_t = nanoflann::L2_Simple_Adaptor<num_t,Derived> >
<a name="l00086"></a>00086 <span class="keyword">class </span>KDTreeCapable
<a name="l00087"></a>00087 {
<a name="l00088"></a>00088 <span class="keyword">public</span>:
<a name="l00089"></a>00089 <span class="comment">// Types ---------------</span>
<a name="l00090"></a><a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#ae848e2006de4cda4f4a50d0c057d086c">00090</a> <span class="keyword">typedef</span> <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html" title="A generic adaptor class for providing Approximate Nearest Neighbors (ANN) (via the nanoflann library)...">KDTreeCapable<Derived,num_t,metric_t></a> <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#ae848e2006de4cda4f4a50d0c057d086c">self_t</a>;
<a name="l00091"></a>00091 <span class="comment">// ---------------------</span>
<a name="l00092"></a>00092 <span class="comment"></span>
<a name="l00093"></a>00093 <span class="comment"> /// Constructor</span>
<a name="l00094"></a><a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a64b7fa73845473d69b4677109826f167">00094</a> <span class="comment"></span> <span class="keyword">inline</span> <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a64b7fa73845473d69b4677109826f167" title="Constructor.">KDTreeCapable</a>() : <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#af3dfc28c75edd8ba3e8ea4b4caced0aa" title="whether the KD tree needs to be rebuilt or not.">m_kdtree_is_uptodate</a>(false) { }
<a name="l00095"></a>00095 <span class="comment"></span>
<a name="l00096"></a>00096 <span class="comment"> /// Destructor (nothing needed to do here)</span>
<a name="l00097"></a><a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#ad811570de62b2a46232e85050b483b42">00097</a> <span class="comment"></span> <span class="keyword">inline</span> <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#ad811570de62b2a46232e85050b483b42" title="Destructor (nothing needed to do here)">~KDTreeCapable</a>() { }
<a name="l00098"></a>00098 <span class="comment"></span>
<a name="l00099"></a>00099 <span class="comment"> /// CRTP helper method</span>
<a name="l00100"></a><a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a1f2be858dfb40e06cbae68375c98941b">00100</a> <span class="comment"></span> <span class="keyword">inline</span> <span class="keyword">const</span> Derived& <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a1f2be858dfb40e06cbae68375c98941b" title="CRTP helper method.">derived</a>()<span class="keyword"> const </span>{ <span class="keywordflow">return</span> *<span class="keyword">static_cast<</span><span class="keyword">const </span>Derived*<span class="keyword">></span>(<span class="keyword">this</span>); }<span class="comment"></span>
<a name="l00101"></a>00101 <span class="comment"> /// CRTP helper method</span>
<a name="l00102"></a><a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a0be68a7843dfe5d5dfa5206c06e3d4fb">00102</a> <span class="comment"></span> <span class="keyword">inline</span> Derived& <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a0be68a7843dfe5d5dfa5206c06e3d4fb" title="CRTP helper method.">derived</a>() { <span class="keywordflow">return</span> *<span class="keyword">static_cast<</span>Derived*<span class="keyword">></span>(<span class="keyword">this</span>); }
<a name="l00103"></a>00103
<a name="l00104"></a>00104 <span class="keyword">struct </span>TKDTreeSearchParams
<a name="l00105"></a>00105 {
<a name="l00106"></a><a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_search_params.html#a500cc29d59351f2bb5debaf40c7ffa05">00106</a> <a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_search_params.html#a500cc29d59351f2bb5debaf40c7ffa05">TKDTreeSearchParams</a>() :
<a name="l00107"></a>00107 <a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_search_params.html#a54ba56007b68b4fa6a2a2b1018ca9935" title="The number of checks for ANN (default: 32) - corresponds to FLANN's SearchParams::check.">nChecks</a>(32),
<a name="l00108"></a>00108 <a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_search_params.html#a3ef8cecc39f4227d58ac7955e5c65478" title="Max points per leaf.">leaf_max_size</a>(10)
<a name="l00109"></a>00109 {
<a name="l00110"></a>00110 }
<a name="l00111"></a>00111
<a name="l00112"></a><a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_search_params.html#a54ba56007b68b4fa6a2a2b1018ca9935">00112</a> <span class="keywordtype">int</span> <a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_search_params.html#a54ba56007b68b4fa6a2a2b1018ca9935" title="The number of checks for ANN (default: 32) - corresponds to FLANN's SearchParams::check.">nChecks</a>; <span class="comment">//!< The number of checks for ANN (default: 32) - corresponds to FLANN's SearchParams::check</span>
<a name="l00113"></a><a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_search_params.html#a3ef8cecc39f4227d58ac7955e5c65478">00113</a> <span class="comment"></span> <span class="keywordtype">int</span> <a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_search_params.html#a3ef8cecc39f4227d58ac7955e5c65478" title="Max points per leaf.">leaf_max_size</a>; <span class="comment">//!< Max points per leaf</span>
<a name="l00114"></a>00114 <span class="comment"></span> };
<a name="l00115"></a>00115
<a name="l00116"></a><a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a65f5da9a222c2c7b913f7e6242c6c77e">00116</a> TKDTreeSearchParams <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a65f5da9a222c2c7b913f7e6242c6c77e" title="Parameters to tune the ANN searches.">kdtree_search_params</a>; <span class="comment">//!< Parameters to tune the ANN searches</span>
<a name="l00117"></a>00117 <span class="comment"></span><span class="comment"></span>
<a name="l00118"></a>00118 <span class="comment"> /** @name Public utility methods to query the KD-tree</span>
<a name="l00119"></a>00119 <span class="comment"> @{ */</span>
<a name="l00120"></a>00120 <span class="comment"></span>
<a name="l00121"></a>00121 <span class="comment"> /** KD Tree-based search for the closest point (only ONE) to some given 2D coordinates.</span>
<a name="l00122"></a>00122 <span class="comment"> * This method automatically build the "m_kdtree_data" structure when:</span>
<a name="l00123"></a>00123 <span class="comment"> * - It is called for the first time</span>
<a name="l00124"></a>00124 <span class="comment"> * - The map has changed</span>
<a name="l00125"></a>00125 <span class="comment"> * - The KD-tree was build for 3D.</span>
<a name="l00126"></a>00126 <span class="comment"> *</span>
<a name="l00127"></a>00127 <span class="comment"> * \param x0 The X coordinate of the query.</span>
<a name="l00128"></a>00128 <span class="comment"> * \param y0 The Y coordinate of the query.</span>
<a name="l00129"></a>00129 <span class="comment"> * \param out_x The X coordinate of the found closest correspondence.</span>
<a name="l00130"></a>00130 <span class="comment"> * \param out_y The Y coordinate of the found closest correspondence.</span>
<a name="l00131"></a>00131 <span class="comment"> * \param out_dist_sqr The square distance between the query and the returned point.</span>
<a name="l00132"></a>00132 <span class="comment"> *</span>
<a name="l00133"></a>00133 <span class="comment"> * \return The index of the closest point in the map array.</span>
<a name="l00134"></a>00134 <span class="comment"> * \sa kdTreeClosestPoint3D, kdTreeTwoClosestPoint2D</span>
<a name="l00135"></a>00135 <span class="comment"> */</span>
<a name="l00136"></a><a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#ae9c0bb8eae4945085a0b03408adaa180">00136</a> <span class="keyword">inline</span> <span class="keywordtype">size_t</span> <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#ae9c0bb8eae4945085a0b03408adaa180" title="KD Tree-based search for the closest point (only ONE) to some given 2D coordinates.">kdTreeClosestPoint2D</a>(
<a name="l00137"></a>00137 <span class="keywordtype">float</span> x0,
<a name="l00138"></a>00138 <span class="keywordtype">float</span> y0,
<a name="l00139"></a>00139 <span class="keywordtype">float</span> &out_x,
<a name="l00140"></a>00140 <span class="keywordtype">float</span> &out_y,
<a name="l00141"></a>00141 <span class="keywordtype">float</span> &out_dist_sqr
<a name="l00142"></a>00142 )<span class="keyword"> const</span>
<a name="l00143"></a>00143 <span class="keyword"> </span>{
<a name="l00144"></a>00144 <a class="code" href="mrpt__macros_8h.html#a45b840af519f33816311acdbb28d7c10">MRPT_START</a>
<a name="l00145"></a>00145 <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#ac0bd164a47e9c98506a301d2146b316e" title="Rebuild, if needed the KD-tree for 2D (nDims=2), 3D (nDims=3), ... asking the child class for the dat...">rebuild_kdTree_2D</a>(); <span class="comment">// First: Create the 2D KD-Tree if required</span>
<a name="l00146"></a>00146 <span class="keywordflow">if</span> ( !<a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a3abb2bf223afb15c769598d0ef65bcd9">m_kdtree2d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#a524ecb048be5467a306d5cbbd2a589a2">m_num_points</a> ) <a class="code" href="mrpt__macros_8h.html#aaa3f404ea85a6575a7139f8d101370ba">THROW_EXCEPTION</a>(<span class="stringliteral">"There are no points in the KD-tree."</span>)
<a name="l00147"></a>00147
<a name="l00148"></a>00148 <span class="keyword">const</span> <span class="keywordtype">int</span> knn = 1; <span class="comment">// Number of points to retrieve</span>
<a name="l00149"></a>00149 <span class="keywordtype">int</span> ret_index;
<a name="l00150"></a>00150 nanoflann::KNNResultSet<num_t> resultSet(knn);
<a name="l00151"></a>00151 resultSet.<a class="code" href="classnanoflann_1_1_k_n_n_result_set.html#a4fefecb0ff9480ceeb1212afa67f9966">init</a>(&ret_index, &out_dist_sqr );
<a name="l00152"></a>00152
<a name="l00153"></a>00153 <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a3abb2bf223afb15c769598d0ef65bcd9">m_kdtree2d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#a36efc2e6a2110e60c17c527919f98a57">query_point</a>[0] = x0;
<a name="l00154"></a>00154 <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a3abb2bf223afb15c769598d0ef65bcd9">m_kdtree2d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#a36efc2e6a2110e60c17c527919f98a57">query_point</a>[1] = y0;
<a name="l00155"></a>00155 <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a3abb2bf223afb15c769598d0ef65bcd9">m_kdtree2d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#ad2ca3223a1f27615feeb5d17321d7165" title="NULL or the up-to-date index.">index</a>->findNeighbors(resultSet, &<a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a3abb2bf223afb15c769598d0ef65bcd9">m_kdtree2d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#a36efc2e6a2110e60c17c527919f98a57">query_point</a>[0], <a class="code" href="structnanoflann_1_1_search_params.html" title="Search options for KDTreeSingleIndexAdaptor::findNeighbors()">nanoflann::SearchParams</a>(<a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a65f5da9a222c2c7b913f7e6242c6c77e" title="Parameters to tune the ANN searches.">kdtree_search_params</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_search_params.html#a54ba56007b68b4fa6a2a2b1018ca9935" title="The number of checks for ANN (default: 32) - corresponds to FLANN's SearchParams::check.">nChecks</a>));
<a name="l00156"></a>00156
<a name="l00157"></a>00157 <span class="comment">// Copy output to user vars:</span>
<a name="l00158"></a>00158 out_x = <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a1f2be858dfb40e06cbae68375c98941b" title="CRTP helper method.">derived</a>().kdtree_get_pt(ret_index,0);
<a name="l00159"></a>00159 out_y = <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a1f2be858dfb40e06cbae68375c98941b" title="CRTP helper method.">derived</a>().kdtree_get_pt(ret_index,1);
<a name="l00160"></a>00160
<a name="l00161"></a>00161 <span class="keywordflow">return</span> <span class="keyword">static_cast<</span><span class="keywordtype">size_t</span><span class="keyword">></span>(ret_index);
<a name="l00162"></a>00162 <a class="code" href="mrpt__macros_8h.html#a88a917260793b56abd83ad2a0d849eb1">MRPT_END</a>
<a name="l00163"></a>00163 }
<a name="l00164"></a>00164 <span class="comment"></span>
<a name="l00165"></a>00165 <span class="comment"> /// \overload</span>
<a name="l00166"></a><a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#acccf80ebe86d7512a2eabb1f4cd7d5e7">00166</a> <span class="comment"></span> <span class="keyword">inline</span> <span class="keywordtype">size_t</span> <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#ae9c0bb8eae4945085a0b03408adaa180" title="KD Tree-based search for the closest point (only ONE) to some given 2D coordinates.">kdTreeClosestPoint2D</a>(
<a name="l00167"></a>00167 <span class="keywordtype">float</span> x0,
<a name="l00168"></a>00168 <span class="keywordtype">float</span> y0,
<a name="l00169"></a>00169 <span class="keywordtype">float</span> &out_dist_sqr )<span class="keyword"> const</span>
<a name="l00170"></a>00170 <span class="keyword"> </span>{
<a name="l00171"></a>00171 <a class="code" href="mrpt__macros_8h.html#a45b840af519f33816311acdbb28d7c10">MRPT_START</a>
<a name="l00172"></a>00172 <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#ac0bd164a47e9c98506a301d2146b316e" title="Rebuild, if needed the KD-tree for 2D (nDims=2), 3D (nDims=3), ... asking the child class for the dat...">rebuild_kdTree_2D</a>(); <span class="comment">// First: Create the 2D KD-Tree if required</span>
<a name="l00173"></a>00173 <span class="keywordflow">if</span> ( !<a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a3abb2bf223afb15c769598d0ef65bcd9">m_kdtree2d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#a524ecb048be5467a306d5cbbd2a589a2">m_num_points</a> ) <a class="code" href="mrpt__macros_8h.html#aaa3f404ea85a6575a7139f8d101370ba">THROW_EXCEPTION</a>(<span class="stringliteral">"There are no points in the KD-tree."</span>)
<a name="l00174"></a>00174
<a name="l00175"></a>00175 <span class="keyword">const</span> <span class="keywordtype">int</span> knn = 1; <span class="comment">// Number of points to retrieve</span>
<a name="l00176"></a>00176 <span class="keywordtype">int</span> ret_index;
<a name="l00177"></a>00177 nanoflann::KNNResultSet<num_t> resultSet(knn);
<a name="l00178"></a>00178 resultSet.<a class="code" href="classnanoflann_1_1_k_n_n_result_set.html#a4fefecb0ff9480ceeb1212afa67f9966">init</a>(&ret_index, &out_dist_sqr );
<a name="l00179"></a>00179
<a name="l00180"></a>00180 <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a3abb2bf223afb15c769598d0ef65bcd9">m_kdtree2d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#a36efc2e6a2110e60c17c527919f98a57">query_point</a>[0] = x0;
<a name="l00181"></a>00181 <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a3abb2bf223afb15c769598d0ef65bcd9">m_kdtree2d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#a36efc2e6a2110e60c17c527919f98a57">query_point</a>[1] = y0;
<a name="l00182"></a>00182 <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a3abb2bf223afb15c769598d0ef65bcd9">m_kdtree2d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#ad2ca3223a1f27615feeb5d17321d7165" title="NULL or the up-to-date index.">index</a>->findNeighbors(resultSet, &<a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a3abb2bf223afb15c769598d0ef65bcd9">m_kdtree2d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#a36efc2e6a2110e60c17c527919f98a57">query_point</a>[0], <a class="code" href="structnanoflann_1_1_search_params.html" title="Search options for KDTreeSingleIndexAdaptor::findNeighbors()">nanoflann::SearchParams</a>(<a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a65f5da9a222c2c7b913f7e6242c6c77e" title="Parameters to tune the ANN searches.">kdtree_search_params</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_search_params.html#a54ba56007b68b4fa6a2a2b1018ca9935" title="The number of checks for ANN (default: 32) - corresponds to FLANN's SearchParams::check.">nChecks</a>));
<a name="l00183"></a>00183
<a name="l00184"></a>00184 <span class="keywordflow">return</span> <span class="keyword">static_cast<</span><span class="keywordtype">size_t</span><span class="keyword">></span>(ret_index);
<a name="l00185"></a>00185 <a class="code" href="mrpt__macros_8h.html#a88a917260793b56abd83ad2a0d849eb1">MRPT_END</a>
<a name="l00186"></a>00186 }
<a name="l00187"></a>00187 <span class="comment"></span>
<a name="l00188"></a>00188 <span class="comment"> /// \overload</span>
<a name="l00189"></a><a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#aa931ad90835e719b966cf61c317c6e6d">00189</a> <span class="comment"></span> <span class="keyword">inline</span> <span class="keywordtype">size_t</span> <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#aa931ad90835e719b966cf61c317c6e6d">kdTreeClosestPoint2D</a>(<span class="keyword">const</span> <a class="code" href="structmrpt_1_1math_1_1_t_point2_d.html" title="Lightweight 2D point.">TPoint2D</a> &p0,<a class="code" href="structmrpt_1_1math_1_1_t_point2_d.html" title="Lightweight 2D point.">TPoint2D</a> &pOut,<span class="keywordtype">float</span> &outDistSqr)<span class="keyword"> const </span>{
<a name="l00190"></a>00190 <span class="keywordtype">float</span> dmy1,dmy2;
<a name="l00191"></a>00191 <span class="keywordtype">size_t</span> res=<a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#ae9c0bb8eae4945085a0b03408adaa180" title="KD Tree-based search for the closest point (only ONE) to some given 2D coordinates.">kdTreeClosestPoint2D</a>(static_cast<float>(p0.<a class="code" href="structmrpt_1_1math_1_1_t_point2_d.html#a084825c8b4d02df7b89d66f8b08e26f2" title="X coordinate.">x</a>),static_cast<float>(p0.<a class="code" href="structmrpt_1_1math_1_1_t_point2_d.html#a1b6b3c44215ec8285ae97ff1ad1f6fcf" title="Y coordinate.">y</a>),dmy1,dmy2,outDistSqr);
<a name="l00192"></a>00192 pOut.<a class="code" href="structmrpt_1_1math_1_1_t_point2_d.html#a084825c8b4d02df7b89d66f8b08e26f2" title="X coordinate.">x</a>=dmy1;
<a name="l00193"></a>00193 pOut.<a class="code" href="structmrpt_1_1math_1_1_t_point2_d.html#a1b6b3c44215ec8285ae97ff1ad1f6fcf" title="Y coordinate.">y</a>=dmy2;
<a name="l00194"></a>00194 <span class="keywordflow">return</span> res;
<a name="l00195"></a>00195 }
<a name="l00196"></a>00196 <span class="comment"></span>
<a name="l00197"></a>00197 <span class="comment"> /** Like kdTreeClosestPoint2D, but just return the square error from some point to its closest neighbor.</span>
<a name="l00198"></a>00198 <span class="comment"> */</span>
<a name="l00199"></a><a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#aa8a35f96809c7677b730f8282c77e1ac">00199</a> <span class="keyword">inline</span> <span class="keywordtype">float</span> <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#aa8a35f96809c7677b730f8282c77e1ac" title="Like kdTreeClosestPoint2D, but just return the square error from some point to its closest neighbor...">kdTreeClosestPoint2DsqrError</a>(
<a name="l00200"></a>00200 <span class="keywordtype">float</span> x0,
<a name="l00201"></a>00201 <span class="keywordtype">float</span> y0 )<span class="keyword"> const</span>
<a name="l00202"></a>00202 <span class="keyword"> </span>{
<a name="l00203"></a>00203 <span class="keywordtype">float</span> closerx,closery,closer_dist;
<a name="l00204"></a>00204 <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#ae9c0bb8eae4945085a0b03408adaa180" title="KD Tree-based search for the closest point (only ONE) to some given 2D coordinates.">kdTreeClosestPoint2D</a>(x0,y0, closerx,closery,closer_dist);
<a name="l00205"></a>00205 <span class="keywordflow">return</span> closer_dist;
<a name="l00206"></a>00206 }
<a name="l00207"></a>00207
<a name="l00208"></a><a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#abee2f8ff656e8eca62d0444021577776">00208</a> <span class="keyword">inline</span> <span class="keywordtype">float</span> <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#abee2f8ff656e8eca62d0444021577776">kdTreeClosestPoint2DsqrError</a>(<span class="keyword">const</span> <a class="code" href="structmrpt_1_1math_1_1_t_point2_d.html" title="Lightweight 2D point.">TPoint2D</a> &p0)<span class="keyword"> const </span>{
<a name="l00209"></a>00209 <span class="keywordflow">return</span> <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#aa8a35f96809c7677b730f8282c77e1ac" title="Like kdTreeClosestPoint2D, but just return the square error from some point to its closest neighbor...">kdTreeClosestPoint2DsqrError</a>(static_cast<float>(p0.<a class="code" href="structmrpt_1_1math_1_1_t_point2_d.html#a084825c8b4d02df7b89d66f8b08e26f2" title="X coordinate.">x</a>),static_cast<float>(p0.<a class="code" href="structmrpt_1_1math_1_1_t_point2_d.html#a1b6b3c44215ec8285ae97ff1ad1f6fcf" title="Y coordinate.">y</a>));
<a name="l00210"></a>00210 }
<a name="l00211"></a>00211 <span class="comment"></span>
<a name="l00212"></a>00212 <span class="comment"> /** KD Tree-based search for the TWO closest point to some given 2D coordinates.</span>
<a name="l00213"></a>00213 <span class="comment"> * This method automatically build the "m_kdtree_data" structure when:</span>
<a name="l00214"></a>00214 <span class="comment"> * - It is called for the first time</span>
<a name="l00215"></a>00215 <span class="comment"> * - The map has changed</span>
<a name="l00216"></a>00216 <span class="comment"> * - The KD-tree was build for 3D.</span>
<a name="l00217"></a>00217 <span class="comment"> *</span>
<a name="l00218"></a>00218 <span class="comment"> * \param x0 The X coordinate of the query.</span>
<a name="l00219"></a>00219 <span class="comment"> * \param y0 The Y coordinate of the query.</span>
<a name="l00220"></a>00220 <span class="comment"> * \param out_x1 The X coordinate of the first correspondence.</span>
<a name="l00221"></a>00221 <span class="comment"> * \param out_y1 The Y coordinate of the first correspondence.</span>
<a name="l00222"></a>00222 <span class="comment"> * \param out_x2 The X coordinate of the second correspondence.</span>
<a name="l00223"></a>00223 <span class="comment"> * \param out_y2 The Y coordinate of the second correspondence.</span>
<a name="l00224"></a>00224 <span class="comment"> * \param out_dist_sqr1 The square distance between the query and the first returned point.</span>
<a name="l00225"></a>00225 <span class="comment"> * \param out_dist_sqr2 The square distance between the query and the second returned point.</span>
<a name="l00226"></a>00226 <span class="comment"> *</span>
<a name="l00227"></a>00227 <span class="comment"> * \sa kdTreeClosestPoint2D</span>
<a name="l00228"></a>00228 <span class="comment"> */</span>
<a name="l00229"></a><a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a9edfbd2ada1466c02e7bf136c39ca9dd">00229</a> <span class="keyword">inline</span> <span class="keywordtype">void</span> <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a9edfbd2ada1466c02e7bf136c39ca9dd" title="KD Tree-based search for the TWO closest point to some given 2D coordinates.">kdTreeTwoClosestPoint2D</a>(
<a name="l00230"></a>00230 <span class="keywordtype">float</span> x0,
<a name="l00231"></a>00231 <span class="keywordtype">float</span> y0,
<a name="l00232"></a>00232 <span class="keywordtype">float</span> &out_x1,
<a name="l00233"></a>00233 <span class="keywordtype">float</span> &out_y1,
<a name="l00234"></a>00234 <span class="keywordtype">float</span> &out_x2,
<a name="l00235"></a>00235 <span class="keywordtype">float</span> &out_y2,
<a name="l00236"></a>00236 <span class="keywordtype">float</span> &out_dist_sqr1,
<a name="l00237"></a>00237 <span class="keywordtype">float</span> &out_dist_sqr2 )<span class="keyword"> const</span>
<a name="l00238"></a>00238 <span class="keyword"> </span>{
<a name="l00239"></a>00239 <a class="code" href="mrpt__macros_8h.html#a45b840af519f33816311acdbb28d7c10">MRPT_START</a>
<a name="l00240"></a>00240 <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#ac0bd164a47e9c98506a301d2146b316e" title="Rebuild, if needed the KD-tree for 2D (nDims=2), 3D (nDims=3), ... asking the child class for the dat...">rebuild_kdTree_2D</a>(); <span class="comment">// First: Create the 2D KD-Tree if required</span>
<a name="l00241"></a>00241 <span class="keywordflow">if</span> ( !<a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a3abb2bf223afb15c769598d0ef65bcd9">m_kdtree2d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#a524ecb048be5467a306d5cbbd2a589a2">m_num_points</a> ) <a class="code" href="mrpt__macros_8h.html#aaa3f404ea85a6575a7139f8d101370ba">THROW_EXCEPTION</a>(<span class="stringliteral">"There are no points in the KD-tree."</span>)
<a name="l00242"></a>00242
<a name="l00243"></a>00243 <span class="keyword">const</span> <span class="keywordtype">int</span> knn = 2; <span class="comment">// Number of points to retrieve</span>
<a name="l00244"></a>00244 <span class="keywordtype">int</span> ret_indexes[2];
<a name="l00245"></a>00245 <span class="keywordtype">float</span> ret_sqdist[2];
<a name="l00246"></a>00246 nanoflann::KNNResultSet<num_t> resultSet(knn);
<a name="l00247"></a>00247 resultSet.<a class="code" href="classnanoflann_1_1_k_n_n_result_set.html#a4fefecb0ff9480ceeb1212afa67f9966">init</a>(&ret_indexes[0], &ret_sqdist[0] );
<a name="l00248"></a>00248
<a name="l00249"></a>00249 <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a3abb2bf223afb15c769598d0ef65bcd9">m_kdtree2d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#a36efc2e6a2110e60c17c527919f98a57">query_point</a>[0] = x0;
<a name="l00250"></a>00250 <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a3abb2bf223afb15c769598d0ef65bcd9">m_kdtree2d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#a36efc2e6a2110e60c17c527919f98a57">query_point</a>[1] = y0;
<a name="l00251"></a>00251 <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a3abb2bf223afb15c769598d0ef65bcd9">m_kdtree2d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#ad2ca3223a1f27615feeb5d17321d7165" title="NULL or the up-to-date index.">index</a>->findNeighbors(resultSet, &<a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a3abb2bf223afb15c769598d0ef65bcd9">m_kdtree2d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#a36efc2e6a2110e60c17c527919f98a57">query_point</a>[0], <a class="code" href="structnanoflann_1_1_search_params.html" title="Search options for KDTreeSingleIndexAdaptor::findNeighbors()">nanoflann::SearchParams</a>(<a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a65f5da9a222c2c7b913f7e6242c6c77e" title="Parameters to tune the ANN searches.">kdtree_search_params</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_search_params.html#a54ba56007b68b4fa6a2a2b1018ca9935" title="The number of checks for ANN (default: 32) - corresponds to FLANN's SearchParams::check.">nChecks</a>));
<a name="l00252"></a>00252
<a name="l00253"></a>00253 <span class="comment">// Copy output to user vars:</span>
<a name="l00254"></a>00254 out_x1 = <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a1f2be858dfb40e06cbae68375c98941b" title="CRTP helper method.">derived</a>().kdtree_get_pt(ret_indexes[0],0);
<a name="l00255"></a>00255 out_y1 = <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a1f2be858dfb40e06cbae68375c98941b" title="CRTP helper method.">derived</a>().kdtree_get_pt(ret_indexes[0],1);
<a name="l00256"></a>00256 out_dist_sqr1 = ret_sqdist[0];
<a name="l00257"></a>00257
<a name="l00258"></a>00258 out_x2 = <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a1f2be858dfb40e06cbae68375c98941b" title="CRTP helper method.">derived</a>().kdtree_get_pt(ret_indexes[1],0);
<a name="l00259"></a>00259 out_y2 = <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a1f2be858dfb40e06cbae68375c98941b" title="CRTP helper method.">derived</a>().kdtree_get_pt(ret_indexes[1],1);
<a name="l00260"></a>00260 out_dist_sqr2 = ret_sqdist[0];
<a name="l00261"></a>00261
<a name="l00262"></a>00262 <a class="code" href="mrpt__macros_8h.html#a88a917260793b56abd83ad2a0d849eb1">MRPT_END</a>
<a name="l00263"></a>00263 }
<a name="l00264"></a>00264
<a name="l00265"></a>00265
<a name="l00266"></a><a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#ad9af4bb0f1547c6718c7a5ef5f3e7591">00266</a> <span class="keyword">inline</span> <span class="keywordtype">void</span> <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#ad9af4bb0f1547c6718c7a5ef5f3e7591">kdTreeTwoClosestPoint2D</a>(<span class="keyword">const</span> <a class="code" href="structmrpt_1_1math_1_1_t_point2_d.html" title="Lightweight 2D point.">TPoint2D</a> &p0,<a class="code" href="structmrpt_1_1math_1_1_t_point2_d.html" title="Lightweight 2D point.">TPoint2D</a> &pOut1,<a class="code" href="structmrpt_1_1math_1_1_t_point2_d.html" title="Lightweight 2D point.">TPoint2D</a> &pOut2,<span class="keywordtype">float</span> &outDistSqr1,<span class="keywordtype">float</span> &outDistSqr2)<span class="keyword"> const </span>{
<a name="l00267"></a>00267 <span class="keywordtype">float</span> dmy1,dmy2,dmy3,dmy4;
<a name="l00268"></a>00268 <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a9edfbd2ada1466c02e7bf136c39ca9dd" title="KD Tree-based search for the TWO closest point to some given 2D coordinates.">kdTreeTwoClosestPoint2D</a>(p0.<a class="code" href="structmrpt_1_1math_1_1_t_point2_d.html#a084825c8b4d02df7b89d66f8b08e26f2" title="X coordinate.">x</a>,p0.<a class="code" href="structmrpt_1_1math_1_1_t_point2_d.html#a1b6b3c44215ec8285ae97ff1ad1f6fcf" title="Y coordinate.">y</a>,dmy1,dmy2,dmy3,dmy4,outDistSqr1,outDistSqr2);
<a name="l00269"></a>00269 pOut1.<a class="code" href="structmrpt_1_1math_1_1_t_point2_d.html#a084825c8b4d02df7b89d66f8b08e26f2" title="X coordinate.">x</a>=<span class="keyword">static_cast<</span><span class="keywordtype">double</span><span class="keyword">></span>(dmy1);
<a name="l00270"></a>00270 pOut1.<a class="code" href="structmrpt_1_1math_1_1_t_point2_d.html#a1b6b3c44215ec8285ae97ff1ad1f6fcf" title="Y coordinate.">y</a>=<span class="keyword">static_cast<</span><span class="keywordtype">double</span><span class="keyword">></span>(dmy2);
<a name="l00271"></a>00271 pOut2.<a class="code" href="structmrpt_1_1math_1_1_t_point2_d.html#a084825c8b4d02df7b89d66f8b08e26f2" title="X coordinate.">x</a>=<span class="keyword">static_cast<</span><span class="keywordtype">double</span><span class="keyword">></span>(dmy3);
<a name="l00272"></a>00272 pOut2.<a class="code" href="structmrpt_1_1math_1_1_t_point2_d.html#a1b6b3c44215ec8285ae97ff1ad1f6fcf" title="Y coordinate.">y</a>=<span class="keyword">static_cast<</span><span class="keywordtype">double</span><span class="keyword">></span>(dmy4);
<a name="l00273"></a>00273 }
<a name="l00274"></a>00274 <span class="comment"></span>
<a name="l00275"></a>00275 <span class="comment"> /** KD Tree-based search for the N closest point to some given 2D coordinates.</span>
<a name="l00276"></a>00276 <span class="comment"> * This method automatically build the "m_kdtree_data" structure when:</span>
<a name="l00277"></a>00277 <span class="comment"> * - It is called for the first time</span>
<a name="l00278"></a>00278 <span class="comment"> * - The map has changed</span>
<a name="l00279"></a>00279 <span class="comment"> * - The KD-tree was build for 3D.</span>
<a name="l00280"></a>00280 <span class="comment"> *</span>
<a name="l00281"></a>00281 <span class="comment"> * \param x0 The X coordinate of the query.</span>
<a name="l00282"></a>00282 <span class="comment"> * \param y0 The Y coordinate of the query.</span>
<a name="l00283"></a>00283 <span class="comment"> * \param N The number of closest points to search.</span>
<a name="l00284"></a>00284 <span class="comment"> * \param out_x The vector containing the X coordinates of the correspondences.</span>
<a name="l00285"></a>00285 <span class="comment"> * \param out_y The vector containing the Y coordinates of the correspondences.</span>
<a name="l00286"></a>00286 <span class="comment"> * \param out_dist_sqr The vector containing the square distance between the query and the returned points.</span>
<a name="l00287"></a>00287 <span class="comment"> *</span>
<a name="l00288"></a>00288 <span class="comment"> * \return The list of indices</span>
<a name="l00289"></a>00289 <span class="comment"> * \sa kdTreeClosestPoint2D</span>
<a name="l00290"></a>00290 <span class="comment"> * \sa kdTreeTwoClosestPoint2D</span>
<a name="l00291"></a>00291 <span class="comment"> */</span>
<a name="l00292"></a>00292 <span class="keyword">inline</span>
<a name="l00293"></a><a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#abf55cf62066eeff4c9db446b040c481d">00293</a> std<a class="code" href="classstd_1_1vector.html">::vector<int></a> <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#abf55cf62066eeff4c9db446b040c481d" title="KD Tree-based search for the N closest point to some given 2D coordinates.">kdTreeNClosestPoint2D</a>(
<a name="l00294"></a>00294 <span class="keywordtype">float</span> x0,
<a name="l00295"></a>00295 <span class="keywordtype">float</span> y0,
<a name="l00296"></a>00296 <span class="keywordtype">size_t</span> knn,
<a name="l00297"></a>00297 <a class="code" href="classstd_1_1vector.html">std::vector<float></a> &out_x,
<a name="l00298"></a>00298 <a class="code" href="classstd_1_1vector.html">std::vector<float></a> &out_y,
<a name="l00299"></a>00299 <a class="code" href="classstd_1_1vector.html">std::vector<float></a> &out_dist_sqr )<span class="keyword"> const</span>
<a name="l00300"></a>00300 <span class="keyword"> </span>{
<a name="l00301"></a>00301 <a class="code" href="mrpt__macros_8h.html#a45b840af519f33816311acdbb28d7c10">MRPT_START</a>
<a name="l00302"></a>00302 <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#ac0bd164a47e9c98506a301d2146b316e" title="Rebuild, if needed the KD-tree for 2D (nDims=2), 3D (nDims=3), ... asking the child class for the dat...">rebuild_kdTree_2D</a>(); <span class="comment">// First: Create the 2D KD-Tree if required</span>
<a name="l00303"></a>00303 <span class="keywordflow">if</span> ( !<a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a3abb2bf223afb15c769598d0ef65bcd9">m_kdtree2d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#a524ecb048be5467a306d5cbbd2a589a2">m_num_points</a> ) <a class="code" href="mrpt__macros_8h.html#aaa3f404ea85a6575a7139f8d101370ba">THROW_EXCEPTION</a>(<span class="stringliteral">"There are no points in the KD-tree."</span>)
<a name="l00304"></a>00304
<a name="l00305"></a>00305 std<a class="code" href="classstd_1_1vector.html">::vector<int></a> ret_indexes(knn);
<a name="l00306"></a>00306 out_x.resize(knn);
<a name="l00307"></a>00307 out_y.resize(knn);
<a name="l00308"></a>00308 out_dist_sqr.resize(knn);
<a name="l00309"></a>00309
<a name="l00310"></a>00310 nanoflann::KNNResultSet<num_t> resultSet(knn);
<a name="l00311"></a>00311 resultSet.<a class="code" href="classnanoflann_1_1_k_n_n_result_set.html#a4fefecb0ff9480ceeb1212afa67f9966">init</a>(&ret_indexes[0], &out_dist_sqr[0] );
<a name="l00312"></a>00312
<a name="l00313"></a>00313 <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a3abb2bf223afb15c769598d0ef65bcd9">m_kdtree2d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#a36efc2e6a2110e60c17c527919f98a57">query_point</a>[0] = x0;
<a name="l00314"></a>00314 <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a3abb2bf223afb15c769598d0ef65bcd9">m_kdtree2d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#a36efc2e6a2110e60c17c527919f98a57">query_point</a>[1] = y0;
<a name="l00315"></a>00315 <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a3abb2bf223afb15c769598d0ef65bcd9">m_kdtree2d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#ad2ca3223a1f27615feeb5d17321d7165" title="NULL or the up-to-date index.">index</a>->findNeighbors(resultSet, &<a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a3abb2bf223afb15c769598d0ef65bcd9">m_kdtree2d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#a36efc2e6a2110e60c17c527919f98a57">query_point</a>[0], <a class="code" href="structnanoflann_1_1_search_params.html" title="Search options for KDTreeSingleIndexAdaptor::findNeighbors()">nanoflann::SearchParams</a>(<a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a65f5da9a222c2c7b913f7e6242c6c77e" title="Parameters to tune the ANN searches.">kdtree_search_params</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_search_params.html#a54ba56007b68b4fa6a2a2b1018ca9935" title="The number of checks for ANN (default: 32) - corresponds to FLANN's SearchParams::check.">nChecks</a>));
<a name="l00316"></a>00316
<a name="l00317"></a>00317 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i=0;i<knn;i++)
<a name="l00318"></a>00318 {
<a name="l00319"></a>00319 out_x[i] = <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a1f2be858dfb40e06cbae68375c98941b" title="CRTP helper method.">derived</a>().kdtree_get_pt(ret_indexes[i],0);
<a name="l00320"></a>00320 out_y[i] = <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a1f2be858dfb40e06cbae68375c98941b" title="CRTP helper method.">derived</a>().kdtree_get_pt(ret_indexes[i],1);
<a name="l00321"></a>00321 }
<a name="l00322"></a>00322 <span class="keywordflow">return</span> ret_indexes;
<a name="l00323"></a>00323 <a class="code" href="mrpt__macros_8h.html#a88a917260793b56abd83ad2a0d849eb1">MRPT_END</a>
<a name="l00324"></a>00324 }
<a name="l00325"></a>00325
<a name="l00326"></a><a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a98394b54ad28658bdb397116bcd7a1d7">00326</a> <span class="keyword">inline</span> std<a class="code" href="classstd_1_1vector.html">::vector<int></a> <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a98394b54ad28658bdb397116bcd7a1d7">kdTreeNClosestPoint2D</a>(<span class="keyword">const</span> <a class="code" href="structmrpt_1_1math_1_1_t_point2_d.html" title="Lightweight 2D point.">TPoint2D</a> &p0,<span class="keywordtype">size_t</span> N,<a class="code" href="classstd_1_1vector.html">std::vector<TPoint2D></a> &pOut,<a class="code" href="classstd_1_1vector.html">std::vector<float></a> &outDistSqr)<span class="keyword"> const </span>{
<a name="l00327"></a>00327 std<a class="code" href="classstd_1_1vector.html">::vector<float></a> dmy1,dmy2;
<a name="l00328"></a>00328 std<a class="code" href="classstd_1_1vector.html">::vector<int></a> res=<a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#abf55cf62066eeff4c9db446b040c481d" title="KD Tree-based search for the N closest point to some given 2D coordinates.">kdTreeNClosestPoint2D</a>(static_cast<float>(p0.<a class="code" href="structmrpt_1_1math_1_1_t_point2_d.html#a084825c8b4d02df7b89d66f8b08e26f2" title="X coordinate.">x</a>),static_cast<float>(p0.<a class="code" href="structmrpt_1_1math_1_1_t_point2_d.html#a1b6b3c44215ec8285ae97ff1ad1f6fcf" title="Y coordinate.">y</a>),N,dmy1,dmy2,outDistSqr);
<a name="l00329"></a>00329 pOut.resize(dmy1.size());
<a name="l00330"></a>00330 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i=0;i<dmy1.size();i++) {
<a name="l00331"></a>00331 pOut[i].x=<span class="keyword">static_cast<</span><span class="keywordtype">double</span><span class="keyword">></span>(dmy1[i]);
<a name="l00332"></a>00332 pOut[i].y=<span class="keyword">static_cast<</span><span class="keywordtype">double</span><span class="keyword">></span>(dmy2[i]);
<a name="l00333"></a>00333 }
<a name="l00334"></a>00334 <span class="keywordflow">return</span> res;
<a name="l00335"></a>00335 }
<a name="l00336"></a>00336 <span class="comment"></span>
<a name="l00337"></a>00337 <span class="comment"> /** KD Tree-based search for the N closest point to some given 2D coordinates and returns their indexes.</span>
<a name="l00338"></a>00338 <span class="comment"> * This method automatically build the "m_kdtree_data" structure when:</span>
<a name="l00339"></a>00339 <span class="comment"> * - It is called for the first time</span>
<a name="l00340"></a>00340 <span class="comment"> * - The map has changed</span>
<a name="l00341"></a>00341 <span class="comment"> * - The KD-tree was build for 3D.</span>
<a name="l00342"></a>00342 <span class="comment"> *</span>
<a name="l00343"></a>00343 <span class="comment"> * \param x0 The X coordinate of the query.</span>
<a name="l00344"></a>00344 <span class="comment"> * \param y0 The Y coordinate of the query.</span>
<a name="l00345"></a>00345 <span class="comment"> * \param N The number of closest points to search.</span>
<a name="l00346"></a>00346 <span class="comment"> * \param out_idx The indexes of the found closest correspondence.</span>
<a name="l00347"></a>00347 <span class="comment"> * \param out_dist_sqr The square distance between the query and the returned point.</span>
<a name="l00348"></a>00348 <span class="comment"> *</span>
<a name="l00349"></a>00349 <span class="comment"> * \sa kdTreeClosestPoint2D</span>
<a name="l00350"></a>00350 <span class="comment"> */</span>
<a name="l00351"></a><a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a17665632b43c9a65530cbc97581128bc">00351</a> <span class="keyword">inline</span> <span class="keywordtype">void</span> <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a17665632b43c9a65530cbc97581128bc" title="KD Tree-based search for the N closest point to some given 2D coordinates and returns their indexes...">kdTreeNClosestPoint2DIdx</a>(
<a name="l00352"></a>00352 <span class="keywordtype">float</span> x0,
<a name="l00353"></a>00353 <span class="keywordtype">float</span> y0,
<a name="l00354"></a>00354 <span class="keywordtype">size_t</span> knn,
<a name="l00355"></a>00355 <a class="code" href="classstd_1_1vector.html">std::vector<int></a> &out_idx,
<a name="l00356"></a>00356 <a class="code" href="classstd_1_1vector.html">std::vector<float></a> &out_dist_sqr )<span class="keyword"> const</span>
<a name="l00357"></a>00357 <span class="keyword"> </span>{
<a name="l00358"></a>00358 <a class="code" href="mrpt__macros_8h.html#a45b840af519f33816311acdbb28d7c10">MRPT_START</a>
<a name="l00359"></a>00359 <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#ac0bd164a47e9c98506a301d2146b316e" title="Rebuild, if needed the KD-tree for 2D (nDims=2), 3D (nDims=3), ... asking the child class for the dat...">rebuild_kdTree_2D</a>(); <span class="comment">// First: Create the 2D KD-Tree if required</span>
<a name="l00360"></a>00360 <span class="keywordflow">if</span> ( !<a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a3abb2bf223afb15c769598d0ef65bcd9">m_kdtree2d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#a524ecb048be5467a306d5cbbd2a589a2">m_num_points</a> ) <a class="code" href="mrpt__macros_8h.html#aaa3f404ea85a6575a7139f8d101370ba">THROW_EXCEPTION</a>(<span class="stringliteral">"There are no points in the KD-tree."</span>)
<a name="l00361"></a>00361
<a name="l00362"></a>00362 out_idx.resize(knn);
<a name="l00363"></a>00363 out_dist_sqr.resize(knn);
<a name="l00364"></a>00364 nanoflann::KNNResultSet<num_t> resultSet(knn);
<a name="l00365"></a>00365 resultSet.<a class="code" href="classnanoflann_1_1_k_n_n_result_set.html#a4fefecb0ff9480ceeb1212afa67f9966">init</a>(&out_idx[0], &out_dist_sqr[0] );
<a name="l00366"></a>00366
<a name="l00367"></a>00367 <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a3abb2bf223afb15c769598d0ef65bcd9">m_kdtree2d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#a36efc2e6a2110e60c17c527919f98a57">query_point</a>[0] = x0;
<a name="l00368"></a>00368 <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a3abb2bf223afb15c769598d0ef65bcd9">m_kdtree2d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#a36efc2e6a2110e60c17c527919f98a57">query_point</a>[1] = y0;
<a name="l00369"></a>00369 <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a3abb2bf223afb15c769598d0ef65bcd9">m_kdtree2d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#ad2ca3223a1f27615feeb5d17321d7165" title="NULL or the up-to-date index.">index</a>->findNeighbors(resultSet, &<a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a3abb2bf223afb15c769598d0ef65bcd9">m_kdtree2d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#a36efc2e6a2110e60c17c527919f98a57">query_point</a>[0], <a class="code" href="structnanoflann_1_1_search_params.html" title="Search options for KDTreeSingleIndexAdaptor::findNeighbors()">nanoflann::SearchParams</a>(<a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a65f5da9a222c2c7b913f7e6242c6c77e" title="Parameters to tune the ANN searches.">kdtree_search_params</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_search_params.html#a54ba56007b68b4fa6a2a2b1018ca9935" title="The number of checks for ANN (default: 32) - corresponds to FLANN's SearchParams::check.">nChecks</a>));
<a name="l00370"></a>00370 <a class="code" href="mrpt__macros_8h.html#a88a917260793b56abd83ad2a0d849eb1">MRPT_END</a>
<a name="l00371"></a>00371 }
<a name="l00372"></a>00372
<a name="l00373"></a><a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a6d16ab2d09b0e2f59b368ce43f4ba944">00373</a> <span class="keyword">inline</span> <span class="keywordtype">void</span> <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a6d16ab2d09b0e2f59b368ce43f4ba944">kdTreeNClosestPoint2DIdx</a>(<span class="keyword">const</span> <a class="code" href="structmrpt_1_1math_1_1_t_point2_d.html" title="Lightweight 2D point.">TPoint2D</a> &p0,<span class="keywordtype">size_t</span> N,<a class="code" href="classstd_1_1vector.html">std::vector<int></a> &outIdx,<a class="code" href="classstd_1_1vector.html">std::vector<float></a> &outDistSqr)<span class="keyword"> const </span>{
<a name="l00374"></a>00374 <span class="keywordflow">return</span> <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a17665632b43c9a65530cbc97581128bc" title="KD Tree-based search for the N closest point to some given 2D coordinates and returns their indexes...">kdTreeNClosestPoint2DIdx</a>(static_cast<float>(p0.<a class="code" href="structmrpt_1_1math_1_1_t_point2_d.html#a084825c8b4d02df7b89d66f8b08e26f2" title="X coordinate.">x</a>),static_cast<float>(p0.<a class="code" href="structmrpt_1_1math_1_1_t_point2_d.html#a1b6b3c44215ec8285ae97ff1ad1f6fcf" title="Y coordinate.">y</a>),N,outIdx,outDistSqr);
<a name="l00375"></a>00375 }
<a name="l00376"></a>00376 <span class="comment"></span>
<a name="l00377"></a>00377 <span class="comment"> /** KD Tree-based search for the closest point (only ONE) to some given 3D coordinates.</span>
<a name="l00378"></a>00378 <span class="comment"> * This method automatically build the "m_kdtree_data" structure when:</span>
<a name="l00379"></a>00379 <span class="comment"> * - It is called for the first time</span>
<a name="l00380"></a>00380 <span class="comment"> * - The map has changed</span>
<a name="l00381"></a>00381 <span class="comment"> * - The KD-tree was build for 2D.</span>
<a name="l00382"></a>00382 <span class="comment"> *</span>
<a name="l00383"></a>00383 <span class="comment"> * \param x0 The X coordinate of the query.</span>
<a name="l00384"></a>00384 <span class="comment"> * \param y0 The Y coordinate of the query.</span>
<a name="l00385"></a>00385 <span class="comment"> * \param z0 The Z coordinate of the query.</span>
<a name="l00386"></a>00386 <span class="comment"> * \param out_x The X coordinate of the found closest correspondence.</span>
<a name="l00387"></a>00387 <span class="comment"> * \param out_y The Y coordinate of the found closest correspondence.</span>
<a name="l00388"></a>00388 <span class="comment"> * \param out_z The Z coordinate of the found closest correspondence.</span>
<a name="l00389"></a>00389 <span class="comment"> * \param out_dist_sqr The square distance between the query and the returned point.</span>
<a name="l00390"></a>00390 <span class="comment"> *</span>
<a name="l00391"></a>00391 <span class="comment"> * \return The index of the closest point in the map array.</span>
<a name="l00392"></a>00392 <span class="comment"> * \sa kdTreeClosestPoint2D</span>
<a name="l00393"></a>00393 <span class="comment"> */</span>
<a name="l00394"></a><a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a89eab3ed58a61ba1dd561552562a1215">00394</a> <span class="keyword">inline</span> <span class="keywordtype">size_t</span> <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a89eab3ed58a61ba1dd561552562a1215" title="KD Tree-based search for the closest point (only ONE) to some given 3D coordinates.">kdTreeClosestPoint3D</a>(
<a name="l00395"></a>00395 <span class="keywordtype">float</span> x0,
<a name="l00396"></a>00396 <span class="keywordtype">float</span> y0,
<a name="l00397"></a>00397 <span class="keywordtype">float</span> z0,
<a name="l00398"></a>00398 <span class="keywordtype">float</span> &out_x,
<a name="l00399"></a>00399 <span class="keywordtype">float</span> &out_y,
<a name="l00400"></a>00400 <span class="keywordtype">float</span> &out_z,
<a name="l00401"></a>00401 <span class="keywordtype">float</span> &out_dist_sqr
<a name="l00402"></a>00402 )<span class="keyword"> const</span>
<a name="l00403"></a>00403 <span class="keyword"> </span>{
<a name="l00404"></a>00404 <a class="code" href="mrpt__macros_8h.html#a45b840af519f33816311acdbb28d7c10">MRPT_START</a>
<a name="l00405"></a>00405 <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a71540b68861ea0afaea0b0dd24bd7c14" title="Rebuild, if needed the KD-tree for 2D (nDims=2), 3D (nDims=3), ... asking the child class for the dat...">rebuild_kdTree_3D</a>(); <span class="comment">// First: Create the 3D KD-Tree if required</span>
<a name="l00406"></a>00406 <span class="keywordflow">if</span> ( !<a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a2fe9f0119fbc4ed259f761b4cb42eba6">m_kdtree3d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#a524ecb048be5467a306d5cbbd2a589a2">m_num_points</a> ) <a class="code" href="mrpt__macros_8h.html#aaa3f404ea85a6575a7139f8d101370ba">THROW_EXCEPTION</a>(<span class="stringliteral">"There are no points in the KD-tree."</span>)
<a name="l00407"></a>00407
<a name="l00408"></a>00408 <span class="keyword">const</span> <span class="keywordtype">int</span> knn = 1; <span class="comment">// Number of points to retrieve</span>
<a name="l00409"></a>00409 <span class="keywordtype">int</span> ret_index;
<a name="l00410"></a>00410 nanoflann::KNNResultSet<num_t> resultSet(knn);
<a name="l00411"></a>00411 resultSet.<a class="code" href="classnanoflann_1_1_k_n_n_result_set.html#a4fefecb0ff9480ceeb1212afa67f9966">init</a>(&ret_index, &out_dist_sqr );
<a name="l00412"></a>00412
<a name="l00413"></a>00413 <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a2fe9f0119fbc4ed259f761b4cb42eba6">m_kdtree3d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#a36efc2e6a2110e60c17c527919f98a57">query_point</a>[0] = x0;
<a name="l00414"></a>00414 <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a2fe9f0119fbc4ed259f761b4cb42eba6">m_kdtree3d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#a36efc2e6a2110e60c17c527919f98a57">query_point</a>[1] = y0;
<a name="l00415"></a>00415 <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a2fe9f0119fbc4ed259f761b4cb42eba6">m_kdtree3d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#a36efc2e6a2110e60c17c527919f98a57">query_point</a>[2] = z0;
<a name="l00416"></a>00416 <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a2fe9f0119fbc4ed259f761b4cb42eba6">m_kdtree3d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#ad2ca3223a1f27615feeb5d17321d7165" title="NULL or the up-to-date index.">index</a>->findNeighbors(resultSet, &<a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a2fe9f0119fbc4ed259f761b4cb42eba6">m_kdtree3d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#a36efc2e6a2110e60c17c527919f98a57">query_point</a>[0], <a class="code" href="structnanoflann_1_1_search_params.html" title="Search options for KDTreeSingleIndexAdaptor::findNeighbors()">nanoflann::SearchParams</a>(<a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a65f5da9a222c2c7b913f7e6242c6c77e" title="Parameters to tune the ANN searches.">kdtree_search_params</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_search_params.html#a54ba56007b68b4fa6a2a2b1018ca9935" title="The number of checks for ANN (default: 32) - corresponds to FLANN's SearchParams::check.">nChecks</a>));
<a name="l00417"></a>00417
<a name="l00418"></a>00418 <span class="comment">// Copy output to user vars:</span>
<a name="l00419"></a>00419 out_x = <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a1f2be858dfb40e06cbae68375c98941b" title="CRTP helper method.">derived</a>().kdtree_get_pt(ret_index,0);
<a name="l00420"></a>00420 out_y = <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a1f2be858dfb40e06cbae68375c98941b" title="CRTP helper method.">derived</a>().kdtree_get_pt(ret_index,1);
<a name="l00421"></a>00421 out_z = <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a1f2be858dfb40e06cbae68375c98941b" title="CRTP helper method.">derived</a>().kdtree_get_pt(ret_index,2);
<a name="l00422"></a>00422
<a name="l00423"></a>00423 <span class="keywordflow">return</span> <span class="keyword">static_cast<</span><span class="keywordtype">size_t</span><span class="keyword">></span>(ret_index);
<a name="l00424"></a>00424 <a class="code" href="mrpt__macros_8h.html#a88a917260793b56abd83ad2a0d849eb1">MRPT_END</a>
<a name="l00425"></a>00425 }
<a name="l00426"></a>00426 <span class="comment"></span>
<a name="l00427"></a>00427 <span class="comment"> /// \overload</span>
<a name="l00428"></a><a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a44afd69e49d989baa2973ffd7f7037e6">00428</a> <span class="comment"></span> <span class="keyword">inline</span> <span class="keywordtype">size_t</span> <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a89eab3ed58a61ba1dd561552562a1215" title="KD Tree-based search for the closest point (only ONE) to some given 3D coordinates.">kdTreeClosestPoint3D</a>(
<a name="l00429"></a>00429 <span class="keywordtype">float</span> x0,
<a name="l00430"></a>00430 <span class="keywordtype">float</span> y0,
<a name="l00431"></a>00431 <span class="keywordtype">float</span> z0,
<a name="l00432"></a>00432 <span class="keywordtype">float</span> &out_dist_sqr
<a name="l00433"></a>00433 )<span class="keyword"> const</span>
<a name="l00434"></a>00434 <span class="keyword"> </span>{
<a name="l00435"></a>00435 <a class="code" href="mrpt__macros_8h.html#a45b840af519f33816311acdbb28d7c10">MRPT_START</a>
<a name="l00436"></a>00436 <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a71540b68861ea0afaea0b0dd24bd7c14" title="Rebuild, if needed the KD-tree for 2D (nDims=2), 3D (nDims=3), ... asking the child class for the dat...">rebuild_kdTree_3D</a>(); <span class="comment">// First: Create the 3D KD-Tree if required</span>
<a name="l00437"></a>00437 <span class="keywordflow">if</span> ( !<a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a2fe9f0119fbc4ed259f761b4cb42eba6">m_kdtree3d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#a524ecb048be5467a306d5cbbd2a589a2">m_num_points</a> ) <a class="code" href="mrpt__macros_8h.html#aaa3f404ea85a6575a7139f8d101370ba">THROW_EXCEPTION</a>(<span class="stringliteral">"There are no points in the KD-tree."</span>)
<a name="l00438"></a>00438
<a name="l00439"></a>00439 <span class="keyword">const</span> <span class="keywordtype">int</span> knn = 1; <span class="comment">// Number of points to retrieve</span>
<a name="l00440"></a>00440 <span class="keywordtype">int</span> ret_index;
<a name="l00441"></a>00441 nanoflann::KNNResultSet<num_t> resultSet(knn);
<a name="l00442"></a>00442 resultSet.<a class="code" href="classnanoflann_1_1_k_n_n_result_set.html#a4fefecb0ff9480ceeb1212afa67f9966">init</a>(&ret_index, &out_dist_sqr );
<a name="l00443"></a>00443
<a name="l00444"></a>00444 <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a2fe9f0119fbc4ed259f761b4cb42eba6">m_kdtree3d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#a36efc2e6a2110e60c17c527919f98a57">query_point</a>[0] = x0;
<a name="l00445"></a>00445 <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a2fe9f0119fbc4ed259f761b4cb42eba6">m_kdtree3d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#a36efc2e6a2110e60c17c527919f98a57">query_point</a>[1] = y0;
<a name="l00446"></a>00446 <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a2fe9f0119fbc4ed259f761b4cb42eba6">m_kdtree3d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#a36efc2e6a2110e60c17c527919f98a57">query_point</a>[2] = z0;
<a name="l00447"></a>00447 <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a2fe9f0119fbc4ed259f761b4cb42eba6">m_kdtree3d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#ad2ca3223a1f27615feeb5d17321d7165" title="NULL or the up-to-date index.">index</a>->findNeighbors(resultSet, &<a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a2fe9f0119fbc4ed259f761b4cb42eba6">m_kdtree3d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#a36efc2e6a2110e60c17c527919f98a57">query_point</a>[0], <a class="code" href="structnanoflann_1_1_search_params.html" title="Search options for KDTreeSingleIndexAdaptor::findNeighbors()">nanoflann::SearchParams</a>(<a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a65f5da9a222c2c7b913f7e6242c6c77e" title="Parameters to tune the ANN searches.">kdtree_search_params</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_search_params.html#a54ba56007b68b4fa6a2a2b1018ca9935" title="The number of checks for ANN (default: 32) - corresponds to FLANN's SearchParams::check.">nChecks</a>));
<a name="l00448"></a>00448
<a name="l00449"></a>00449 <span class="keywordflow">return</span> <span class="keyword">static_cast<</span><span class="keywordtype">size_t</span><span class="keyword">></span>(ret_index);
<a name="l00450"></a>00450 <a class="code" href="mrpt__macros_8h.html#a88a917260793b56abd83ad2a0d849eb1">MRPT_END</a>
<a name="l00451"></a>00451 }
<a name="l00452"></a>00452 <span class="comment"></span>
<a name="l00453"></a>00453 <span class="comment"> /// \overload</span>
<a name="l00454"></a><a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a2f18fd292f21f156b9ac5ae3503f44aa">00454</a> <span class="comment"></span> <span class="keyword">inline</span> <span class="keywordtype">size_t</span> <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a2f18fd292f21f156b9ac5ae3503f44aa">kdTreeClosestPoint3D</a>(<span class="keyword">const</span> <a class="code" href="structmrpt_1_1math_1_1_t_point3_d.html" title="Lightweight 3D point.">TPoint3D</a> &p0,<a class="code" href="structmrpt_1_1math_1_1_t_point3_d.html" title="Lightweight 3D point.">TPoint3D</a> &pOut,<span class="keywordtype">float</span> &outDistSqr)<span class="keyword"> const </span>{
<a name="l00455"></a>00455 <span class="keywordtype">float</span> dmy1,dmy2,dmy3;
<a name="l00456"></a>00456 <span class="keywordtype">size_t</span> res=<a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a89eab3ed58a61ba1dd561552562a1215" title="KD Tree-based search for the closest point (only ONE) to some given 3D coordinates.">kdTreeClosestPoint3D</a>(static_cast<float>(p0.<a class="code" href="structmrpt_1_1math_1_1_t_point3_d.html#a5014ee49d97866d293568300b619a7e2" title="X coordinate.">x</a>),static_cast<float>(p0.<a class="code" href="structmrpt_1_1math_1_1_t_point3_d.html#aec879c0d61d8446e93b7d09344931d37" title="Y coordinate.">y</a>),static_cast<float>(p0.<a class="code" href="structmrpt_1_1math_1_1_t_point3_d.html#a61df04839b9146696e696fc7af3bc307" title="Z coordinate.">z</a>),dmy1,dmy2,dmy3,outDistSqr);
<a name="l00457"></a>00457 pOut.<a class="code" href="structmrpt_1_1math_1_1_t_point3_d.html#a5014ee49d97866d293568300b619a7e2" title="X coordinate.">x</a>=<span class="keyword">static_cast<</span><span class="keywordtype">double</span><span class="keyword">></span>(dmy1);
<a name="l00458"></a>00458 pOut.<a class="code" href="structmrpt_1_1math_1_1_t_point3_d.html#aec879c0d61d8446e93b7d09344931d37" title="Y coordinate.">y</a>=<span class="keyword">static_cast<</span><span class="keywordtype">double</span><span class="keyword">></span>(dmy2);
<a name="l00459"></a>00459 pOut.<a class="code" href="structmrpt_1_1math_1_1_t_point3_d.html#a61df04839b9146696e696fc7af3bc307" title="Z coordinate.">z</a>=<span class="keyword">static_cast<</span><span class="keywordtype">double</span><span class="keyword">></span>(dmy3);
<a name="l00460"></a>00460 <span class="keywordflow">return</span> res;
<a name="l00461"></a>00461 }
<a name="l00462"></a>00462 <span class="comment"></span>
<a name="l00463"></a>00463 <span class="comment"> /** KD Tree-based search for the N closest points to some given 3D coordinates.</span>
<a name="l00464"></a>00464 <span class="comment"> * This method automatically build the "m_kdtree_data" structure when:</span>
<a name="l00465"></a>00465 <span class="comment"> * - It is called for the first time</span>
<a name="l00466"></a>00466 <span class="comment"> * - The map has changed</span>
<a name="l00467"></a>00467 <span class="comment"> * - The KD-tree was build for 2D.</span>
<a name="l00468"></a>00468 <span class="comment"> *</span>
<a name="l00469"></a>00469 <span class="comment"> * \param x0 The X coordinate of the query.</span>
<a name="l00470"></a>00470 <span class="comment"> * \param y0 The Y coordinate of the query.</span>
<a name="l00471"></a>00471 <span class="comment"> * \param z0 The Z coordinate of the query.</span>
<a name="l00472"></a>00472 <span class="comment"> * \param N The number of closest points to search.</span>
<a name="l00473"></a>00473 <span class="comment"> * \param out_x The vector containing the X coordinates of the correspondences.</span>
<a name="l00474"></a>00474 <span class="comment"> * \param out_y The vector containing the Y coordinates of the correspondences.</span>
<a name="l00475"></a>00475 <span class="comment"> * \param out_z The vector containing the Z coordinates of the correspondences.</span>
<a name="l00476"></a>00476 <span class="comment"> * \param out_dist_sqr The vector containing the square distance between the query and the returned points.</span>
<a name="l00477"></a>00477 <span class="comment"> *</span>
<a name="l00478"></a>00478 <span class="comment"> * \sa kdTreeNClosestPoint2D</span>
<a name="l00479"></a>00479 <span class="comment"> */</span>
<a name="l00480"></a><a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a9fd5af04995704c55b1a17f4515da803">00480</a> <span class="keyword">inline</span> <span class="keywordtype">void</span> <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a9fd5af04995704c55b1a17f4515da803" title="KD Tree-based search for the N closest points to some given 3D coordinates.">kdTreeNClosestPoint3D</a>(
<a name="l00481"></a>00481 <span class="keywordtype">float</span> x0,
<a name="l00482"></a>00482 <span class="keywordtype">float</span> y0,
<a name="l00483"></a>00483 <span class="keywordtype">float</span> z0,
<a name="l00484"></a>00484 <span class="keywordtype">size_t</span> knn,
<a name="l00485"></a>00485 <a class="code" href="classstd_1_1vector.html">std::vector<float></a> &out_x,
<a name="l00486"></a>00486 <a class="code" href="classstd_1_1vector.html">std::vector<float></a> &out_y,
<a name="l00487"></a>00487 <a class="code" href="classstd_1_1vector.html">std::vector<float></a> &out_z,
<a name="l00488"></a>00488 <a class="code" href="classstd_1_1vector.html">std::vector<float></a> &out_dist_sqr )<span class="keyword"> const</span>
<a name="l00489"></a>00489 <span class="keyword"> </span>{
<a name="l00490"></a>00490 <a class="code" href="mrpt__macros_8h.html#a45b840af519f33816311acdbb28d7c10">MRPT_START</a>
<a name="l00491"></a>00491 <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a71540b68861ea0afaea0b0dd24bd7c14" title="Rebuild, if needed the KD-tree for 2D (nDims=2), 3D (nDims=3), ... asking the child class for the dat...">rebuild_kdTree_3D</a>(); <span class="comment">// First: Create the 3D KD-Tree if required</span>
<a name="l00492"></a>00492 <span class="keywordflow">if</span> ( !<a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a2fe9f0119fbc4ed259f761b4cb42eba6">m_kdtree3d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#a524ecb048be5467a306d5cbbd2a589a2">m_num_points</a> ) <a class="code" href="mrpt__macros_8h.html#aaa3f404ea85a6575a7139f8d101370ba">THROW_EXCEPTION</a>(<span class="stringliteral">"There are no points in the KD-tree."</span>)
<a name="l00493"></a>00493
<a name="l00494"></a>00494 std<a class="code" href="classstd_1_1vector.html">::vector<int></a> ret_indexes(knn);
<a name="l00495"></a>00495 out_x.resize(knn);
<a name="l00496"></a>00496 out_y.resize(knn);
<a name="l00497"></a>00497 out_z.resize(knn);
<a name="l00498"></a>00498 out_dist_sqr.resize(knn);
<a name="l00499"></a>00499
<a name="l00500"></a>00500 nanoflann::KNNResultSet<num_t> resultSet(knn);
<a name="l00501"></a>00501 resultSet.<a class="code" href="classnanoflann_1_1_k_n_n_result_set.html#a4fefecb0ff9480ceeb1212afa67f9966">init</a>(&ret_indexes[0], &out_dist_sqr[0] );
<a name="l00502"></a>00502
<a name="l00503"></a>00503 <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a2fe9f0119fbc4ed259f761b4cb42eba6">m_kdtree3d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#a36efc2e6a2110e60c17c527919f98a57">query_point</a>[0] = x0;
<a name="l00504"></a>00504 <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a2fe9f0119fbc4ed259f761b4cb42eba6">m_kdtree3d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#a36efc2e6a2110e60c17c527919f98a57">query_point</a>[1] = y0;
<a name="l00505"></a>00505 <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a2fe9f0119fbc4ed259f761b4cb42eba6">m_kdtree3d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#a36efc2e6a2110e60c17c527919f98a57">query_point</a>[2] = z0;
<a name="l00506"></a>00506 <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a2fe9f0119fbc4ed259f761b4cb42eba6">m_kdtree3d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#ad2ca3223a1f27615feeb5d17321d7165" title="NULL or the up-to-date index.">index</a>->findNeighbors(resultSet, &<a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a2fe9f0119fbc4ed259f761b4cb42eba6">m_kdtree3d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#a36efc2e6a2110e60c17c527919f98a57">query_point</a>[0], <a class="code" href="structnanoflann_1_1_search_params.html" title="Search options for KDTreeSingleIndexAdaptor::findNeighbors()">nanoflann::SearchParams</a>(<a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a65f5da9a222c2c7b913f7e6242c6c77e" title="Parameters to tune the ANN searches.">kdtree_search_params</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_search_params.html#a54ba56007b68b4fa6a2a2b1018ca9935" title="The number of checks for ANN (default: 32) - corresponds to FLANN's SearchParams::check.">nChecks</a>));
<a name="l00507"></a>00507
<a name="l00508"></a>00508 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i=0;i<knn;i++)
<a name="l00509"></a>00509 {
<a name="l00510"></a>00510 out_x[i] = <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a1f2be858dfb40e06cbae68375c98941b" title="CRTP helper method.">derived</a>().kdtree_get_pt(ret_indexes[i],0);
<a name="l00511"></a>00511 out_y[i] = <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a1f2be858dfb40e06cbae68375c98941b" title="CRTP helper method.">derived</a>().kdtree_get_pt(ret_indexes[i],1);
<a name="l00512"></a>00512 out_z[i] = <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a1f2be858dfb40e06cbae68375c98941b" title="CRTP helper method.">derived</a>().kdtree_get_pt(ret_indexes[i],2);
<a name="l00513"></a>00513 }
<a name="l00514"></a>00514 <a class="code" href="mrpt__macros_8h.html#a88a917260793b56abd83ad2a0d849eb1">MRPT_END</a>
<a name="l00515"></a>00515 }
<a name="l00516"></a>00516
<a name="l00517"></a><a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a61a35dc755ae1225f6d09e6d81eefb05">00517</a> <span class="keyword">inline</span> <span class="keywordtype">void</span> <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a61a35dc755ae1225f6d09e6d81eefb05">kdTreeNClosestPoint3D</a>(<span class="keyword">const</span> <a class="code" href="structmrpt_1_1math_1_1_t_point3_d.html" title="Lightweight 3D point.">TPoint3D</a> &p0,<span class="keywordtype">size_t</span> N,<a class="code" href="classstd_1_1vector.html">std::vector<TPoint3D></a> &pOut,<a class="code" href="classstd_1_1vector.html">std::vector<float></a> &outDistSqr)<span class="keyword"> const </span>{
<a name="l00518"></a>00518 std<a class="code" href="classstd_1_1vector.html">::vector<float></a> dmy1,dmy2,dmy3;
<a name="l00519"></a>00519 <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a9fd5af04995704c55b1a17f4515da803" title="KD Tree-based search for the N closest points to some given 3D coordinates.">kdTreeNClosestPoint3D</a>(static_cast<float>(p0.<a class="code" href="structmrpt_1_1math_1_1_t_point3_d.html#a5014ee49d97866d293568300b619a7e2" title="X coordinate.">x</a>),static_cast<float>(p0.<a class="code" href="structmrpt_1_1math_1_1_t_point3_d.html#aec879c0d61d8446e93b7d09344931d37" title="Y coordinate.">y</a>),static_cast<float>(p0.<a class="code" href="structmrpt_1_1math_1_1_t_point3_d.html#a61df04839b9146696e696fc7af3bc307" title="Z coordinate.">z</a>),N,dmy1,dmy2,dmy3,outDistSqr);
<a name="l00520"></a>00520 pOut.resize(dmy1.size());
<a name="l00521"></a>00521 <span class="keywordflow">for</span> (<span class="keywordtype">size_t</span> i=0;i<dmy1.size();i++) {
<a name="l00522"></a>00522 pOut[i].x=<span class="keyword">static_cast<</span><span class="keywordtype">double</span><span class="keyword">></span>(dmy1[i]);
<a name="l00523"></a>00523 pOut[i].y=<span class="keyword">static_cast<</span><span class="keywordtype">double</span><span class="keyword">></span>(dmy2[i]);
<a name="l00524"></a>00524 pOut[i].z=<span class="keyword">static_cast<</span><span class="keywordtype">double</span><span class="keyword">></span>(dmy3[i]);
<a name="l00525"></a>00525 }
<a name="l00526"></a>00526 }
<a name="l00527"></a>00527 <span class="comment"></span>
<a name="l00528"></a>00528 <span class="comment"> /** KD Tree-based search for the N closest point to some given 3D coordinates and returns their indexes.</span>
<a name="l00529"></a>00529 <span class="comment"> * This method automatically build the "m_kdtree_data" structure when:</span>
<a name="l00530"></a>00530 <span class="comment"> * - It is called for the first time</span>
<a name="l00531"></a>00531 <span class="comment"> * - The map has changed</span>
<a name="l00532"></a>00532 <span class="comment"> * - The KD-tree was build for 2D.</span>
<a name="l00533"></a>00533 <span class="comment"> *</span>
<a name="l00534"></a>00534 <span class="comment"> * \param x0 The X coordinate of the query.</span>
<a name="l00535"></a>00535 <span class="comment"> * \param y0 The Y coordinate of the query.</span>
<a name="l00536"></a>00536 <span class="comment"> * \param z0 The Z coordinate of the query.</span>
<a name="l00537"></a>00537 <span class="comment"> * \param N The number of closest points to search.</span>
<a name="l00538"></a>00538 <span class="comment"> * \param out_idx The indexes of the found closest correspondence.</span>
<a name="l00539"></a>00539 <span class="comment"> * \param out_dist_sqr The square distance between the query and the returned point.</span>
<a name="l00540"></a>00540 <span class="comment"> *</span>
<a name="l00541"></a>00541 <span class="comment"> * \sa kdTreeClosestPoint2D</span>
<a name="l00542"></a>00542 <span class="comment"> */</span>
<a name="l00543"></a><a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a343546d3f23d9c1df671c4d8c5c9eb39">00543</a> <span class="keyword">inline</span> <span class="keywordtype">void</span> <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a343546d3f23d9c1df671c4d8c5c9eb39" title="KD Tree-based search for the N closest point to some given 3D coordinates and returns their indexes...">kdTreeNClosestPoint3DIdx</a>(
<a name="l00544"></a>00544 <span class="keywordtype">float</span> x0,
<a name="l00545"></a>00545 <span class="keywordtype">float</span> y0,
<a name="l00546"></a>00546 <span class="keywordtype">float</span> z0,
<a name="l00547"></a>00547 <span class="keywordtype">size_t</span> knn,
<a name="l00548"></a>00548 <a class="code" href="classstd_1_1vector.html">std::vector<int></a> &out_idx,
<a name="l00549"></a>00549 <a class="code" href="classstd_1_1vector.html">std::vector<float></a> &out_dist_sqr )<span class="keyword"> const</span>
<a name="l00550"></a>00550 <span class="keyword"> </span>{
<a name="l00551"></a>00551 <a class="code" href="mrpt__macros_8h.html#a45b840af519f33816311acdbb28d7c10">MRPT_START</a>
<a name="l00552"></a>00552 <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a71540b68861ea0afaea0b0dd24bd7c14" title="Rebuild, if needed the KD-tree for 2D (nDims=2), 3D (nDims=3), ... asking the child class for the dat...">rebuild_kdTree_3D</a>(); <span class="comment">// First: Create the 3D KD-Tree if required</span>
<a name="l00553"></a>00553 <span class="keywordflow">if</span> ( !<a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a2fe9f0119fbc4ed259f761b4cb42eba6">m_kdtree3d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#a524ecb048be5467a306d5cbbd2a589a2">m_num_points</a> ) <a class="code" href="mrpt__macros_8h.html#aaa3f404ea85a6575a7139f8d101370ba">THROW_EXCEPTION</a>(<span class="stringliteral">"There are no points in the KD-tree."</span>)
<a name="l00554"></a>00554
<a name="l00555"></a>00555 out_idx.resize(knn);
<a name="l00556"></a>00556 out_dist_sqr.resize(knn);
<a name="l00557"></a>00557 nanoflann::KNNResultSet<num_t> resultSet(knn);
<a name="l00558"></a>00558 resultSet.<a class="code" href="classnanoflann_1_1_k_n_n_result_set.html#a4fefecb0ff9480ceeb1212afa67f9966">init</a>(&out_idx[0], &out_dist_sqr[0] );
<a name="l00559"></a>00559
<a name="l00560"></a>00560 <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a2fe9f0119fbc4ed259f761b4cb42eba6">m_kdtree3d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#a36efc2e6a2110e60c17c527919f98a57">query_point</a>[0] = x0;
<a name="l00561"></a>00561 <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a2fe9f0119fbc4ed259f761b4cb42eba6">m_kdtree3d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#a36efc2e6a2110e60c17c527919f98a57">query_point</a>[1] = y0;
<a name="l00562"></a>00562 <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a2fe9f0119fbc4ed259f761b4cb42eba6">m_kdtree3d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#a36efc2e6a2110e60c17c527919f98a57">query_point</a>[2] = z0;
<a name="l00563"></a>00563 <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a2fe9f0119fbc4ed259f761b4cb42eba6">m_kdtree3d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#ad2ca3223a1f27615feeb5d17321d7165" title="NULL or the up-to-date index.">index</a>->findNeighbors(resultSet, &<a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a2fe9f0119fbc4ed259f761b4cb42eba6">m_kdtree3d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#a36efc2e6a2110e60c17c527919f98a57">query_point</a>[0], <a class="code" href="structnanoflann_1_1_search_params.html" title="Search options for KDTreeSingleIndexAdaptor::findNeighbors()">nanoflann::SearchParams</a>(<a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a65f5da9a222c2c7b913f7e6242c6c77e" title="Parameters to tune the ANN searches.">kdtree_search_params</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_search_params.html#a54ba56007b68b4fa6a2a2b1018ca9935" title="The number of checks for ANN (default: 32) - corresponds to FLANN's SearchParams::check.">nChecks</a>));
<a name="l00564"></a>00564 <a class="code" href="mrpt__macros_8h.html#a88a917260793b56abd83ad2a0d849eb1">MRPT_END</a>
<a name="l00565"></a>00565 }
<a name="l00566"></a>00566
<a name="l00567"></a><a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#ab6112525867566c872402236781413e7">00567</a> <span class="keyword">inline</span> <span class="keywordtype">void</span> <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#ab6112525867566c872402236781413e7">kdTreeNClosestPoint3DIdx</a>(<span class="keyword">const</span> <a class="code" href="structmrpt_1_1math_1_1_t_point3_d.html" title="Lightweight 3D point.">TPoint3D</a> &p0,<span class="keywordtype">size_t</span> N,<a class="code" href="classstd_1_1vector.html">std::vector<int></a> &outIdx,<a class="code" href="classstd_1_1vector.html">std::vector<float></a> &outDistSqr)<span class="keyword"> const </span>{
<a name="l00568"></a>00568 <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a343546d3f23d9c1df671c4d8c5c9eb39" title="KD Tree-based search for the N closest point to some given 3D coordinates and returns their indexes...">kdTreeNClosestPoint3DIdx</a>(static_cast<float>(p0.<a class="code" href="structmrpt_1_1math_1_1_t_point3_d.html#a5014ee49d97866d293568300b619a7e2" title="X coordinate.">x</a>),static_cast<float>(p0.<a class="code" href="structmrpt_1_1math_1_1_t_point3_d.html#aec879c0d61d8446e93b7d09344931d37" title="Y coordinate.">y</a>),static_cast<float>(p0.<a class="code" href="structmrpt_1_1math_1_1_t_point3_d.html#a61df04839b9146696e696fc7af3bc307" title="Z coordinate.">z</a>),N,outIdx,outDistSqr);
<a name="l00569"></a>00569 }
<a name="l00570"></a>00570
<a name="l00571"></a>00571 <span class="comment">/* @} */</span>
<a name="l00572"></a>00572
<a name="l00573"></a>00573 <span class="keyword">protected</span>:<span class="comment"></span>
<a name="l00574"></a>00574 <span class="comment"> /** To be called by child classes when KD tree data changes. */</span>
<a name="l00575"></a><a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a257dd22c1a52d1150117195aaf416f16">00575</a> <span class="keyword">inline</span> <span class="keywordtype">void</span> <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a257dd22c1a52d1150117195aaf416f16" title="To be called by child classes when KD tree data changes.">kdtree_mark_as_outdated</a>()<span class="keyword"> const </span>{ <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#af3dfc28c75edd8ba3e8ea4b4caced0aa" title="whether the KD tree needs to be rebuilt or not.">m_kdtree_is_uptodate</a> = <span class="keyword">false</span>; }
<a name="l00576"></a>00576
<a name="l00577"></a>00577 <span class="keyword">private</span>:<span class="comment"></span>
<a name="l00578"></a>00578 <span class="comment"> /** Internal structure with the KD-tree representation (mainly used to avoid copying pointers with the = operator) */</span>
<a name="l00579"></a>00579 <span class="keyword">template</span> <<span class="keywordtype">int</span> _DIM = -1>
<a name="l00580"></a>00580 <span class="keyword">struct </span>TKDTreeDataHolder
<a name="l00581"></a>00581 {<span class="comment"></span>
<a name="l00582"></a>00582 <span class="comment"> /** Init the pointer to NULL. */</span>
<a name="l00583"></a><a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#a957554f539e1dc0a04679778d4774283">00583</a> <span class="keyword">inline</span> <a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#a957554f539e1dc0a04679778d4774283" title="Init the pointer to NULL.">TKDTreeDataHolder</a>() : <a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#ad2ca3223a1f27615feeb5d17321d7165" title="NULL or the up-to-date index.">index</a>(NULL),<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#abbf1577f90eee5d2c2cb2da7df5b4678" title="Dimensionality. typ: 2,3.">m_dim</a>(_DIM), <a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#a524ecb048be5467a306d5cbbd2a589a2">m_num_points</a>(0) { }
<a name="l00584"></a>00584 <span class="comment"></span>
<a name="l00585"></a>00585 <span class="comment"> /** Copy constructor: It actually does NOT copy the kd-tree, a new object will be created if required! */</span>
<a name="l00586"></a><a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#a6308ab85f2ea04e0685d8c6a8163da51">00586</a> <span class="keyword">inline</span> <a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#a6308ab85f2ea04e0685d8c6a8163da51" title="Copy constructor: It actually does NOT copy the kd-tree, a new object will be created if required!...">TKDTreeDataHolder</a>(<span class="keyword">const</span> <a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html" title="Internal structure with the KD-tree representation (mainly used to avoid copying pointers with the = ...">TKDTreeDataHolder</a> &o) : <a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#ad2ca3223a1f27615feeb5d17321d7165" title="NULL or the up-to-date index.">index</a>(NULL),<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#abbf1577f90eee5d2c2cb2da7df5b4678" title="Dimensionality. typ: 2,3.">m_dim</a>(_DIM), <a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#a524ecb048be5467a306d5cbbd2a589a2">m_num_points</a>(0) { }
<a name="l00587"></a>00587 <span class="comment"></span>
<a name="l00588"></a>00588 <span class="comment"> /** Copy operator: It actually does NOT copy the kd-tree, a new object will be created if required! */</span>
<a name="l00589"></a><a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#a3a47a392575533863cb9b188e96faac9">00589</a> <span class="keyword">inline</span> <a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html" title="Internal structure with the KD-tree representation (mainly used to avoid copying pointers with the = ...">TKDTreeDataHolder</a>& <a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#a3a47a392575533863cb9b188e96faac9" title="Copy operator: It actually does NOT copy the kd-tree, a new object will be created if required!...">operator =</a>(<span class="keyword">const</span> <a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html" title="Internal structure with the KD-tree representation (mainly used to avoid copying pointers with the = ...">TKDTreeDataHolder</a> &o) {
<a name="l00590"></a>00590 <span class="keywordflow">if</span> (&o!=<span class="keyword">this</span>) <a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#ac68b3c5e65eed0a0563c94e67232b629" title="Free memory (if allocated)">clear</a>();
<a name="l00591"></a>00591 <span class="keywordflow">return</span> *<span class="keyword">this</span>;
<a name="l00592"></a>00592 }
<a name="l00593"></a>00593 <span class="comment"></span>
<a name="l00594"></a>00594 <span class="comment"> /** Free memory (if allocated) */</span>
<a name="l00595"></a><a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#a3c470f12bb97e8ed7fac9ce167f26d48">00595</a> <span class="keyword">inline</span> <a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#a3c470f12bb97e8ed7fac9ce167f26d48" title="Free memory (if allocated)">~TKDTreeDataHolder</a>() { <a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#ac68b3c5e65eed0a0563c94e67232b629" title="Free memory (if allocated)">clear</a>(); }
<a name="l00596"></a>00596 <span class="comment"></span>
<a name="l00597"></a>00597 <span class="comment"> /** Free memory (if allocated) */</span>
<a name="l00598"></a><a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#ac68b3c5e65eed0a0563c94e67232b629">00598</a> <span class="keyword">inline</span> <span class="keywordtype">void</span> <a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#ac68b3c5e65eed0a0563c94e67232b629" title="Free memory (if allocated)">clear</a>() { <a class="code" href="namespacemrpt_1_1utils.html#ab93b62cba7a458f04259e1d1964cb08b" title="Calls "delete" to free an object only if the pointer is not NULL, then set the pointer to NULL...">mrpt::utils::delete_safe</a>( <a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#ad2ca3223a1f27615feeb5d17321d7165" title="NULL or the up-to-date index.">index</a> ); }
<a name="l00599"></a>00599
<a name="l00600"></a><a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#aea5bc99273fe5b7598aea068e884e035">00600</a> <span class="keyword">typedef</span> nanoflann::KDTreeSingleIndexAdaptor<metric_t,Derived, _DIM> <a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#aea5bc99273fe5b7598aea068e884e035">kdtree_index_t</a>;
<a name="l00601"></a>00601
<a name="l00602"></a><a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#ad2ca3223a1f27615feeb5d17321d7165">00602</a> <a class="code" href="classnanoflann_1_1_k_d_tree_single_index_adaptor.html" title="kd-tree index">kdtree_index_t</a> *<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#ad2ca3223a1f27615feeb5d17321d7165" title="NULL or the up-to-date index.">index</a>; <span class="comment">//!< NULL or the up-to-date index</span>
<a name="l00603"></a>00603 <span class="comment"></span>
<a name="l00604"></a><a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#a36efc2e6a2110e60c17c527919f98a57">00604</a> std<a class="code" href="classstd_1_1vector.html">::vector<num_t></a> <a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#a36efc2e6a2110e60c17c527919f98a57">query_point</a>;
<a name="l00605"></a><a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#abbf1577f90eee5d2c2cb2da7df5b4678">00605</a> <span class="keywordtype">size_t</span> <a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#abbf1577f90eee5d2c2cb2da7df5b4678" title="Dimensionality. typ: 2,3.">m_dim</a>; <span class="comment">//!< Dimensionality. typ: 2,3</span>
<a name="l00606"></a><a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#a524ecb048be5467a306d5cbbd2a589a2">00606</a> <span class="comment"></span> <span class="keywordtype">size_t</span> <a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#a524ecb048be5467a306d5cbbd2a589a2">m_num_points</a>;
<a name="l00607"></a>00607 };
<a name="l00608"></a>00608
<a name="l00609"></a><a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a3abb2bf223afb15c769598d0ef65bcd9">00609</a> <span class="keyword">mutable</span> TKDTreeDataHolder<2> <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a3abb2bf223afb15c769598d0ef65bcd9">m_kdtree2d_data</a>;
<a name="l00610"></a><a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a2fe9f0119fbc4ed259f761b4cb42eba6">00610</a> <span class="keyword">mutable</span> TKDTreeDataHolder<3> <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a2fe9f0119fbc4ed259f761b4cb42eba6">m_kdtree3d_data</a>;
<a name="l00611"></a><a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a18737e23301e1deae98c33adb52566fc">00611</a> <span class="keyword">mutable</span> TKDTreeDataHolder<> <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a18737e23301e1deae98c33adb52566fc">m_kdtreeNd_data</a>;
<a name="l00612"></a><a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#af3dfc28c75edd8ba3e8ea4b4caced0aa">00612</a> <span class="keyword">mutable</span> <span class="keywordtype">bool</span> <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#af3dfc28c75edd8ba3e8ea4b4caced0aa" title="whether the KD tree needs to be rebuilt or not.">m_kdtree_is_uptodate</a>; <span class="comment">//!< whether the KD tree needs to be rebuilt or not.</span>
<a name="l00613"></a>00613 <span class="comment"></span><span class="comment"></span>
<a name="l00614"></a>00614 <span class="comment"> /// Rebuild, if needed the KD-tree for 2D (nDims=2), 3D (nDims=3), ... asking the child class for the data points.</span>
<a name="l00615"></a><a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#ac0bd164a47e9c98506a301d2146b316e">00615</a> <span class="comment"></span> <span class="keywordtype">void</span> <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#ac0bd164a47e9c98506a301d2146b316e" title="Rebuild, if needed the KD-tree for 2D (nDims=2), 3D (nDims=3), ... asking the child class for the dat...">rebuild_kdTree_2D</a>()<span class="keyword"> const</span>
<a name="l00616"></a>00616 <span class="keyword"> </span>{
<a name="l00617"></a>00617 <span class="keyword">typedef</span> <span class="keyword">typename</span> <a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#aea5bc99273fe5b7598aea068e884e035">TKDTreeDataHolder<2>::kdtree_index_t</a> tree2d_t;
<a name="l00618"></a>00618
<a name="l00619"></a>00619 <span class="keywordflow">if</span> (!<a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#af3dfc28c75edd8ba3e8ea4b4caced0aa" title="whether the KD tree needs to be rebuilt or not.">m_kdtree_is_uptodate</a>) { <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a3abb2bf223afb15c769598d0ef65bcd9">m_kdtree2d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#ac68b3c5e65eed0a0563c94e67232b629" title="Free memory (if allocated)">clear</a>(); <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a2fe9f0119fbc4ed259f761b4cb42eba6">m_kdtree3d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#ac68b3c5e65eed0a0563c94e67232b629" title="Free memory (if allocated)">clear</a>(); <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a18737e23301e1deae98c33adb52566fc">m_kdtreeNd_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#ac68b3c5e65eed0a0563c94e67232b629" title="Free memory (if allocated)">clear</a>(); }
<a name="l00620"></a>00620
<a name="l00621"></a>00621 <span class="keywordflow">if</span> (!<a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a3abb2bf223afb15c769598d0ef65bcd9">m_kdtree2d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#ad2ca3223a1f27615feeb5d17321d7165" title="NULL or the up-to-date index.">index</a>)
<a name="l00622"></a>00622 {
<a name="l00623"></a>00623 <span class="comment">// Erase previous tree:</span>
<a name="l00624"></a>00624 <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a3abb2bf223afb15c769598d0ef65bcd9">m_kdtree2d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#ac68b3c5e65eed0a0563c94e67232b629" title="Free memory (if allocated)">clear</a>();
<a name="l00625"></a>00625 <span class="comment">// And build new index:</span>
<a name="l00626"></a>00626 <span class="keyword">const</span> <span class="keywordtype">size_t</span> N = <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a1f2be858dfb40e06cbae68375c98941b" title="CRTP helper method.">derived</a>().kdtree_get_point_count();
<a name="l00627"></a>00627 <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a3abb2bf223afb15c769598d0ef65bcd9">m_kdtree2d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#a524ecb048be5467a306d5cbbd2a589a2">m_num_points</a> = N;
<a name="l00628"></a>00628 <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a3abb2bf223afb15c769598d0ef65bcd9">m_kdtree2d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#abbf1577f90eee5d2c2cb2da7df5b4678" title="Dimensionality. typ: 2,3.">m_dim</a> = 2;
<a name="l00629"></a>00629 <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a3abb2bf223afb15c769598d0ef65bcd9">m_kdtree2d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#a36efc2e6a2110e60c17c527919f98a57">query_point</a>.resize(2);
<a name="l00630"></a>00630 <span class="keywordflow">if</span> (N)
<a name="l00631"></a>00631 {
<a name="l00632"></a>00632 <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a3abb2bf223afb15c769598d0ef65bcd9">m_kdtree2d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#ad2ca3223a1f27615feeb5d17321d7165" title="NULL or the up-to-date index.">index</a> = <span class="keyword">new</span> tree2d_t(2, <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a1f2be858dfb40e06cbae68375c98941b" title="CRTP helper method.">derived</a>(), <a class="code" href="structnanoflann_1_1_k_d_tree_single_index_adaptor_params.html" title="Parameters.">nanoflann::KDTreeSingleIndexAdaptorParams</a>(<a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a65f5da9a222c2c7b913f7e6242c6c77e" title="Parameters to tune the ANN searches.">kdtree_search_params</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_search_params.html#a3ef8cecc39f4227d58ac7955e5c65478" title="Max points per leaf.">leaf_max_size</a>, 2 ) );
<a name="l00633"></a>00633 <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a3abb2bf223afb15c769598d0ef65bcd9">m_kdtree2d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#ad2ca3223a1f27615feeb5d17321d7165" title="NULL or the up-to-date index.">index</a>->buildIndex();
<a name="l00634"></a>00634 }
<a name="l00635"></a>00635 <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#af3dfc28c75edd8ba3e8ea4b4caced0aa" title="whether the KD tree needs to be rebuilt or not.">m_kdtree_is_uptodate</a> = <span class="keyword">true</span>;
<a name="l00636"></a>00636 }
<a name="l00637"></a>00637 }
<a name="l00638"></a>00638 <span class="comment"></span>
<a name="l00639"></a>00639 <span class="comment"> /// Rebuild, if needed the KD-tree for 2D (nDims=2), 3D (nDims=3), ... asking the child class for the data points.</span>
<a name="l00640"></a><a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a71540b68861ea0afaea0b0dd24bd7c14">00640</a> <span class="comment"></span> <span class="keywordtype">void</span> <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a71540b68861ea0afaea0b0dd24bd7c14" title="Rebuild, if needed the KD-tree for 2D (nDims=2), 3D (nDims=3), ... asking the child class for the dat...">rebuild_kdTree_3D</a>()<span class="keyword"> const</span>
<a name="l00641"></a>00641 <span class="keyword"> </span>{
<a name="l00642"></a>00642 <span class="keyword">typedef</span> <span class="keyword">typename</span> <a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#aea5bc99273fe5b7598aea068e884e035">TKDTreeDataHolder<3>::kdtree_index_t</a> tree3d_t;
<a name="l00643"></a>00643
<a name="l00644"></a>00644 <span class="keywordflow">if</span> (!<a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#af3dfc28c75edd8ba3e8ea4b4caced0aa" title="whether the KD tree needs to be rebuilt or not.">m_kdtree_is_uptodate</a>) { <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a3abb2bf223afb15c769598d0ef65bcd9">m_kdtree2d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#ac68b3c5e65eed0a0563c94e67232b629" title="Free memory (if allocated)">clear</a>(); <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a2fe9f0119fbc4ed259f761b4cb42eba6">m_kdtree3d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#ac68b3c5e65eed0a0563c94e67232b629" title="Free memory (if allocated)">clear</a>(); <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a18737e23301e1deae98c33adb52566fc">m_kdtreeNd_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#ac68b3c5e65eed0a0563c94e67232b629" title="Free memory (if allocated)">clear</a>(); }
<a name="l00645"></a>00645
<a name="l00646"></a>00646 <span class="keywordflow">if</span> (!<a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a2fe9f0119fbc4ed259f761b4cb42eba6">m_kdtree3d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#ad2ca3223a1f27615feeb5d17321d7165" title="NULL or the up-to-date index.">index</a>)
<a name="l00647"></a>00647 {
<a name="l00648"></a>00648 <span class="comment">// Erase previous tree:</span>
<a name="l00649"></a>00649 <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a2fe9f0119fbc4ed259f761b4cb42eba6">m_kdtree3d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#ac68b3c5e65eed0a0563c94e67232b629" title="Free memory (if allocated)">clear</a>();
<a name="l00650"></a>00650 <span class="comment">// And build new index:</span>
<a name="l00651"></a>00651 <span class="keyword">const</span> <span class="keywordtype">size_t</span> N = <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a1f2be858dfb40e06cbae68375c98941b" title="CRTP helper method.">derived</a>().kdtree_get_point_count();
<a name="l00652"></a>00652 <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a2fe9f0119fbc4ed259f761b4cb42eba6">m_kdtree3d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#a524ecb048be5467a306d5cbbd2a589a2">m_num_points</a> = N;
<a name="l00653"></a>00653 <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a2fe9f0119fbc4ed259f761b4cb42eba6">m_kdtree3d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#abbf1577f90eee5d2c2cb2da7df5b4678" title="Dimensionality. typ: 2,3.">m_dim</a> = 3;
<a name="l00654"></a>00654 <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a2fe9f0119fbc4ed259f761b4cb42eba6">m_kdtree3d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#a36efc2e6a2110e60c17c527919f98a57">query_point</a>.resize(3);
<a name="l00655"></a>00655 <span class="keywordflow">if</span> (N)
<a name="l00656"></a>00656 {
<a name="l00657"></a>00657 <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a2fe9f0119fbc4ed259f761b4cb42eba6">m_kdtree3d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#ad2ca3223a1f27615feeb5d17321d7165" title="NULL or the up-to-date index.">index</a> = <span class="keyword">new</span> tree3d_t(3, <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a1f2be858dfb40e06cbae68375c98941b" title="CRTP helper method.">derived</a>(), <a class="code" href="structnanoflann_1_1_k_d_tree_single_index_adaptor_params.html" title="Parameters.">nanoflann::KDTreeSingleIndexAdaptorParams</a>(<a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a65f5da9a222c2c7b913f7e6242c6c77e" title="Parameters to tune the ANN searches.">kdtree_search_params</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_search_params.html#a3ef8cecc39f4227d58ac7955e5c65478" title="Max points per leaf.">leaf_max_size</a>, 3 ) );
<a name="l00658"></a>00658 <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a2fe9f0119fbc4ed259f761b4cb42eba6">m_kdtree3d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#ad2ca3223a1f27615feeb5d17321d7165" title="NULL or the up-to-date index.">index</a>->buildIndex();
<a name="l00659"></a>00659 }
<a name="l00660"></a>00660 <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#af3dfc28c75edd8ba3e8ea4b4caced0aa" title="whether the KD tree needs to be rebuilt or not.">m_kdtree_is_uptodate</a> = <span class="keyword">true</span>;
<a name="l00661"></a>00661 }
<a name="l00662"></a>00662 }
<a name="l00663"></a>00663 <span class="comment"></span>
<a name="l00664"></a>00664 <span class="comment"> /// Rebuild, if needed the KD-tree for 2D (nDims=2), 3D (nDims=3), ... asking the child class for the data points.</span>
<a name="l00665"></a><a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a02359aa8c2c687a9195414692d8f7036">00665</a> <span class="comment"></span> <span class="keywordtype">void</span> <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a02359aa8c2c687a9195414692d8f7036" title="Rebuild, if needed the KD-tree for 2D (nDims=2), 3D (nDims=3), ... asking the child class for the dat...">rebuild_kdTree_Nd</a>(<span class="keyword">const</span> <span class="keywordtype">size_t</span> nDims)<span class="keyword"> const</span>
<a name="l00666"></a>00666 <span class="keyword"> </span>{
<a name="l00667"></a>00667 <span class="keyword">typedef</span> <span class="keyword">typename</span> TKDTreeDataHolder<>::kdtree_index_t treeNd_t;
<a name="l00668"></a>00668
<a name="l00669"></a>00669 <span class="keywordflow">if</span> (!<a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#af3dfc28c75edd8ba3e8ea4b4caced0aa" title="whether the KD tree needs to be rebuilt or not.">m_kdtree_is_uptodate</a>) { <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a3abb2bf223afb15c769598d0ef65bcd9">m_kdtree2d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#ac68b3c5e65eed0a0563c94e67232b629" title="Free memory (if allocated)">clear</a>(); <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a2fe9f0119fbc4ed259f761b4cb42eba6">m_kdtree3d_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#ac68b3c5e65eed0a0563c94e67232b629" title="Free memory (if allocated)">clear</a>(); <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a18737e23301e1deae98c33adb52566fc">m_kdtreeNd_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#ac68b3c5e65eed0a0563c94e67232b629" title="Free memory (if allocated)">clear</a>(); }
<a name="l00670"></a>00670
<a name="l00671"></a>00671 <span class="keywordflow">if</span> (!<a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a18737e23301e1deae98c33adb52566fc">m_kdtreeNd_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#ad2ca3223a1f27615feeb5d17321d7165" title="NULL or the up-to-date index.">index</a> || <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a18737e23301e1deae98c33adb52566fc">m_kdtreeNd_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#abbf1577f90eee5d2c2cb2da7df5b4678" title="Dimensionality. typ: 2,3.">m_dim</a>!=nDims )
<a name="l00672"></a>00672 {
<a name="l00673"></a>00673 <span class="comment">// Erase previous tree:</span>
<a name="l00674"></a>00674 <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a18737e23301e1deae98c33adb52566fc">m_kdtreeNd_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#ac68b3c5e65eed0a0563c94e67232b629" title="Free memory (if allocated)">clear</a>();
<a name="l00675"></a>00675 <span class="comment">// And build new index:</span>
<a name="l00676"></a>00676 <span class="keyword">const</span> <span class="keywordtype">size_t</span> N = <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a1f2be858dfb40e06cbae68375c98941b" title="CRTP helper method.">derived</a>().kdtree_get_point_count();
<a name="l00677"></a>00677 <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a18737e23301e1deae98c33adb52566fc">m_kdtreeNd_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#a524ecb048be5467a306d5cbbd2a589a2">m_num_points</a> = N;
<a name="l00678"></a>00678 <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a18737e23301e1deae98c33adb52566fc">m_kdtreeNd_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#abbf1577f90eee5d2c2cb2da7df5b4678" title="Dimensionality. typ: 2,3.">m_dim</a> = nDims;
<a name="l00679"></a>00679 <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a18737e23301e1deae98c33adb52566fc">m_kdtreeNd_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#a36efc2e6a2110e60c17c527919f98a57">query_point</a>.resize(nDims);
<a name="l00680"></a>00680 <span class="keywordflow">if</span> (N)
<a name="l00681"></a>00681 {
<a name="l00682"></a>00682 <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a18737e23301e1deae98c33adb52566fc">m_kdtreeNd_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#ad2ca3223a1f27615feeb5d17321d7165" title="NULL or the up-to-date index.">index</a> = <span class="keyword">new</span> treeNd_t(nDims, <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a1f2be858dfb40e06cbae68375c98941b" title="CRTP helper method.">derived</a>(), <a class="code" href="structnanoflann_1_1_k_d_tree_single_index_adaptor_params.html" title="Parameters.">nanoflann::KDTreeSingleIndexAdaptorParams</a>(<a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a65f5da9a222c2c7b913f7e6242c6c77e" title="Parameters to tune the ANN searches.">kdtree_search_params</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_search_params.html#a3ef8cecc39f4227d58ac7955e5c65478" title="Max points per leaf.">leaf_max_size</a>, nDims ) );
<a name="l00683"></a>00683 <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#a18737e23301e1deae98c33adb52566fc">m_kdtreeNd_data</a>.<a class="code" href="structmrpt_1_1math_1_1_k_d_tree_capable_1_1_t_k_d_tree_data_holder.html#ad2ca3223a1f27615feeb5d17321d7165" title="NULL or the up-to-date index.">index</a>->buildIndex();
<a name="l00684"></a>00684 }
<a name="l00685"></a>00685 <a class="code" href="classmrpt_1_1math_1_1_k_d_tree_capable.html#af3dfc28c75edd8ba3e8ea4b4caced0aa" title="whether the KD tree needs to be rebuilt or not.">m_kdtree_is_uptodate</a> = <span class="keyword">true</span>;
<a name="l00686"></a>00686 }
<a name="l00687"></a>00687 } <span class="comment">// end of rebuild_kdTree</span>
<a name="l00688"></a>00688
<a name="l00689"></a>00689 }; <span class="comment">// end of KDTreeCapable</span>
<a name="l00690"></a>00690 <span class="comment"></span>
<a name="l00691"></a>00691 <span class="comment"> /** @} */</span> <span class="comment">// end of grouping</span>
<a name="l00692"></a>00692
<a name="l00693"></a>00693 } <span class="comment">// End of namespace</span>
<a name="l00694"></a>00694 } <span class="comment">// End of namespace</span>
<a name="l00695"></a>00695 <span class="preprocessor">#endif</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>
