neighborhood_feature.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2009, Willow Garage
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 #include <ias_descriptors_3d/generic/neighborhood_feature.h>
00036 
00037 using namespace std;
00038 
00039 // --------------------------------------------------------------
00040 /* See function definition */
00041 // --------------------------------------------------------------
00042 NeighborhoodFeature::NeighborhoodFeature()
00043 {
00044   neighborhood_radius_ = -1.0;
00045   neighborhood_radius_defined_ = false;
00046 }
00047 
00048 // --------------------------------------------------------------
00049 /* See function definition */
00050 // --------------------------------------------------------------
00051 NeighborhoodFeature::~NeighborhoodFeature()
00052 {
00053 }
00054 
00055 // --------------------------------------------------------------
00056 /* See function definition */
00057 // --------------------------------------------------------------
00058 void NeighborhoodFeature::doComputation(const sensor_msgs::PointCloud& data,
00059                                         cloud_kdtree::KdTree& data_kdtree,
00060                                         const std::vector<const geometry_msgs::Point32*>& interest_pts,
00061                                         std::vector<std::vector<float> >& results)
00062 {
00063   // ----------------------------------------
00064   ROS_INFO("myComputation");
00065   if (neighborhood_radius_defined_ == false)
00066   {
00067     ROS_ERROR("NeighborhoodFeature::doComputation() radius not defined yet");
00068     return;
00069   }
00070   if (neighborhood_radius_ < 1e-6)
00071   {
00072     ROS_ERROR("NeighborhoodFeature::doComputation() radius is negative: %f", neighborhood_radius_);
00073   }
00074 
00075   // ----------------------------------------
00076   // Iterate over each interest point, compute local neighborhood, compute feature
00077   int nbr_interest_pts = interest_pts.size();
00078 #pragma omp parallel for
00079   for (int i = 0 ; i < nbr_interest_pts ; i++)
00080   {
00081     // Retrieve interest point
00082     const geometry_msgs::Point32* curr_interest_pt = interest_pts[static_cast<size_t> (i)];
00083     if (curr_interest_pt == NULL)
00084     {
00085       ROS_WARN("NeighborhoodFeature::doComputation() passed NULL interest point");
00086     }
00087     else
00088     {
00089       // Grab neighbors around interest point
00090       vector<int> neighbor_indices;
00091       vector<float> neighbor_distances; // unused
00092       // radiusSearch returning false (0 points) is handled in inherited class
00093       data_kdtree.radiusSearch(*curr_interest_pt, neighborhood_radius_, neighbor_indices, neighbor_distances);
00094 
00095       computeNeighborhoodFeature(data, neighbor_indices, i, results[static_cast<size_t> (i)]);
00096     }
00097   }
00098 }
00099 
00100 // --------------------------------------------------------------
00101 /* See function definition */
00102 // --------------------------------------------------------------
00103 void NeighborhoodFeature::doComputation(const sensor_msgs::PointCloud& data,
00104                                         cloud_kdtree::KdTree& data_kdtree,
00105                                         const std::vector<const vector<int>*>& interest_region_indices,
00106                                         std::vector<std::vector<float> >& results)
00107 {
00108   // ----------------------------------------
00109   if (neighborhood_radius_defined_ == false)
00110   {
00111     ROS_ERROR("NeighborhoodFeature::doComputation() radius not defined yet");
00112     return;
00113   }
00114 
00115   // ----------------------------------------
00116   // Iterate over each interest region, compute local neighborhood, compute feature
00117   int nbr_interest_regions = interest_region_indices.size();
00118 #pragma omp parallel for
00119   for (int i = 0 ; i < nbr_interest_regions ; i++)
00120   {
00121     // Retrieve interest region
00122     const vector<int>* curr_interest_region = interest_region_indices[static_cast<size_t> (i)];
00123     if (curr_interest_region == NULL)
00124     {
00125       ROS_WARN("NeighborhoodFeature::doComputation() passed NULL interest region");
00126     }
00127     else
00128     {
00129       // Find the neighborhood around the region's centroid if indicated to.
00130       vector<int> neighbor_indices;
00131       if (neighborhood_radius_ > 1e-6)
00132       {
00133         // Compute centroid of interest region
00134         geometry_msgs::Point32 region_centroid;
00135         cloud_geometry::nearest::computeCentroid(data, *curr_interest_region, region_centroid);
00136 
00137         vector<float> neighbor_distances; // unused
00138         // radiusSearch returning false (0 points) is handled in inherited class
00139         data_kdtree.radiusSearch(region_centroid, neighborhood_radius_, neighbor_indices, neighbor_distances);
00140 
00141         // Now point to the neighboring points from radiusSearch
00142         curr_interest_region = &neighbor_indices;
00143       }
00144 
00145       computeNeighborhoodFeature(data, *curr_interest_region, i, results[static_cast<size_t> (i)]);
00146     }
00147   }
00148 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


ias_descriptors_3d
Author(s): Daniel Munoz/ dmunoz@willowgarage.com, Dejan Pangercic (patched version)
autogenerated on Thu May 23 2013 14:01:18