spin_image_custom.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/spin_image_custom.h>
00036 
00037 using namespace std;
00038 
00039 // --------------------------------------------------------------
00040 /* See function definition */
00041 // --------------------------------------------------------------
00042 SpinImageCustom::SpinImageCustom(const double ref_x,
00043                                  const double ref_y,
00044                                  const double ref_z,
00045                                  const double row_res,
00046                                  const double col_res,
00047                                  const unsigned int nbr_rows,
00048                                  const unsigned int nbr_cols,
00049                                  const bool use_interest_regions_only)
00050 {
00051   custom_axis_[0] = ref_x;
00052   custom_axis_[1] = ref_y;
00053   custom_axis_[2] = ref_z;
00054   row_res_ = row_res;
00055   col_res_ = col_res;
00056   nbr_rows_ = nbr_rows;
00057   nbr_cols_ = nbr_cols;
00058 
00059   result_size_ = nbr_rows * nbr_cols;
00060   result_size_defined_ = true;
00061 
00062   if (use_interest_regions_only)
00063   {
00064     neighborhood_radius_ = -1.0;
00065   }
00066   else
00067   {
00068     neighborhood_radius_ = sqrt(pow(row_res_ * nbr_rows_, 2.0) + pow(col_res_ * nbr_cols_, 2.0));
00069   }
00070   neighborhood_radius_defined_ = true;
00071 }
00072 
00073 // --------------------------------------------------------------
00074 /* See function definition */
00075 // --------------------------------------------------------------
00076 void SpinImageCustom::clearShared()
00077 {
00078 }
00079 
00080 
00081 
00082 // --------------------------------------------------------------
00083 /* See function definition */
00084 // --------------------------------------------------------------
00085 std::string SpinImageCustom::getName() const
00086 {
00087   ostringstream oss;
00088   oss << "SpinImageCustom_axis" << custom_axis_[0] << "," << custom_axis_[1] << "," << custom_axis_[2];
00089   oss << "_rowRes" << row_res_ << "_colRes" << col_res_ << "_numRows" << nbr_rows_ << "_numCols" << nbr_cols_;
00090   oss << "_radius" << neighborhood_radius_; 
00091   return oss.str();
00092 }
00093 
00094 
00095 // --------------------------------------------------------------
00096 /* See function definition */
00097 // --------------------------------------------------------------
00098 int SpinImageCustom::precompute(const sensor_msgs::PointCloud& data,
00099                                 cloud_kdtree::KdTree& data_kdtree,
00100                                 const std::vector<const geometry_msgs::Point32*>& interest_pts)
00101 {
00102   // Point spin_axis_ to duplicate copies of the custom spin axis
00103   size_t nbr_interest_pts = interest_pts.size();
00104   custom_axis_duplicated_.assign(nbr_interest_pts, &custom_axis_);
00105   spin_axes_ = &custom_axis_duplicated_;
00106 
00107   // Copy the center of the spin images as "Eigen" vectors
00108   spin_image_centers_.resize(nbr_interest_pts);
00109   for (size_t i = 0 ; i < nbr_interest_pts ; i++)
00110   {
00111     // Will be handled in NeighborhoodFeature::doComputation() if interest point is NULL
00112     if (interest_pts[i] != NULL)
00113     {
00114       spin_image_centers_[i][0] = (interest_pts[i])->x;
00115       spin_image_centers_[i][1] = (interest_pts[i])->y;
00116       spin_image_centers_[i][2] = (interest_pts[i])->z;
00117     }
00118   }
00119 
00120   return 0;
00121 }
00122 
00123 // --------------------------------------------------------------
00124 /* See function definition */
00125 // --------------------------------------------------------------
00126 int SpinImageCustom::precompute(const sensor_msgs::PointCloud& data,
00127                                 cloud_kdtree::KdTree& data_kdtree,
00128                                 const std::vector<const std::vector<int>*>& interest_region_indices)
00129 {
00130   // Point spin_axis_ to duplicate copies of the custom spin axis
00131   size_t nbr_interest_regions = interest_region_indices.size();
00132   custom_axis_duplicated_.assign(nbr_interest_regions, &custom_axis_);
00133   spin_axes_ = &custom_axis_duplicated_;
00134 
00135   // Copy the center (centroid of regions) of the spin images as "Eigen" vectors
00136   spin_image_centers_.resize(nbr_interest_regions);
00137   for (size_t i = 0 ; i < nbr_interest_regions ; i++)
00138   {
00139     // Will be handled in NeighborhoodFeature::doComputation() if region_indices is NULL
00140     // Will be handled in SpinImageGeneric::computeNeighborhoodFeature() if spin axis is NULL
00141     if (interest_region_indices[i] != NULL && (*spin_axes_)[i] != NULL)
00142     {
00143       geometry_msgs::Point32 region_centroid;
00144       cloud_geometry::nearest::computeCentroid(data, *(interest_region_indices[i]), region_centroid);
00145       spin_image_centers_[i][0] = region_centroid.x;
00146       spin_image_centers_[i][1] = region_centroid.y;
00147       spin_image_centers_[i][2] = region_centroid.z;
00148     }
00149   }
00150 
00151   return 0;
00152 }
00153 
 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