shape_spectral.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/shape_spectral.h>
00036 
00037 using namespace std;
00038 
00039 // --------------------------------------------------------------
00040 /* See function definition */
00041 // --------------------------------------------------------------
00042 ShapeSpectral::ShapeSpectral(SpectralAnalysis& spectral_information)
00043 {
00044   result_size_ = 3;
00045   result_size_defined_ = true;
00046 
00047   spectral_information_ = &spectral_information;
00048   eig_vals_ = NULL;
00049 }
00050 
00051 // --------------------------------------------------------------
00052 /* See function definition */
00053 // --------------------------------------------------------------
00054 void ShapeSpectral::clearShared()
00055 {
00056   spectral_information_->clearSpectral();
00057 }
00058 
00059 std::string ShapeSpectral::getName() const
00060 {
00061   ostringstream oss;
00062   oss << "ShapeSpectral_radius" << spectral_information_->getRadius();
00063 
00064   return oss.str();
00065 }
00066 
00067 
00068 // --------------------------------------------------------------
00069 /* See function definition */
00070 // --------------------------------------------------------------
00071 int ShapeSpectral::precompute(const sensor_msgs::PointCloud& data,
00072                               cloud_kdtree::KdTree& data_kdtree,
00073                               const std::vector<const geometry_msgs::Point32*>& interest_pts)
00074 {
00075   // Compute spectral information if not already done
00076   if (spectral_information_->isSpectralComputed() == false)
00077   {
00078     if (spectral_information_->analyzeInterestPoints(data, data_kdtree, interest_pts) < 0)
00079     {
00080       return -1;
00081     }
00082   }
00083 
00084   // Retrieve necessary spectral information this class needs to compute features
00085   eig_vals_ = &(spectral_information_->getEigenValues());
00086 
00087   // verify the eigenvectors are for the interest points
00088   if (eig_vals_->size() != interest_pts.size())
00089   {
00090     ROS_ERROR("ShapeSpectral::precompute() inconsistent number of points and spectral info");
00091     eig_vals_ = NULL;
00092     return -1;
00093   }
00094 
00095   return 0;
00096 }
00097 
00098 // --------------------------------------------------------------
00099 /* See function definition */
00100 // --------------------------------------------------------------
00101 int ShapeSpectral::precompute(const sensor_msgs::PointCloud& data,
00102                               cloud_kdtree::KdTree& data_kdtree,
00103                               const std::vector<const std::vector<int>*>& interest_region_indices)
00104 {
00105   // Compute spectral information if not already done
00106   if (spectral_information_->isSpectralComputed() == false)
00107   {
00108     if (spectral_information_->analyzeInterestRegions(data, data_kdtree, interest_region_indices) < 0)
00109     {
00110       return -1;
00111     }
00112   }
00113 
00114   // Retrieve necessary spectral information this class needs to compute features
00115   eig_vals_ = &(spectral_information_->getEigenValues());
00116 
00117   // verify the eigenvectors are for the interest regions
00118   if (eig_vals_->size() != interest_region_indices.size())
00119   {
00120     ROS_ERROR("ShapeSpectral::precompute() inconsistent number of regions and spectral info");
00121     eig_vals_ = NULL;
00122     return -1;
00123   }
00124 
00125   return 0;
00126 }
00127 
00128 // --------------------------------------------------------------
00129 /* See function definition */
00130 // --------------------------------------------------------------
00131 void ShapeSpectral::doComputation(const sensor_msgs::PointCloud& data,
00132                                   cloud_kdtree::KdTree& data_kdtree,
00133                                   const std::vector<const geometry_msgs::Point32*>& interest_pts,
00134                                   std::vector<std::vector<float> >& results)
00135 {
00136   // ----------------------------------------
00137   // Compute the shape features for each interest point
00138   int nbr_interest_pts = interest_pts.size();
00139 #pragma omp parallel for
00140   for (int i = 0 ; i < nbr_interest_pts ; i++)
00141   {
00142     computeShapeFeatures(i, results[static_cast<size_t> (i)]);
00143   }
00144 }
00145 
00146 // --------------------------------------------------------------
00147 /* See function definition */
00148 // --------------------------------------------------------------
00149 void ShapeSpectral::doComputation(const sensor_msgs::PointCloud& data,
00150                                   cloud_kdtree::KdTree& data_kdtree,
00151                                   const std::vector<const std::vector<int>*>& interest_region_indices,
00152                                   std::vector<std::vector<float> >& results)
00153 {
00154   // ----------------------------------------
00155   // Compute the shape features for each interest region
00156   int nbr_interest_regions = interest_region_indices.size();
00157 #pragma omp parallel for
00158   for (int i = 0 ; i < nbr_interest_regions ; i++)
00159   {
00160     computeShapeFeatures(i, results[static_cast<size_t> (i)]);
00161   }
00162 }
00163 
00164 // --------------------------------------------------------------
00165 /* See function definition
00166  * Invariant: interest_sample_idx is within bounds */
00167 // --------------------------------------------------------------
00168 void ShapeSpectral::computeShapeFeatures(const unsigned int interest_sample_idx, std::vector<float>& result) const
00169 {
00170   // Retrieve the eigenvalues for current interest point/region
00171   const Eigen3::Vector3d* curr_eigen_vals = (*eig_vals_)[interest_sample_idx];
00172 
00173   // NULL indicates couldnt compute spectral information for interest sample
00174   if (curr_eigen_vals != NULL)
00175   {
00176     result.resize(result_size_);
00177 
00178     result[0] = static_cast<float> ((*curr_eigen_vals)[0]); // scatter
00179     result[1] = static_cast<float> ((*curr_eigen_vals)[2] - (*curr_eigen_vals)[1]); // linear
00180     result[2] = static_cast<float> ((*curr_eigen_vals)[1] - (*curr_eigen_vals)[0]); // surface
00181   }
00182 }
00183 


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