stereo_wall_detection: kdtree_ann.cpp Source File

00001 /*
00002  * Copyright (c) 2008 Radu Bogdan Rusu <rusu -=- cs.tum.edu>
00003  *
00004  * All rights reserved.
00005  *
00006  * Redistribution and use in source and binary forms, with or without
00007  * modification, are permitted provided that the following conditions are met:
00008  *
00009  *     * Redistributions of source code must retain the above copyright
00010  *       notice, this list of conditions and the following disclaimer.
00011  *     * Redistributions in binary form must reproduce the above copyright
00012  *       notice, this list of conditions and the following disclaimer in the
00013  *       documentation and/or other materials provided with the distribution.
00014  *
00015  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00016  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00017  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00018  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00019  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00020  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00021  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00022  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00023  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00024  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00025  * POSSIBILITY OF SUCH DAMAGE.
00026  *
00027  * $Id: kdtree_ann.cpp 21050 2009-08-07 21:24:30Z jfaustwg $
00028  *
00029  */
00030 
00033 #include "stereo_wall_detection/kdtree/kdtree_ann.h"
00034 
00035 namespace cloud_kdtree
00036 {
00038 
00044   void
00045     KdTreeANN::nearestKSearch (const geometry_msgs::Point32 &p_q, int k, std::vector<int> &k_indices, std::vector<float> &k_distances)
00046   {
00047     k_indices.resize (k);
00048     k_distances.resize (k);
00049 
00050     ANNpoint p = annAllocPt (3);
00051     p[0] = p_q.x; p[1] = p_q.y; p[2] = p_q.z;
00052 
00053     m_lock_.lock ();
00054     ann_kd_tree_->annkSearch (p, k, &k_indices[0], &k_distances[0], epsilon_);
00055     m_lock_.unlock ();
00056 
00057     annDeallocPt (p);
00058     return;
00059   }
00060 
00062 
00069   void
00070     KdTreeANN::nearestKSearch (const sensor_msgs::PointCloud &points, int index, int k, std::vector<int> &k_indices, std::vector<float> &k_distances)
00071   {
00072     if (index >= (int)points.points.size ())
00073       return;
00074 
00075     k_indices.resize (k);
00076     k_distances.resize (k);
00077 
00078     ANNpoint p = annAllocPt (3);
00079     p[0] = points.points.at (index).x; p[1] = points.points.at (index).y; p[2] = points.points.at (index).z;
00080 
00081     m_lock_.lock ();
00082     ann_kd_tree_->annkSearch (p, k, &k_indices[0], &k_distances[0], epsilon_);
00083     m_lock_.unlock ();
00084 
00085     annDeallocPt (p);
00086     return;
00087   }
00088 
00090 
00097   bool
00098     KdTreeANN::radiusSearch (const geometry_msgs::Point32 &p_q, double radius, std::vector<int> &k_indices, std::vector<float> &k_distances,
00099                              int max_nn)
00100   {
00101     ANNpoint p = annAllocPt (3);
00102     p[0] = p_q.x; p[1] = p_q.y; p[2] = p_q.z;
00103     radius *= radius;
00104 
00105     m_lock_.lock ();
00106     int neighbors_in_radius_ = ann_kd_tree_->annkFRSearch (p, radius, 0, NULL, NULL, epsilon_);
00107     m_lock_.unlock ();
00108 
00109     // No neighbors found ? Return false
00110     if (neighbors_in_radius_ == 0)
00111     {
00112       annDeallocPt (p);
00113       return (false);
00114     }
00115 
00116     if (neighbors_in_radius_  > max_nn) neighbors_in_radius_ = max_nn;
00117     k_indices.resize (neighbors_in_radius_);
00118     k_distances.resize (neighbors_in_radius_);
00119 
00120     m_lock_.lock ();
00121     ann_kd_tree_->annkFRSearch (p, radius, neighbors_in_radius_, &k_indices[0], &k_distances[0], epsilon_);
00122     m_lock_.unlock ();
00123 
00124     annDeallocPt (p);
00125     return (true);
00126   }
00127 
00129 
00137   bool
00138     KdTreeANN::radiusSearch (const sensor_msgs::PointCloud &points, int index, double radius, std::vector<int> &k_indices, std::vector<float> &k_distances,
00139                              int max_nn)
00140   {
00141     ANNpoint p = annAllocPt (3);
00142     p[0] = points.points.at (index).x; p[1] = points.points.at (index).y; p[2] = points.points.at (index).z;
00143     radius *= radius;
00144 
00145     m_lock_.lock ();
00146     int neighbors_in_radius_ = ann_kd_tree_->annkFRSearch (p, radius, 0, NULL, NULL, epsilon_);
00147     m_lock_.unlock ();
00148 
00149     // No neighbors found ? Return false
00150     if (neighbors_in_radius_ == 0)
00151     {
00152       annDeallocPt (p);
00153       return (false);
00154     }
00155 
00156     if (neighbors_in_radius_  > max_nn) neighbors_in_radius_ = max_nn;
00157     k_indices.resize (neighbors_in_radius_);
00158     k_distances.resize (neighbors_in_radius_);
00159 
00160     m_lock_.lock ();
00161     ann_kd_tree_->annkFRSearch (p, radius, neighbors_in_radius_, &k_indices[0], &k_distances[0], epsilon_);
00162     m_lock_.unlock ();
00163 
00164     annDeallocPt (p);
00165     return (true);
00166   }
00167 
00169 
00173   int
00174     KdTreeANN::convertCloudToArray (const sensor_msgs::PointCloud &ros_cloud)
00175   {
00176     // No point in doing anything if the array is empty
00177     if (ros_cloud.points.size () == 0)
00178     {
00179       m_lock_.lock ();
00180       points_ = NULL;
00181       m_lock_.unlock ();
00182       return (0);
00183     }
00184 
00185     m_lock_.lock ();
00186     points_ = annAllocPts (ros_cloud.points.size (), 3);       // default number of dimensions (3 = xyz)
00187 
00188     for (unsigned int cp = 0; cp < ros_cloud.points.size (); cp++)
00189     {
00190       points_[cp][0] = ros_cloud.points[cp].x;
00191       points_[cp][1] = ros_cloud.points[cp].y;
00192       points_[cp][2] = ros_cloud.points[cp].z;
00193     }
00194     m_lock_.unlock ();
00195 
00196     return (ros_cloud.points.size ());
00197   }
00198 
00200 
00208   int
00209     KdTreeANN::convertCloudToArray (const sensor_msgs::PointCloud &ros_cloud, const std::vector<int> &indices)
00210   {
00211     // No point in doing anything if the array is empty
00212     if (ros_cloud.points.size () == 0 || indices.size () > ros_cloud.points.size ())
00213     {
00214       m_lock_.lock ();
00215       points_ = NULL;
00216       m_lock_.unlock ();
00217       return (0);
00218     }
00219 
00220     m_lock_.lock ();
00221     points_ = annAllocPts (indices.size (), 3);       // default number of dimensions (3 = xyz)
00222 
00223     for (unsigned int cp = 0; cp < indices.size (); cp++)
00224     {
00225       points_[cp][0] = ros_cloud.points[indices.at (cp)].x;
00226       points_[cp][1] = ros_cloud.points[indices.at (cp)].y;
00227       points_[cp][2] = ros_cloud.points[indices.at (cp)].z;
00228     }
00229     m_lock_.unlock ();
00230 
00231     return (indices.size ());
00232   }
00233 }