spin_image_generic.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/generic/spin_image_generic.h>
00036 
00037 using namespace std;
00038 
00039 // --------------------------------------------------------------
00040 /* See function definition */
00041 // --------------------------------------------------------------
00042 SpinImageGeneric::SpinImageGeneric()
00043 {
00044   spin_axes_ = NULL;
00045 }
00046 
00047 // --------------------------------------------------------------
00048 /* See function definition */
00049 // --------------------------------------------------------------
00050 SpinImageGeneric::~SpinImageGeneric()
00051 {
00052 }
00053 
00054 void SpinImageGeneric::display(const std::vector<float>& spin_image) const
00055 {
00056   if(spin_image.empty()) {
00057     ROS_WARN("No spin image to display.");
00058     return;
00059   }
00060   
00061   assert(spin_image.size() == nbr_rows_ * nbr_cols_);
00062 
00063   CvSize sz = cvSize(nbr_cols_, nbr_rows_);
00064   IplImage* img = cvCreateImage(sz, IPL_DEPTH_8U, 1);
00065   
00066   for(int y=0; y<img->height; ++y) { 
00067     uchar* ptr = (uchar*)img->imageData + y * img->widthStep;
00068     for(int x=0; x<img->width; ++x, ++ptr) { 
00069       *ptr = spin_image[x + y * nbr_cols_] * 255;
00070     }
00071   }
00072 
00073   float scale = 40;
00074   IplImage* img_big = cvCreateImage(cvSize(((float)img->width)*scale, ((float)img->height)*scale), img->depth, img->nChannels);
00075   cvResize(img, img_big, CV_INTER_AREA);
00076 
00077 
00078   ostringstream oss;
00079   oss << nbr_rows_ * row_res_ << "x" << nbr_cols_ * col_res_ << " m, " << row_res_ << "x" << col_res_ << "pix/m" << endl;
00080   cvNamedWindow(oss.str().c_str());
00081   cvShowImage(oss.str().c_str(), img_big);
00082   cvWaitKey();
00083   cvDestroyWindow(oss.str().c_str());
00084   cvReleaseImage(&img);
00085   cvReleaseImage(&img_big);
00086 }
00087 
00088 // --------------------------------------------------------------
00089 /* See function definition */
00090 // --------------------------------------------------------------
00091 void SpinImageGeneric::computeNeighborhoodFeature(const sensor_msgs::PointCloud& data,
00092                                                   const std::vector<int>& neighbor_indices,
00093                                                   const unsigned int interest_sample_idx,
00094                                                   std::vector<float>& result) const
00095 {
00096   const Eigen3::Vector3d* curr_spin_axis = (*spin_axes_)[interest_sample_idx];
00097   if (curr_spin_axis == NULL)
00098   {
00099     ROS_DEBUG("SpinImageGeneric::computeNeighborhoodFeature() no spin axis for sample %u", interest_sample_idx);
00100     return;
00101   }
00102 
00103   // Clear out result for counting
00104   result.resize(result_size_);
00105   for (size_t i = 0 ; i < result_size_ ; i++)
00106   {
00107     result[i] = 0.0;
00108   }
00109 
00110   const Eigen3::Vector3d& center_pt = spin_image_centers_[interest_sample_idx];
00111 
00112   // Offset into row number (center point is middle of the spin image)
00113   const unsigned int row_offset = nbr_rows_ / 2;
00114 
00115   float max_bin_count = 1.0; // init to 1.0 so avoid divide by 0 if no neighbor points
00116   unsigned int nbr_neighbors = neighbor_indices.size();
00117   for (unsigned int i = 0 ; i < nbr_neighbors ; i++)
00118   {
00119     // Create vector from center point to neighboring point
00120     const geometry_msgs::Point32& curr_neighbor_pt = data.points.at(neighbor_indices[i]);
00121     Eigen3::Vector3d neighbor_vec;
00122     neighbor_vec[0] = curr_neighbor_pt.x - center_pt[0];
00123     neighbor_vec[1] = curr_neighbor_pt.y - center_pt[1];
00124     neighbor_vec[2] = curr_neighbor_pt.z - center_pt[2];
00125     const double neighbor_vec_norm = neighbor_vec.norm();
00126 
00127     // ----------------------------------------
00128     // Scalar projection of neighbor_vec onto spin axis (unit length)
00129     const double axis_projection = neighbor_vec_norm * neighbor_vec.dot(*curr_spin_axis);
00130     // Computed signed bin along the axis
00131     const int signed_row_nbr = static_cast<int> (floor(axis_projection / row_res_));
00132     // Offset the bin
00133     const int curr_row = signed_row_nbr + row_offset;
00134 
00135     // ----------------------------------------
00136     // Two vectors a and b originating from the same point form a parallelogram
00137     // |a x b| is the area Q of a parallelogram with base |a| and height h (Q = |a|h)
00138     // http://en.wikipedia.org/wiki/Cross_product
00139     // a = spin axis (unit length)
00140     // b = neighbor_vec
00141     // h = Q / |a| = |a x b| / |a| = |a x b|
00142     const unsigned int curr_col = static_cast<unsigned int> (floor((curr_spin_axis->cross(
00143         neighbor_vec)).norm() / col_res_));
00144 
00145     // Increment appropriate spin image cell.
00146     // First verify the point is contained in the image
00147     if (curr_row >= 0 && static_cast<unsigned int> (curr_row) < nbr_rows_ && curr_col < nbr_cols_)
00148     {
00149       // Compute vectorized cell number
00150       size_t cell_nbr = static_cast<size_t> (curr_row * nbr_cols_ + curr_col);
00151       result[cell_nbr] += 1.0;
00152 
00153       // Update counter for cell with most points
00154       if (result[cell_nbr] > max_bin_count)
00155       {
00156         max_bin_count = result[cell_nbr];
00157       }
00158     }
00159   }
00160 
00161   // Normalize all cells between 0 and 1
00162   for (size_t i = 0 ; i < result_size_ ; i++)
00163   {
00164     result[i] /= max_bin_count;
00165   }
00166 
00167   if(debug_)
00168     display(result);
00169 }


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