Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[13070] pkg/trunk
|
From: <ve...@us...> - 2009-03-28 00:04:27
|
Revision: 13070
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=13070&view=rev
Author: veedee
Date: 2009-03-28 00:04:21 +0000 (Sat, 28 Mar 2009)
Log Message:
-----------
FLANN skel in, changed ANN distances to floats from doubles
Modified Paths:
--------------
pkg/trunk/3rdparty/ANN/Makefile
pkg/trunk/mapping/door_handle_detector/src/geometric_helper.cpp
pkg/trunk/mapping/plug_onbase_detector/src/plug_onbase_detector.cpp
pkg/trunk/mapping/point_cloud_mapping/CMakeLists.txt
pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/kdtree/kdtree.h
pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/kdtree/kdtree_ann.h
pkg/trunk/mapping/point_cloud_mapping/src/cloud_kdtree/kdtree_ann.cpp
pkg/trunk/mapping/point_cloud_mapping/src/normal_estimation.cpp
pkg/trunk/mapping/point_cloud_mapping/test/cloud_kdtree/test_kdtree.cpp
pkg/trunk/mapping/semantic_point_annotator/src/semantic_point_annotator_omp.cpp
pkg/trunk/mapping/stereo_convex_patch_histogram/src/convex_patch_histogram.cpp
pkg/trunk/mapping/table_object_detector/src/table_object_detector.cpp
Added Paths:
-----------
pkg/trunk/3rdparty/ANN/distance.patch
pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/kdtree/kdtree_flann.h
pkg/trunk/mapping/point_cloud_mapping/src/cloud_kdtree/kdtree_flann.cpp
Modified: pkg/trunk/3rdparty/ANN/Makefile
===================================================================
--- pkg/trunk/3rdparty/ANN/Makefile 2009-03-27 23:53:28 UTC (rev 13069)
+++ pkg/trunk/3rdparty/ANN/Makefile 2009-03-28 00:04:21 UTC (rev 13070)
@@ -4,7 +4,7 @@
TARBALL_URL = http://www.cs.umd.edu/~mount/ANN/Files/1.1.1/ann_1.1.1.tar.gz
UNPACK_CMD = tar xfz
SOURCE_DIR = build/ann_1.1.1
-TARBALL_PATCH = gcc43_shared.patch fPIC.patch
+TARBALL_PATCH = gcc43_shared.patch fPIC.patch distance.patch
include $(shell rospack find mk)/download_unpack_build.mk
Added: pkg/trunk/3rdparty/ANN/distance.patch
===================================================================
--- pkg/trunk/3rdparty/ANN/distance.patch (rev 0)
+++ pkg/trunk/3rdparty/ANN/distance.patch 2009-03-28 00:04:21 UTC (rev 13070)
@@ -0,0 +1,11 @@
+--- include/ANN/ANN.h.orig 2009-03-28 00:51:04.000000000 +0100
++++ include/ANN/ANN.h 2009-03-28 00:51:24.000000000 +0100
+@@ -152,7 +152,7 @@
+ //----------------------------------------------------------------------
+
+ typedef float ANNcoord; // coordinate data type
+-typedef double ANNdist; // distance data type
++typedef float ANNdist; // distance data type
+
+ //----------------------------------------------------------------------
+ // ANNidx
Modified: pkg/trunk/mapping/door_handle_detector/src/geometric_helper.cpp
===================================================================
--- pkg/trunk/mapping/door_handle_detector/src/geometric_helper.cpp 2009-03-27 23:53:28 UTC (rev 13069)
+++ pkg/trunk/mapping/door_handle_detector/src/geometric_helper.cpp 2009-03-28 00:04:21 UTC (rev 13070)
@@ -477,7 +477,7 @@
processed.resize (nr_points, false);
vector<int> nn_indices;
- vector<double> nn_distances;
+ vector<float> nn_distances;
// Process all points in the indices vector
for (int i = 0; i < nr_points; i++)
{
@@ -656,7 +656,7 @@
// Allocate enough space for point indices
vector<vector<int> > points_k_indices (points_down.pts.size ());
- vector<double> distances;
+ vector<float> distances;
// Get the nearest neighbors for all the point indices in the bounds
for (int i = 0; i < (int)points_down.pts.size (); i++)
@@ -724,7 +724,7 @@
// Allocate enough space for point indices
vector<vector<int> > points_k_indices (point_indices.size ());
- vector<double> distances;
+ vector<float> distances;
// Get the nearest neighbors for all the point indices in the bounds
for (unsigned int i = 0; i < point_indices.size (); i++)
@@ -793,7 +793,7 @@
// Allocate enough space for point indices
vector<vector<int> > points_k_indices (nr_points);
- vector<double> distances;
+ vector<float> distances;
// Get the nearest neighbors for all the point indices in the bounds
for (int i = 0; i < nr_points; i++)
@@ -953,10 +953,10 @@
cloud_kdtree::KdTree *kdtree = new cloud_kdtree::KdTreeANN (points, indices);
// Get the nearest neighbors for all the point indices in the bounds
+ vector<float> distances;
for (unsigned int i = 0; i < cluster.size (); i++)
{
vector<int> points_k_indices;
- vector<double> distances;
kdtree->radiusSearch (points.pts[cluster.at (i)], dist_thresh, points_k_indices, distances);
// Copy the inliers
Modified: pkg/trunk/mapping/plug_onbase_detector/src/plug_onbase_detector.cpp
===================================================================
--- pkg/trunk/mapping/plug_onbase_detector/src/plug_onbase_detector.cpp 2009-03-27 23:53:28 UTC (rev 13069)
+++ pkg/trunk/mapping/plug_onbase_detector/src/plug_onbase_detector.cpp 2009-03-28 00:04:21 UTC (rev 13070)
@@ -318,7 +318,7 @@
processed.resize (indices.size (), false);
vector<int> nn_indices;
- vector<double> nn_distances;
+ vector<float> nn_distances;
// Process all points in the indices vector
for (unsigned int i = 0; i < indices.size (); i++)
{
Modified: pkg/trunk/mapping/point_cloud_mapping/CMakeLists.txt
===================================================================
--- pkg/trunk/mapping/point_cloud_mapping/CMakeLists.txt 2009-03-27 23:53:28 UTC (rev 13069)
+++ pkg/trunk/mapping/point_cloud_mapping/CMakeLists.txt 2009-03-28 00:04:21 UTC (rev 13070)
@@ -48,13 +48,9 @@
target_link_libraries (bin/test_cloud_io cloud_io)
# ---[ Cloud Kd-Tree library
-# rospack_add_library (cloud_kdtree
-# src/cloud_kdtree/convert.cpp
-# src/cloud_kdtree/search.cpp
-# )
-
rospack_add_library (cloud_kdtree
src/cloud_kdtree/kdtree_ann.cpp
+ src/cloud_kdtree/kdtree_flann.cpp
)
rospack_add_gtest (bin/test_cloud_kdtree test/cloud_kdtree/test_kdtree.cpp)
Modified: pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/kdtree/kdtree.h
===================================================================
--- pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/kdtree/kdtree.h 2009-03-27 23:53:28 UTC (rev 13069)
+++ pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/kdtree/kdtree.h 2009-03-28 00:04:21 UTC (rev 13070)
@@ -71,15 +71,15 @@
/** \brief Destructor for KdTree. Deletes all allocated data arrays and destroys the kd-tree structures. */
virtual ~KdTree () { }
- virtual void nearestKSearch (const robot_msgs::Point32 &p_q, int k, std::vector<int> &k_indices, std::vector<double> &k_distances) = 0;
- virtual void nearestKSearch (const robot_msgs::PointCloud &points, int index, int k, std::vector<int> &k_indices, std::vector<double> &k_distances) = 0;
- virtual inline void nearestKSearch (int index, int k, std::vector<int> &k_indices, std::vector<double> &k_distances) = 0;
+ virtual void nearestKSearch (const robot_msgs::Point32 &p_q, int k, std::vector<int> &k_indices, std::vector<float> &k_distances) = 0;
+ virtual void nearestKSearch (const robot_msgs::PointCloud &points, int index, int k, std::vector<int> &k_indices, std::vector<float> &k_distances) = 0;
+ virtual inline void nearestKSearch (int index, int k, std::vector<int> &k_indices, std::vector<float> &k_distances) = 0;
- virtual bool radiusSearch (const robot_msgs::Point32 &p_q, double radius, std::vector<int> &k_indices, std::vector<double> &k_distances,
+ virtual bool radiusSearch (const robot_msgs::Point32 &p_q, double radius, std::vector<int> &k_indices, std::vector<float> &k_distances,
int max_nn = INT_MAX) = 0;
- virtual bool radiusSearch (const robot_msgs::PointCloud &points, int index, double radius, std::vector<int> &k_indices, std::vector<double> &k_distances,
+ virtual bool radiusSearch (const robot_msgs::PointCloud &points, int index, double radius, std::vector<int> &k_indices, std::vector<float> &k_distances,
int max_nn = INT_MAX) = 0;
- virtual inline bool radiusSearch (int index, double radius, std::vector<int> &k_indices, std::vector<double> &k_distances,
+ virtual inline bool radiusSearch (int index, double radius, std::vector<int> &k_indices, std::vector<float> &k_distances,
int max_nn = INT_MAX) = 0;
Modified: pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/kdtree/kdtree_ann.h
===================================================================
--- pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/kdtree/kdtree_ann.h 2009-03-27 23:53:28 UTC (rev 13069)
+++ pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/kdtree/kdtree_ann.h 2009-03-28 00:04:21 UTC (rev 13070)
@@ -115,8 +115,8 @@
m_lock_.unlock ();
}
- virtual void nearestKSearch (const robot_msgs::Point32 &p_q, int k, std::vector<int> &k_indices, std::vector<double> &k_distances);
- virtual void nearestKSearch (const robot_msgs::PointCloud &points, int index, int k, std::vector<int> &k_indices, std::vector<double> &k_distances);
+ virtual void nearestKSearch (const robot_msgs::Point32 &p_q, int k, std::vector<int> &k_indices, std::vector<float> &k_distances);
+ virtual void nearestKSearch (const robot_msgs::PointCloud &points, int index, int k, std::vector<int> &k_indices, std::vector<float> &k_distances);
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** \brief Search for k-nearest neighbors for the given query point.
@@ -126,7 +126,7 @@
* \param k_distances the resultant point distances
*/
virtual inline void
- nearestKSearch (int index, int k, std::vector<int> &k_indices, std::vector<double> &k_distances)
+ nearestKSearch (int index, int k, std::vector<int> &k_indices, std::vector<float> &k_distances)
{
if (index >= nr_points_)
return;
@@ -137,8 +137,8 @@
return;
}
- virtual bool radiusSearch (const robot_msgs::Point32 &p_q, double radius, std::vector<int> &k_indices, std::vector<double> &k_distances, int max_nn = INT_MAX);
- virtual bool radiusSearch (const robot_msgs::PointCloud &points, int index, double radius, std::vector<int> &k_indices, std::vector<double> &k_distances, int max_nn = INT_MAX);
+ virtual bool radiusSearch (const robot_msgs::Point32 &p_q, double radius, std::vector<int> &k_indices, std::vector<float> &k_distances, int max_nn = INT_MAX);
+ virtual bool radiusSearch (const robot_msgs::PointCloud &points, int index, double radius, std::vector<int> &k_indices, std::vector<float> &k_distances, int max_nn = INT_MAX);
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** \brief Search for all the nearest neighbors of the query point in a given radius.
@@ -149,7 +149,7 @@
* \param max_nn if given, bounds the maximum returned neighbors to this value
*/
virtual inline bool
- radiusSearch (int index, double radius, std::vector<int> &k_indices, std::vector<double> &k_distances,
+ radiusSearch (int index, double radius, std::vector<int> &k_indices, std::vector<float> &k_distances,
int max_nn = INT_MAX)
{
radius *= radius;
Added: pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/kdtree/kdtree_flann.h
===================================================================
--- pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/kdtree/kdtree_flann.h (rev 0)
+++ pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/kdtree/kdtree_flann.h 2009-03-28 00:04:21 UTC (rev 13070)
@@ -0,0 +1,222 @@
+/*
+ * Copyright (c) 2008 Radu Bogdan Rusu <rusu -=- cs.tum.edu>
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+/** \author Radu Bogdan Rusu */
+
+#ifndef _CLOUD_KDTREE_KDTREE_FLANN_H_
+#define _CLOUD_KDTREE_KDTREE_FLANN_H_
+
+#include "point_cloud_mapping/kdtree/kdtree.h"
+
+#include <boost/thread/mutex.hpp>
+
+#include <flann.h>
+
+namespace cloud_kdtree
+{
+ class KdTreeFLANN : public KdTree
+ {
+ public:
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief Constructor for KdTree.
+ * \param points the ROS point cloud data array
+ */
+ KdTreeFLANN (const robot_msgs::PointCloud &points)
+ {
+ epsilon_ = 0.0; // default error bound value
+ dim_ = 3; // default number of dimensions (3 = xyz)
+
+ // Allocate enough data
+ nr_points_ = convertCloudToArray (points);
+ if (nr_points_ == 0)
+ {
+ ROS_ERROR ("[KdTreeFLANN] Could not create kD-tree for %d points!", nr_points_);
+ return;
+ }
+
+ // Create the kd_tree representation
+ float speedup;
+ FLANNParameters flann_param;
+ flann_param.algorithm = KDTREE;
+ flann_param.log_level = LOG_NONE;
+ flann_param.log_destination = NULL;
+
+ flann_param.checks = 32;
+ flann_param.trees = 8;
+ flann_param.branching = 32;
+ flann_param.iterations = 7;
+ flann_param.target_precision = -1;
+
+ m_lock_.lock ();
+ index_id_ = flann_build_index (points_, nr_points_, dim_, &speedup, &flann_param);
+ m_lock_.unlock ();
+ }
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief Constructor for KdTree.
+ * \note ATTENTION: This method breaks the 1-1 mapping between the indices returned using \a getNeighborsIndices
+ * and the ones from the \a points message ! When using this method, make sure to get the underlying point data
+ * using the \a getPoint method
+ * \param points the ROS point cloud data array
+ * \param indices the point cloud indices
+ */
+ KdTreeFLANN (const robot_msgs::PointCloud &points, const std::vector<int> &indices)
+ {
+ epsilon_ = 0.0; // default error bound value
+ dim_ = 3; // default number of dimensions (3 = xyz)
+
+ // Allocate enough data
+ nr_points_ = convertCloudToArray (points, indices);
+ if (nr_points_ == 0)
+ {
+ ROS_ERROR ("[KdTreeFLANN] Could not create kD-tree for %d points!", nr_points_);
+ return;
+ }
+
+ // Create the kd_tree representation
+ float speedup;
+ m_lock_.lock ();
+ flann_param_.algorithm = KDTREE;
+ flann_param_.log_level = LOG_NONE;
+ flann_param_.log_destination = NULL;
+
+ flann_param_.checks = 32;
+ flann_param_.trees = 8;
+ flann_param_.branching = 32;
+ flann_param_.iterations = 7;
+ flann_param_.target_precision = -1;
+
+ index_id_ = flann_build_index (points_, nr_points_, dim_, &speedup, &flann_param_);
+ m_lock_.unlock ();
+ }
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief Destructor for KdTree. Deletes all allocated data arrays and destroys the kd-tree structures. */
+ virtual ~KdTreeFLANN ()
+ {
+ m_lock_.lock ();
+
+ // Data array cleanup
+ if (points_ != NULL && nr_points_ != 0)
+ free (points_);
+
+ // ANN Cleanup
+ flann_free_index (index_id_, &flann_param_);
+
+ m_lock_.unlock ();
+ }
+
+ virtual void nearestKSearch (const robot_msgs::Point32 &p_q, int k, std::vector<int> &k_indices, std::vector<float> &k_distances);
+ virtual void nearestKSearch (const robot_msgs::PointCloud &points, int index, int k, std::vector<int> &k_indices, std::vector<float> &k_distances);
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief Search for k-nearest neighbors for the given query point.
+ * \param index the index in \a points representing the query point
+ * \param k the number of neighbors to search for
+ * \param k_indices the resultant point indices
+ * \param k_distances the resultant point distances
+ */
+ virtual inline void
+ nearestKSearch (int index, int k, std::vector<int> &k_indices, std::vector<float> &k_distances)
+ {
+ if (index >= nr_points_)
+ return;
+
+ m_lock_.lock ();
+ flann_find_nearest_neighbors_index (index_id_, &points_[index], 1, &k_indices[0], &k_distances[0], k, flann_param_.checks, &flann_param_);
+ m_lock_.unlock ();
+ return;
+ }
+
+ virtual bool radiusSearch (const robot_msgs::Point32 &p_q, double radius, std::vector<int> &k_indices, std::vector<float> &k_distances, int max_nn = INT_MAX);
+ virtual bool radiusSearch (const robot_msgs::PointCloud &points, int index, double radius, std::vector<int> &k_indices, std::vector<float> &k_distances, int max_nn = INT_MAX);
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief Search for all the nearest neighbors of the query point in a given radius.
+ * \param index the index in \a points representing the query point
+ * \param radius the radius of the sphere bounding all of p_q's neighbors
+ * \param k_indices the resultant point indices
+ * \param k_distances the resultant point distances
+ * \param max_nn if given, bounds the maximum returned neighbors to this value
+ */
+ virtual inline bool
+ radiusSearch (int index, double radius, std::vector<int> &k_indices, std::vector<float> &k_distances,
+ int max_nn = INT_MAX)
+ {
+ radius *= radius;
+
+ int neighbors_in_radius_;
+ m_lock_.lock ();
+ //int neighbors_in_radius_ = ann_kd_tree_->annkFRSearch (points_[index], radius, 0, NULL, NULL, epsilon_);
+ m_lock_.unlock ();
+
+ // No neighbors found ? Return false
+ if (neighbors_in_radius_ == 0)
+ return (false);
+
+ if (neighbors_in_radius_ > max_nn) neighbors_in_radius_ = max_nn;
+ k_indices.resize (neighbors_in_radius_);
+ k_distances.resize (neighbors_in_radius_);
+
+ m_lock_.lock ();
+ flann_radius_search (index_id_, &points_[index], &k_indices[0], &k_distances[0], nr_points_, radius, flann_param_.checks, &flann_param_);
+ m_lock_.unlock ();
+
+ return (true);
+ }
+
+ private:
+
+ int convertCloudToArray (const robot_msgs::PointCloud &ros_cloud);
+ int convertCloudToArray (const robot_msgs::PointCloud &ros_cloud, const std::vector<int> &indices);
+
+ private:
+
+ boost::mutex m_lock_;
+
+ /** \brief A FL-ANN type index reference */
+ FLANN_INDEX index_id_;
+
+ /** \brief A pointer to a FL-ANN parameter structure */
+ FLANNParameters flann_param_;
+
+ /** \brief Internal pointer to data */
+ float* points_;
+
+ /** \brief Number of points in the tree */
+ int nr_points_;
+ /** \brief Tree dimensionality (i.e. the number of dimensions per point) */
+ int dim_;
+ };
+
+}
+
+#endif
Property changes on: pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/kdtree/kdtree_flann.h
___________________________________________________________________
Added: svn:keywords
+ Id
Modified: pkg/trunk/mapping/point_cloud_mapping/src/cloud_kdtree/kdtree_ann.cpp
===================================================================
--- pkg/trunk/mapping/point_cloud_mapping/src/cloud_kdtree/kdtree_ann.cpp 2009-03-27 23:53:28 UTC (rev 13069)
+++ pkg/trunk/mapping/point_cloud_mapping/src/cloud_kdtree/kdtree_ann.cpp 2009-03-28 00:04:21 UTC (rev 13070)
@@ -42,7 +42,7 @@
* \param k_distances the resultant point distances
*/
void
- KdTreeANN::nearestKSearch (const robot_msgs::Point32 &p_q, int k, std::vector<int> &k_indices, std::vector<double> &k_distances)
+ KdTreeANN::nearestKSearch (const robot_msgs::Point32 &p_q, int k, std::vector<int> &k_indices, std::vector<float> &k_distances)
{
k_indices.resize (k);
k_distances.resize (k);
@@ -67,7 +67,7 @@
* \param k_distances the resultant point distances
*/
void
- KdTreeANN::nearestKSearch (const robot_msgs::PointCloud &points, int index, int k, std::vector<int> &k_indices, std::vector<double> &k_distances)
+ KdTreeANN::nearestKSearch (const robot_msgs::PointCloud &points, int index, int k, std::vector<int> &k_indices, std::vector<float> &k_distances)
{
if (index >= (int)points.pts.size ())
return;
@@ -95,7 +95,7 @@
* \param max_nn if given, bounds the maximum returned neighbors to this value
*/
bool
- KdTreeANN::radiusSearch (const robot_msgs::Point32 &p_q, double radius, std::vector<int> &k_indices, std::vector<double> &k_distances,
+ KdTreeANN::radiusSearch (const robot_msgs::Point32 &p_q, double radius, std::vector<int> &k_indices, std::vector<float> &k_distances,
int max_nn)
{
ANNpoint p = annAllocPt (3);
@@ -135,7 +135,7 @@
* \param max_nn if given, bounds the maximum returned neighbors to this value
*/
bool
- KdTreeANN::radiusSearch (const robot_msgs::PointCloud &points, int index, double radius, std::vector<int> &k_indices, std::vector<double> &k_distances,
+ KdTreeANN::radiusSearch (const robot_msgs::PointCloud &points, int index, double radius, std::vector<int> &k_indices, std::vector<float> &k_distances,
int max_nn)
{
ANNpoint p = annAllocPt (3);
Added: pkg/trunk/mapping/point_cloud_mapping/src/cloud_kdtree/kdtree_flann.cpp
===================================================================
--- pkg/trunk/mapping/point_cloud_mapping/src/cloud_kdtree/kdtree_flann.cpp (rev 0)
+++ pkg/trunk/mapping/point_cloud_mapping/src/cloud_kdtree/kdtree_flann.cpp 2009-03-28 00:04:21 UTC (rev 13070)
@@ -0,0 +1,237 @@
+/*
+ * Copyright (c) 2008 Radu Bogdan Rusu <rusu -=- cs.tum.edu>
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+/** \author Radu Bogdan Rusu */
+
+#include "point_cloud_mapping/kdtree/kdtree_flann.h"
+
+namespace cloud_kdtree
+{
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief Search for k-nearest neighbors for the given query point.
+ * \param p_q the given query point
+ * \param k the number of neighbors to search for
+ * \param k_indices the resultant point indices
+ * \param k_distances the resultant point distances
+ */
+ void
+ KdTreeFLANN::nearestKSearch (const robot_msgs::Point32 &p_q, int k, std::vector<int> &k_indices, std::vector<float> &k_distances)
+ {
+ k_indices.resize (k);
+ k_distances.resize (k);
+
+ float* p = (float*)malloc (3 * sizeof (float));
+ p[0] = p_q.x; p[1] = p_q.y; p[2] = p_q.z;
+
+ m_lock_.lock ();
+// int* nn_idx_ = (int*) malloc(tcount*nn*sizeof(int));
+// float* nn_dists_ = (float*) malloc(tcount*nn*sizeof(float));
+ flann_find_nearest_neighbors_index (index_id_, p, 1, &k_indices[0], &k_distances[0], k, flann_param_.checks, &flann_param_);
+ m_lock_.unlock ();
+
+ free (p);
+ return;
+ }
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief Search for k-nearest neighbors for the given query point.
+ * \param points the point cloud data
+ * \param index the index in \a points representing the query point
+ * \param k the number of neighbors to search for
+ * \param k_indices the resultant point indices
+ * \param k_distances the resultant point distances
+ */
+ void
+ KdTreeFLANN::nearestKSearch (const robot_msgs::PointCloud &points, int index, int k, std::vector<int> &k_indices, std::vector<float> &k_distances)
+ {
+ if (index >= (int)points.pts.size ())
+ return;
+
+ k_indices.resize (k);
+ k_distances.resize (k);
+
+ float* p = (float*)malloc (3 * sizeof (float));
+ p[0] = points.pts.at (index).x; p[1] = points.pts.at (index).y; p[2] = points.pts.at (index).z;
+
+ m_lock_.lock ();
+ flann_find_nearest_neighbors_index (index_id_, p, 1, &k_indices[0], &k_distances[0], k, flann_param_.checks, &flann_param_);
+ m_lock_.unlock ();
+
+ free (p);
+ return;
+ }
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief Search for all the nearest neighbors of the query point in a given radius.
+ * \param p_q the given query point
+ * \param radius the radius of the sphere bounding all of p_q's neighbors
+ * \param k_indices the resultant point indices
+ * \param k_distances the resultant point distances
+ * \param max_nn if given, bounds the maximum returned neighbors to this value
+ */
+ bool
+ KdTreeFLANN::radiusSearch (const robot_msgs::Point32 &p_q, double radius, std::vector<int> &k_indices, std::vector<float> &k_distances,
+ int max_nn)
+ {
+ float* p = (float*)malloc (3 * sizeof (float));
+ p[0] = p_q.x; p[1] = p_q.y; p[2] = p_q.z;
+ radius *= radius;
+
+ int neighbors_in_radius_;
+ m_lock_.lock ();
+// int neighbors_in_radius_ = ann_kd_tree_->annkFRSearch (p, radius, 0, NULL, NULL, epsilon_);
+ m_lock_.unlock ();
+
+ // No neighbors found ? Return false
+ if (neighbors_in_radius_ == 0)
+ {
+ free (p);
+ return (false);
+ }
+
+ if (neighbors_in_radius_ > max_nn) neighbors_in_radius_ = max_nn;
+ k_indices.resize (neighbors_in_radius_);
+ k_distances.resize (neighbors_in_radius_);
+
+ m_lock_.lock ();
+ flann_radius_search (index_id_, p, &k_indices[0], &k_distances[0], nr_points_, radius, flann_param_.checks, &flann_param_);
+ m_lock_.unlock ();
+
+ free (p);
+ return (true);
+ }
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief Search for all the nearest neighbors of the query point in a given radius.
+ * \param points the point cloud data
+ * \param index the index in \a points representing the query point
+ * \param radius the radius of the sphere bounding all of p_q's neighbors
+ * \param k_indices the resultant point indices
+ * \param k_distances the resultant point distances
+ * \param max_nn if given, bounds the maximum returned neighbors to this value
+ */
+ bool
+ KdTreeFLANN::radiusSearch (const robot_msgs::PointCloud &points, int index, double radius, std::vector<int> &k_indices, std::vector<float> &k_distances,
+ int max_nn)
+ {
+ float* p = (float*)malloc (3 * sizeof (float));
+ p[0] = points.pts.at (index).x; p[1] = points.pts.at (index).y; p[2] = points.pts.at (index).z;
+ radius *= radius;
+
+ int neighbors_in_radius_;
+ m_lock_.lock ();
+// int neighbors_in_radius_ = ann_kd_tree_->annkFRSearch (p, radius, 0, NULL, NULL, epsilon_);
+ m_lock_.unlock ();
+
+ // No neighbors found ? Return false
+ if (neighbors_in_radius_ == 0)
+ {
+ free (p);
+ return (false);
+ }
+
+ if (neighbors_in_radius_ > max_nn) neighbors_in_radius_ = max_nn;
+ k_indices.resize (neighbors_in_radius_);
+ k_distances.resize (neighbors_in_radius_);
+
+ m_lock_.lock ();
+ flann_radius_search (index_id_, p, &k_indices[0], &k_distances[0], nr_points_, radius, flann_param_.checks, &flann_param_);
+ m_lock_.unlock ();
+
+ free (p);
+ return (true);
+ }
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief Converts a ROS PointCloud message to the internal ANN point array representation. Returns the number of
+ * points.
+ * \param ros_cloud the ROS PointCloud message
+ */
+ int
+ KdTreeFLANN::convertCloudToArray (const robot_msgs::PointCloud &ros_cloud)
+ {
+ // No point in doing anything if the array is empty
+ if (ros_cloud.pts.size () == 0)
+ {
+ m_lock_.lock ();
+ points_ = NULL;
+ m_lock_.unlock ();
+ return (0);
+ }
+
+ m_lock_.lock ();
+ points_ = (float*)malloc (ros_cloud.pts.size () * 3 * sizeof (float)); // default number of dimensions (3 = xyz)
+
+ for (unsigned int cp = 0; cp < ros_cloud.pts.size (); cp++)
+ {
+ points_[cp * 3 + 0] = ros_cloud.pts[cp].x;
+ points_[cp * 3 + 1] = ros_cloud.pts[cp].y;
+ points_[cp * 3 + 2] = ros_cloud.pts[cp].z;
+ }
+ m_lock_.unlock ();
+
+ return (ros_cloud.pts.size ());
+ }
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief Converts a ROS PointCloud message with a given set of indices to the internal ANN point array
+ * representation. Returns the number of points.
+ * \note ATTENTION: This method breaks the 1-1 mapping between the indices returned using \a getNeighborsIndices
+ * and the ones from the \a ros_cloud message ! When using this method, make sure to get the underlying point data
+ * using the \a getPoint method
+ * \param ros_cloud the ROS PointCloud message
+ * \param indices the point cloud indices
+ */
+ int
+ KdTreeFLANN::convertCloudToArray (const robot_msgs::PointCloud &ros_cloud, const std::vector<int> &indices)
+ {
+ // No point in doing anything if the array is empty
+ if (ros_cloud.pts.size () == 0 || indices.size () > ros_cloud.pts.size ())
+ {
+ m_lock_.lock ();
+ points_ = NULL;
+ m_lock_.unlock ();
+ return (0);
+ }
+
+ m_lock_.lock ();
+ points_ = (float*)malloc (indices.size () * 3 * sizeof (float)); // default number of dimensions (3 = xyz)
+
+ for (unsigned int cp = 0; cp < indices.size (); cp++)
+ {
+ points_[cp * 3 + 0] = ros_cloud.pts[indices.at (cp)].x;
+ points_[cp * 3 + 1] = ros_cloud.pts[indices.at (cp)].y;
+ points_[cp * 3 + 2] = ros_cloud.pts[indices.at (cp)].z;
+ }
+ m_lock_.unlock ();
+
+ return (indices.size ());
+ }
+}
Property changes on: pkg/trunk/mapping/point_cloud_mapping/src/cloud_kdtree/kdtree_flann.cpp
___________________________________________________________________
Added: svn:keywords
+ Id
Modified: pkg/trunk/mapping/point_cloud_mapping/src/normal_estimation.cpp
===================================================================
--- pkg/trunk/mapping/point_cloud_mapping/src/normal_estimation.cpp 2009-03-27 23:53:28 UTC (rev 13069)
+++ pkg/trunk/mapping/point_cloud_mapping/src/normal_estimation.cpp 2009-03-28 00:04:21 UTC (rev 13070)
@@ -266,7 +266,7 @@
// Get the nerest neighbors for all points
for (int i = 0; i < (int)cloud_normals_.pts.size (); i++)
{
- vector<double> distances (k_);
+ vector<float> distances;
kdtree_->nearestKSearch (i, k_, points_indices_[i], distances);
}
gettimeofday (&t2, NULL);
Modified: pkg/trunk/mapping/point_cloud_mapping/test/cloud_kdtree/test_kdtree.cpp
===================================================================
--- pkg/trunk/mapping/point_cloud_mapping/test/cloud_kdtree/test_kdtree.cpp 2009-03-27 23:53:28 UTC (rev 13069)
+++ pkg/trunk/mapping/point_cloud_mapping/test/cloud_kdtree/test_kdtree.cpp 2009-03-28 00:04:21 UTC (rev 13070)
@@ -60,7 +60,7 @@
bool state;
robot_msgs::PointCloud points;
std::vector<int> indices;
- std::vector<double> distances;
+ std::vector<float> distances;
// Get a point cloud dataset
cloud_kdtree_tests::getBunnyModel (points);
Modified: pkg/trunk/mapping/semantic_point_annotator/src/semantic_point_annotator_omp.cpp
===================================================================
--- pkg/trunk/mapping/semantic_point_annotator/src/semantic_point_annotator_omp.cpp 2009-03-27 23:53:28 UTC (rev 13069)
+++ pkg/trunk/mapping/semantic_point_annotator/src/semantic_point_annotator_omp.cpp 2009-03-28 00:04:21 UTC (rev 13070)
@@ -189,7 +189,7 @@
processed.resize (indices.size (), false);
vector<int> nn_indices;
- vector<double> nn_distances;
+ vector<float> nn_distances;
// Process all points in the indices vector
for (unsigned int i = 0; i < indices.size (); i++)
{
@@ -262,7 +262,7 @@
vector<bool> processed;
processed.resize (indices.size (), false);
vector<int> nn_indices;
- vector<double> nn_distances;
+ vector<float> nn_distances;
// Process all points in the indices vector
int i = 0;
@@ -529,7 +529,7 @@
// Bummer - no omp here
// cloud_kdtree is not thread safe because we rely on ANN/FL-ANN, so get the neighbors here
- vector<double> nn_distances;
+ vector<float> nn_distances;
vector<vector<vector<int> > > neighbors (clusters.size ());
if (concave_)
{
Modified: pkg/trunk/mapping/stereo_convex_patch_histogram/src/convex_patch_histogram.cpp
===================================================================
--- pkg/trunk/mapping/stereo_convex_patch_histogram/src/convex_patch_histogram.cpp 2009-03-27 23:53:28 UTC (rev 13069)
+++ pkg/trunk/mapping/stereo_convex_patch_histogram/src/convex_patch_histogram.cpp 2009-03-28 00:04:21 UTC (rev 13070)
@@ -139,7 +139,7 @@
indices.resize (nr_p);
vector<int> nn_indices;
- vector<double> nn_distances;
+ vector<float> nn_distances;
for (vector<int>::iterator it = indices.begin (); it != indices.end (); ++it)
{
if (processed[*it])
Modified: pkg/trunk/mapping/table_object_detector/src/table_object_detector.cpp
===================================================================
--- pkg/trunk/mapping/table_object_detector/src/table_object_detector.cpp 2009-03-27 23:53:28 UTC (rev 13069)
+++ pkg/trunk/mapping/table_object_detector/src/table_object_detector.cpp 2009-03-28 00:04:21 UTC (rev 13070)
@@ -493,7 +493,7 @@
processed.resize (indices.size (), false);
vector<int> nn_indices;
- vector<double> nn_distances;
+ vector<float> nn_distances;
// Process all points in the indices vector
for (unsigned int i = 0; i < indices.size (); i++)
{
@@ -572,7 +572,7 @@
processed.resize (indices.size (), false);
vector<int> nn_indices;
- vector<double> nn_distances;
+ vector<float> nn_distances;
// Process all points in the indices vector
for (unsigned int i = 0; i < indices.size (); i++)
{
@@ -669,11 +669,9 @@
for (int i = 0; i < (int)cloud.pts.size (); i++)
points_k_indices[i].resize (k_);
// Get the nerest neighbors for all the point indices in the bounds
+ vector<float> distances;
for (int i = 0; i < (int)cloud.pts.size (); i++)
- {
- vector<double> distances (k_);
kdtree->nearestKSearch (i, k_, points_k_indices[i], distances);
- }
// Figure out the viewpoint value in the point cloud frame
PointStamped viewpoint_laser, viewpoint_cloud;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
