bounding_box_raw.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/bounding_box_raw.h>
00036 
00037 using namespace std;
00038 
00039 // --------------------------------------------------------------
00040 /* See function definition */
00041 // --------------------------------------------------------------
00042 BoundingBoxRaw::BoundingBoxRaw(double bbox_radius)
00043 {
00044   result_size_ = 3;
00045   result_size_defined_ = true;
00046 
00047   neighborhood_radius_ = bbox_radius;
00048   neighborhood_radius_defined_ = true;
00049 }
00050 
00051 
00052 
00053 // --------------------------------------------------------------
00054 /* See function definition */
00055 // --------------------------------------------------------------
00056 void BoundingBoxRaw::clearShared()
00057 {
00058 }
00059 
00060 // --------------------------------------------------------------
00061 /* See function definition */
00062 // --------------------------------------------------------------
00063 std::string BoundingBoxRaw::getName() const
00064 {
00065   ostringstream oss;
00066   oss << "BoundingBoxRaw_radius" << neighborhood_radius_;
00067   return oss.str();
00068 }
00069 
00070 
00071 // --------------------------------------------------------------
00072 /* See function definition */
00073 // --------------------------------------------------------------
00074 int BoundingBoxRaw::precompute(const sensor_msgs::PointCloud& data,
00075                                cloud_kdtree::KdTree& data_kdtree,
00076                                const std::vector<const geometry_msgs::Point32*>& interest_pts)
00077 {
00078   return 0;
00079 }
00080 
00081 // --------------------------------------------------------------
00082 /* See function definition */
00083 // --------------------------------------------------------------
00084 int BoundingBoxRaw::precompute(const sensor_msgs::PointCloud& data,
00085                                cloud_kdtree::KdTree& data_kdtree,
00086                                const std::vector<const std::vector<int>*>& interest_region_indices)
00087 {
00088   return 0;
00089 }
00090 
00091 // --------------------------------------------------------------
00092 /* See function definition */
00093 // --------------------------------------------------------------
00094 void BoundingBoxRaw::computeNeighborhoodFeature(const sensor_msgs::PointCloud& data,
00095                                                 const vector<int>& neighbor_indices,
00096                                                 const unsigned int interest_sample_idx,
00097                                                 std::vector<float>& result) const
00098 {
00099   result.resize(result_size_);
00100   const unsigned int nbr_neighbors = neighbor_indices.size();
00101 
00102   // --------------------------
00103   // Check for special case when no points in the bounding box as will initialize
00104   // the min/max extremas using the first point below
00105   if (nbr_neighbors == 0)
00106   {
00107     ROS_INFO("BoundingBoxRaw::computeNeighborhoodFeature() No points to form bounding box");
00108     for (size_t i = 0 ; i < result_size_ ; i++)
00109     {
00110       result[i] = 0.0;
00111     }
00112     return;
00113   }
00114 
00115   // --------------------------
00116   // Initialize extrema values to the first coordinate in the interest region
00117   const geometry_msgs::Point32& first_point = data.points.at(neighbor_indices[0]);
00118   float min_x = first_point.x;
00119   float min_y = first_point.y;
00120   float min_z = first_point.z;
00121   float max_x = min_x;
00122   float max_y = min_y;
00123   float max_z = min_z;
00124 
00125   // --------------------------
00126   // Loop over remaining points in region and update extremas
00127   for (unsigned int i = 1 ; i < nbr_neighbors ; i++)
00128   {
00129     const geometry_msgs::Point32& curr_pt = data.points.at(neighbor_indices[i]);
00130 
00131     // x extremas
00132     float curr_coord = curr_pt.x;
00133     if (curr_coord < min_x)
00134     {
00135       min_x = curr_coord;
00136     }
00137     if (curr_coord > max_x)
00138     {
00139       max_x = curr_coord;
00140     }
00141 
00142     // y extremas
00143     curr_coord = curr_pt.y;
00144     if (curr_coord < min_y)
00145     {
00146       min_y = curr_coord;
00147     }
00148     if (curr_coord > max_y)
00149     {
00150       max_y = curr_coord;
00151     }
00152 
00153     // z extremas
00154     curr_coord = curr_pt.z;
00155     if (curr_coord < min_z)
00156     {
00157       min_z = curr_coord;
00158     }
00159     if (curr_coord > max_z)
00160     {
00161       max_z = curr_coord;
00162     }
00163   }
00164 
00165   // --------------------------
00166   result[0] = max_x - min_x;
00167   result[1] = max_y - min_y;
00168   result[2] = max_z - min_z;
00169 }


ias_descriptors_3d
Author(s): Daniel Munoz/ dmunoz@willowgarage.com, Dejan Pangercic (patched version)
autogenerated on Mon Oct 6 2014 08:48:26