spin_image_normal.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_normal.h>
00036 
00037 using namespace std;
00038 
00039 // --------------------------------------------------------------
00040 /* See function definition */
00041 // --------------------------------------------------------------
00042 SpinImageNormal::SpinImageNormal(const double row_res,
00043                                  const double col_res,
00044                                  const unsigned int nbr_rows,
00045                                  const unsigned int nbr_cols,
00046                                  const bool use_interest_regions_only,
00047                                  SpectralAnalysis& spectral_information)
00048 {
00049   row_res_ = row_res;
00050   col_res_ = col_res;
00051   nbr_rows_ = nbr_rows;
00052   nbr_cols_ = nbr_cols;
00053   spectral_information_ = &spectral_information;
00054 
00055   result_size_ = nbr_rows * nbr_cols;
00056   result_size_defined_ = true;
00057 
00058   if (use_interest_regions_only)
00059   {
00060     neighborhood_radius_ = -1.0;
00061   }
00062   else
00063   {
00064     neighborhood_radius_ = sqrt(pow(row_res_ * nbr_rows_, 2.0) + pow(col_res_ * nbr_cols_, 2.0));
00065   }
00066   neighborhood_radius_defined_ = true;
00067 }
00068 
00069 // --------------------------------------------------------------
00070 /* See function definition */
00071 // --------------------------------------------------------------
00072 string SpinImageNormal::getName() const
00073 {
00074   return string("TODO: Add a name to this feature.");
00075 }
00076 
00077 
00078 // --------------------------------------------------------------
00079 /* See function definition */
00080 // --------------------------------------------------------------
00081 void SpinImageNormal::clearShared()
00082 {
00083   spectral_information_->clearSpectral();
00084 }
00085 
00086 // --------------------------------------------------------------
00087 /* See function definition */
00088 // --------------------------------------------------------------
00089 int SpinImageNormal::precompute(const sensor_msgs::PointCloud& data,
00090                                 cloud_kdtree::KdTree& data_kdtree,
00091                                 const std::vector<const geometry_msgs::Point32*>& interest_pts)
00092 {
00093   // Compute spectral information if not already done
00094   if (spectral_information_->isSpectralComputed() == false)
00095   {
00096     if (spectral_information_->analyzeInterestPoints(data, data_kdtree, interest_pts) < 0)
00097     {
00098       return -1;
00099     }
00100   }
00101 
00102   // Retrieve necessary spectral information this class needs to compute features
00103   spin_axes_ = &(spectral_information_->getNormals());
00104 
00105   // Verify the normals are for the interest points
00106   size_t nbr_interest_pts = interest_pts.size();
00107   if (spin_axes_->size() != nbr_interest_pts)
00108   {
00109     ROS_ERROR("SpinImageNormal::precompute() inconsistent number of points and spectral info");
00110     spin_axes_ = NULL;
00111     return -1;
00112   }
00113 
00114   // Copy the center of the spin images as "Eigen" vectors
00115   spin_image_centers_.resize(nbr_interest_pts);
00116   for (size_t i = 0 ; i < nbr_interest_pts ; i++)
00117   {
00118     // Will be handled in NeighborhoodFeature::doComputation() if interest point is NULL
00119     if (interest_pts[i] != NULL)
00120     {
00121       spin_image_centers_[i][0] = (interest_pts[i])->x;
00122       spin_image_centers_[i][1] = (interest_pts[i])->y;
00123       spin_image_centers_[i][2] = (interest_pts[i])->z;
00124     }
00125   }
00126 
00127   return 0;
00128 }
00129 
00130 // --------------------------------------------------------------
00131 /* See function definition */
00132 // --------------------------------------------------------------
00133 int SpinImageNormal::precompute(const sensor_msgs::PointCloud& data,
00134                                 cloud_kdtree::KdTree& data_kdtree,
00135                                 const std::vector<const std::vector<int>*>& interest_region_indices)
00136 {
00137   // Compute spectral information if not already done
00138   if (spectral_information_->isSpectralComputed() == false)
00139   {
00140     if (spectral_information_->analyzeInterestRegions(data, data_kdtree, interest_region_indices) < 0)
00141     {
00142       return -1;
00143     }
00144   }
00145 
00146   // Retrieve necessary spectral information this class needs to compute features
00147   spin_axes_ = &(spectral_information_->getNormals());
00148 
00149   // Verify the normals are for the interest regions
00150   size_t nbr_interest_regions = interest_region_indices.size();
00151   if (spin_axes_->size() != nbr_interest_regions)
00152   {
00153     ROS_ERROR("SpinImageNormal::precompute() inconsistent number of regions and spectral info");
00154     spin_axes_ = NULL;
00155     return -1;
00156   }
00157 
00158   // Copy the center (centroid of regions) of the spin images as "Eigen" vectors
00159   spin_image_centers_.resize(nbr_interest_regions);
00160   for (size_t i = 0 ; i < nbr_interest_regions ; i++)
00161   {
00162     // Will be handled in NeighborhoodFeature::doComputation() if region_indices is NULL
00163     // Will be handled in SpinImageGeneric::computeNeighborhoodFeature() if spin axis is NULL
00164     if (interest_region_indices[i] != NULL && (*spin_axes_)[i] != NULL)
00165     {
00166       geometry_msgs::Point32 region_centroid;
00167       cloud_geometry::nearest::computeCentroid(data, *(interest_region_indices[i]), region_centroid);
00168       spin_image_centers_[i][0] = region_centroid.x;
00169       spin_image_centers_[i][1] = region_centroid.y;
00170       spin_image_centers_[i][2] = region_centroid.z;
00171     }
00172   }
00173 
00174   return 0;
00175 }
00176 
 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