spin_image_tangent.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_tangent.h>
00036 
00037 using namespace std;
00038 
00039 // --------------------------------------------------------------
00040 /* See function definition */
00041 // --------------------------------------------------------------
00042 SpinImageTangent::SpinImageTangent(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 void SpinImageTangent::clearShared()
00073 {
00074   spectral_information_->clearSpectral();
00075 }
00076 
00077 // --------------------------------------------------------------
00078 /* See function definition */
00079 // --------------------------------------------------------------
00080 std::string SpinImageTangent::getName() const
00081 {
00082   return string("TODO: Add a name to this feature.");
00083 }
00084 
00085 // --------------------------------------------------------------
00086 /* See function definition */
00087 // --------------------------------------------------------------
00088 int SpinImageTangent::precompute(const sensor_msgs::PointCloud& data,
00089                                  cloud_kdtree::KdTree& data_kdtree,
00090                                  const std::vector<const geometry_msgs::Point32*>& interest_pts)
00091 {
00092   // Compute spectral information if not already done
00093   if (spectral_information_->isSpectralComputed() == false)
00094   {
00095     if (spectral_information_->analyzeInterestPoints(data, data_kdtree, interest_pts) < 0)
00096     {
00097       return -1;
00098     }
00099   }
00100 
00101   // Retrieve necessary spectral information this class needs to compute features
00102   spin_axes_ = &(spectral_information_->getTangents());
00103 
00104   // Verify the tangents are for the interest points
00105   size_t nbr_interest_pts = interest_pts.size();
00106   if (spin_axes_->size() != nbr_interest_pts)
00107   {
00108     ROS_ERROR("SpinImageTangent::precompute() inconsistent number of points and spectral info");
00109     spin_axes_ = NULL;
00110     return -1;
00111   }
00112 
00113   // Copy the center of the spin images as "Eigen" vectors
00114   spin_image_centers_.resize(nbr_interest_pts);
00115   for (size_t i = 0 ; i < nbr_interest_pts ; i++)
00116   {
00117     // Will be handled in NeighborhoodFeature::doComputation() if interest point is NULL
00118     if (interest_pts[i] != NULL)
00119     {
00120       spin_image_centers_[i][0] = (interest_pts[i])->x;
00121       spin_image_centers_[i][1] = (interest_pts[i])->y;
00122       spin_image_centers_[i][2] = (interest_pts[i])->z;
00123     }
00124   }
00125 
00126   return 0;
00127 }
00128 
00129 // --------------------------------------------------------------
00130 /* See function definition */
00131 // --------------------------------------------------------------
00132 int SpinImageTangent::precompute(const sensor_msgs::PointCloud& data,
00133                                  cloud_kdtree::KdTree& data_kdtree,
00134                                  const std::vector<const std::vector<int>*>& interest_region_indices)
00135 {
00136   // Compute spectral information if not already done
00137   if (spectral_information_->isSpectralComputed() == false)
00138   {
00139     if (spectral_information_->analyzeInterestRegions(data, data_kdtree, interest_region_indices) < 0)
00140     {
00141       return -1;
00142     }
00143   }
00144 
00145   // Retrieve necessary spectral information this class needs to compute features
00146   spin_axes_ = &(spectral_information_->getTangents());
00147 
00148   // Verify the tangents are for the interest regions
00149   size_t nbr_interest_regions = interest_region_indices.size();
00150   if (spin_axes_->size() != nbr_interest_regions)
00151   {
00152     ROS_ERROR("SpinImageTangent::precompute() inconsistent number of regions and spectral info");
00153     spin_axes_ = NULL;
00154     return -1;
00155   }
00156 
00157   // Copy the center (centroid of regions) of the spin images as "Eigen" vectors
00158   spin_image_centers_.resize(nbr_interest_regions);
00159   for (size_t i = 0 ; i < nbr_interest_regions ; i++)
00160   {
00161     // Will be handled in NeighborhoodFeature::doComputation() if region_indices is NULL
00162     // Will be handled in SpinImageGeneric::computeNeighborhoodFeature() if spin axis is NULL
00163     if (interest_region_indices[i] != NULL && (*spin_axes_)[i] != NULL)
00164     {
00165       geometry_msgs::Point32 region_centroid;
00166       cloud_geometry::nearest::computeCentroid(data, *(interest_region_indices[i]), region_centroid);
00167       spin_image_centers_[i][0] = region_centroid.x;
00168       spin_image_centers_[i][1] = region_centroid.y;
00169       spin_image_centers_[i][2] = region_centroid.z;
00170     }
00171   }
00172 
00173   return 0;
00174 }
00175 


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