Point Cloud Library (PCL)  1.3.1
shot.hpp
Go to the documentation of this file.
00001 /*
00002   * Software License Agreement (BSD License)
00003   *
00004   *  Copyright (c) 2009, Willow Garage, Inc.
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 Willow Garage, Inc. 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   */
00036 
00037 #ifndef PCL_FEATURES_IMPL_SHOT_H_
00038 #define PCL_FEATURES_IMPL_SHOT_H_
00039 
00040 #include "pcl/features/shot.h"
00041 #include "pcl/features/shot_common.h"
00042 #include <utility>
00043 
00045 
00052 inline bool
00053 areEquals (double val1, double val2, double zeroDoubleEps = zeroDoubleEps15)
00054 {
00055   return (fabs (val1 - val2)<zeroDoubleEps);
00056 };
00057 
00059 
00066 inline bool
00067 areEquals (float val1, float val2, float zeroFloatEps = zeroFloatEps8)
00068 {
00069   return (fabs (val1 - val2)<zeroFloatEps);
00070 }
00071 
00073 template <typename PointNT, typename PointOutT> float
00074 pcl::SHOTEstimation<pcl::PointXYZRGBA, PointNT, PointOutT>::sRGB_LUT[256] = {- 1};
00075 
00077 template <typename PointNT, typename PointOutT> float
00078 pcl::SHOTEstimation<pcl::PointXYZRGBA, PointNT, PointOutT>::sXYZ_LUT[4000] = {- 1};
00079 
00081 
00091 template <typename PointNT, typename PointOutT> void
00092 pcl::SHOTEstimation<pcl::PointXYZRGBA, PointNT, PointOutT>::RGB2CIELAB (unsigned char R, unsigned char G,
00093                                                                         unsigned char B, float &L, float &A,
00094                                                                         float &B2)
00095 {
00096   if (sRGB_LUT[0] < 0)
00097   {
00098     for (int i = 0; i < 256; i++)
00099     {
00100       float f = i / 255.0f;
00101       if (f > 0.04045)
00102         sRGB_LUT[i] = (float)pow ((f + 0.055) / 1.055, 2.4);
00103       else
00104         sRGB_LUT[i] = f / 12.92;
00105     }
00106 
00107     for (int i = 0; i < 4000; i++)
00108     {
00109       float f = i / 4000.0f;
00110       if (f > 0.008856)
00111         sXYZ_LUT[i] = pow (f, (float)0.3333);
00112       else
00113         sXYZ_LUT[i] = (7.787 * f) + (16.0 / 116.0);
00114     }
00115   }
00116 
00117   float fr = sRGB_LUT[R];
00118   float fg = sRGB_LUT[G];
00119   float fb = sRGB_LUT[B];
00120 
00121   // Use white = D65
00122   const float x = fr * 0.412453 + fg * 0.357580 + fb * 0.180423;
00123   const float y = fr * 0.212671 + fg * 0.715160 + fb * 0.072169;
00124   const float z = fr * 0.019334 + fg * 0.119193 + fb * 0.950227;
00125 
00126   float vx = x / 0.95047;
00127   float vy = y;
00128   float vz = z / 1.08883;
00129 
00130   vx = sXYZ_LUT[int(vx*4000)];
00131   vy = sXYZ_LUT[int(vy*4000)];
00132   vz = sXYZ_LUT[int(vz*4000)];
00133 
00134   L = 116.0 * vy - 16.0;
00135   if (L>100)
00136     L=100;
00137 
00138   A = 500.0 * (vx - vy);
00139   if (A>120)
00140     A=120;
00141   else if (A<- 120)
00142     A=- 120;
00143 
00144   B2 = 200.0 * (vy - vz);
00145   if (B2>120)
00146     B2=120;
00147   else if (B2<- 120)
00148     B2=- 120;
00149 }
00150 
00152 // Quadrilinear interpolation; used when color and shape descriptions are NOT activated simultaneously
00153 template <typename PointInT, typename PointNT, typename PointOutT> void
00154 pcl::SHOTEstimationBase<PointInT, PointNT, PointOutT>::interpolateSingleChannel (
00155     const std::vector<int> &indices,
00156     const std::vector<float> &dists,
00157     const Eigen::Vector4f &central_point,
00158     const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &rf,
00159     std::vector<double> &binDistance,
00160     const int nr_bins,
00161     Eigen::VectorXf &shot)
00162 {
00163   if (rf.size () != 3)
00164   {
00165     PCL_ERROR ("[pcl::%s::interpolateSingleChannel] RF size different than 9! Aborting...\n");
00166     return;
00167   }
00168 
00169   for (size_t i_idx = 0; i_idx < indices.size (); ++i_idx)
00170   {
00171     Eigen::Vector4f delta = surface_->points[indices[i_idx]].getVector4fMap () - central_point;
00172   delta[3] = 0;
00173 
00174     // Compute the Euclidean norm
00175    double distance = sqrt (dists[i_idx]);
00176 
00177     if (areEquals (distance, 0.0))
00178       continue;
00179 
00180     double xInFeatRef = delta.dot (rf[0]); //(x * feat[i].rf[0] + y * feat[i].rf[1] + z * feat[i].rf[2]);
00181     double yInFeatRef = delta.dot (rf[1]); //(x * feat[i].rf[3] + y * feat[i].rf[4] + z * feat[i].rf[5]);
00182     double zInFeatRef = delta.dot (rf[2]); //(x * feat[i].rf[6] + y * feat[i].rf[7] + z * feat[i].rf[8]);
00183 
00184     // To avoid numerical problems afterwards
00185     if (fabs (yInFeatRef) < 1E-30)
00186       yInFeatRef  = 0;
00187     if (fabs (xInFeatRef) < 1E-30)
00188       xInFeatRef  = 0;
00189     if (fabs (zInFeatRef) < 1E-30)
00190       zInFeatRef  = 0;
00191 
00192 
00193     unsigned char bit4 = ((yInFeatRef > 0) || ((yInFeatRef == 0.0) && (xInFeatRef < 0))) ? 1 : 0;
00194     unsigned char bit3 = ((xInFeatRef > 0) || ((xInFeatRef == 0.0) && (yInFeatRef > 0))) ? !bit4 : bit4;
00195 
00196     assert (bit3 == 0 || bit3 == 1);
00197 
00198     int desc_index = (bit4<<3) + (bit3<<2);
00199 
00200     desc_index = desc_index << 1;
00201 
00202     if ((xInFeatRef * yInFeatRef > 0) || (xInFeatRef == 0.0))
00203       desc_index += (fabs (xInFeatRef) >= fabs (yInFeatRef)) ? 0 : 4;
00204     else
00205       desc_index += (fabs (xInFeatRef) > fabs (yInFeatRef)) ? 4 : 0;
00206 
00207     desc_index += zInFeatRef > 0 ? 1 : 0;
00208 
00209     // 2 RADII
00210     desc_index += (distance > radius1_2_) ? 2 : 0;
00211 
00212     int step_index = static_cast<int>(floor (binDistance[i_idx] +0.5));
00213     int volume_index = desc_index * (nr_bins+1);
00214 
00215     //Interpolation on the cosine (adjacent bins in the histogram)
00216     binDistance[i_idx] -= step_index;
00217     double intWeight = (1- fabs (binDistance[i_idx]));
00218 
00219     if (binDistance[i_idx] > 0)
00220       shot[volume_index + ((step_index+1) % nr_bins)] += binDistance[i_idx];
00221     else
00222       shot[volume_index + ((step_index - 1 + nr_bins) % nr_bins)] += - binDistance[i_idx];
00223 
00224     //Interpolation on the distance (adjacent husks)
00225    
00226     if (distance > radius1_2_)   //external sphere
00227     {
00228       double radiusDistance = (distance - radius3_4_) / radius1_2_;
00229 
00230       if (distance > radius3_4_) //most external sector, votes only for itself
00231         intWeight += 1 - radiusDistance;  //peso=1-d
00232       else  //3/4 of radius, votes also for the internal sphere
00233       {
00234         intWeight += 1 + radiusDistance;
00235         shot[(desc_index - 2) * (nr_bins+1) + step_index] -= radiusDistance;
00236     }
00237     }
00238     else    //internal sphere
00239     {
00240       double radiusDistance = (distance - radius1_4_) / radius1_2_;
00241 
00242       if (distance < radius1_4_) //most internal sector, votes only for itself
00243         intWeight += 1 + radiusDistance;  //weight=1-d
00244       else  //3/4 of radius, votes also for the external sphere
00245       {
00246         intWeight += 1 - radiusDistance;
00247         shot[(desc_index + 2) * (nr_bins+1) + step_index] += radiusDistance;
00248     }
00249     }
00250 
00251     //Interpolation on the inclination (adjacent vertical volumes)
00252     double inclinationCos = zInFeatRef / distance;
00253     if (inclinationCos < - 1.0)
00254       inclinationCos = - 1.0;
00255     if (inclinationCos > 1.0)
00256       inclinationCos = 1.0;
00257 
00258     double inclination = acos (inclinationCos);
00259 
00260     assert (inclination >= 0.0 && inclination <= PST_RAD_180);
00261 
00262     if (inclination > PST_RAD_90 || (fabs (inclination - PST_RAD_90) < 1e-30 && zInFeatRef <= 0))
00263     {
00264       double inclinationDistance = (inclination - PST_RAD_135) / PST_RAD_90;
00265       if (inclination > PST_RAD_135)
00266         intWeight += 1 - inclinationDistance;
00267       else
00268       {
00269         intWeight += 1 + inclinationDistance;
00270         assert ((desc_index + 1) * (nr_bins+1) + step_index >= 0 && (desc_index + 1) * (nr_bins+1) + step_index < descLength_);
00271         shot[(desc_index + 1) * (nr_bins+1) + step_index] -= inclinationDistance;
00272     }
00273     }
00274     else
00275     {
00276       double inclinationDistance = (inclination - PST_RAD_45) / PST_RAD_90;
00277       if (inclination < PST_RAD_45)
00278         intWeight += 1 + inclinationDistance;
00279       else
00280       {
00281         intWeight += 1 - inclinationDistance;
00282         assert ((desc_index - 1) * (nr_bins+1) + step_index >= 0 && (desc_index - 1) * (nr_bins+1) + step_index < descLength_);
00283         shot[(desc_index - 1) * (nr_bins+1) + step_index] += inclinationDistance;
00284     }
00285     }
00286 
00287     if (yInFeatRef != 0.0 || xInFeatRef != 0.0)
00288     {
00289       //Interpolation on the azimuth (adjacent horizontal volumes)
00290       double azimuth = atan2 (yInFeatRef, xInFeatRef);
00291 
00292       int sel = desc_index >> 2;
00293       double angularSectorSpan = PST_RAD_45;
00294       double angularSectorStart = - PST_RAD_PI_7_8;
00295 
00296       double azimuthDistance = (azimuth - (angularSectorStart + angularSectorSpan*sel)) / angularSectorSpan;
00297 
00298       assert ((azimuthDistance < 0.5 || areEquals (azimuthDistance, 0.5)) && (azimuthDistance > - 0.5 || areEquals (azimuthDistance, - 0.5)));
00299 
00300       azimuthDistance = (std::max)(- 0.5, std::min (azimuthDistance, 0.5));
00301 
00302       if (azimuthDistance > 0)
00303       {
00304         intWeight += 1 - azimuthDistance;
00305         int interp_index = (desc_index + 4) % maxAngularSectors_;
00306         assert (interp_index * (nr_bins+1) + step_index >= 0 && interp_index * (nr_bins+1) + step_index < descLength_);
00307         shot[interp_index * (nr_bins+1) + step_index] += azimuthDistance;
00308       }
00309       else
00310       {
00311         int interp_index = (desc_index - 4 + maxAngularSectors_) % maxAngularSectors_;
00312         assert (interp_index * (nr_bins+1) + step_index >= 0 && interp_index * (nr_bins+1) + step_index < descLength_);
00313         intWeight += 1 + azimuthDistance;
00314         shot[interp_index * (nr_bins+1) + step_index] -= azimuthDistance;
00315       }
00316 
00317     }
00318 
00319     assert (volume_index + step_index >= 0 &&  volume_index + step_index < descLength_);
00320     shot[volume_index + step_index] += intWeight;
00321 
00322   }
00323 }
00324 
00326 // Quadrilinear interpolation; used when color and shape descriptions are both activated
00327 template <typename PointNT, typename PointOutT> void
00328 pcl::SHOTEstimation<pcl::PointXYZRGBA, PointNT, PointOutT>::interpolateDoubleChannel (
00329   const std::vector<int> &indices, 
00330   const std::vector<float> &dists,
00331   const Eigen::Vector4f &central_point, 
00332   const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &rf,
00333   std::vector<double> &binDistanceShape,
00334   std::vector<double> &binDistanceColor, 
00335   const int nr_bins_shape, 
00336   const int nr_bins_color, 
00337   Eigen::VectorXf &shot)
00338 {
00339   if (rf.size () != 3)
00340   {
00341     PCL_ERROR ("[pcl::%s::interpolateDoubleChannel] RF size different than 9! Aborting...\n");
00342     return;
00343   }
00344 
00345   int shapeToColorStride = nr_grid_sector_*(nr_bins_shape+1);
00346 
00347   for (size_t i_idx = 0; i_idx < indices.size (); ++i_idx)
00348   {
00349     Eigen::Vector4f delta = surface_->points[indices[i_idx]].getVector4fMap () - central_point;
00350   delta[3] = 0;
00351 
00352     // Compute the Euclidean norm
00353     double distance = sqrt (dists[i_idx]);
00354 
00355     if (areEquals (distance, 0.0))
00356       continue;
00357 
00358     double xInFeatRef = delta.dot (rf[0]); //(x * feat[i].rf[0] + y * feat[i].rf[1] + z * feat[i].rf[2]);
00359     double yInFeatRef = delta.dot (rf[1]); //(x * feat[i].rf[3] + y * feat[i].rf[4] + z * feat[i].rf[5]);
00360     double zInFeatRef = delta.dot (rf[2]); //(x * feat[i].rf[6] + y * feat[i].rf[7] + z * feat[i].rf[8]);
00361 
00362     // To avoid numerical problems afterwards
00363     if (fabs (yInFeatRef) < 1E-30)
00364       yInFeatRef  = 0;
00365     if (fabs (xInFeatRef) < 1E-30)
00366       xInFeatRef  = 0;
00367     if (fabs (zInFeatRef) < 1E-30)
00368       zInFeatRef  = 0;
00369 
00370     unsigned char bit4 = ((yInFeatRef > 0) || ((yInFeatRef == 0.0) && (xInFeatRef < 0))) ? 1 : 0;
00371     unsigned char bit3 = ((xInFeatRef > 0) || ((xInFeatRef == 0.0) && (yInFeatRef > 0))) ? !bit4 : bit4;
00372 
00373     assert (bit3 == 0 || bit3 == 1);
00374 
00375     int desc_index = (bit4<<3) + (bit3<<2);
00376 
00377     desc_index = desc_index << 1;
00378 
00379     if ((xInFeatRef * yInFeatRef > 0) || (xInFeatRef == 0.0))
00380       desc_index += (fabs (xInFeatRef) >= fabs (yInFeatRef)) ? 0 : 4;
00381     else
00382       desc_index += (fabs (xInFeatRef) > fabs (yInFeatRef)) ? 4 : 0;
00383 
00384     desc_index += zInFeatRef > 0 ? 1 : 0;
00385 
00386     // 2 RADII
00387     desc_index += (distance > radius1_2_) ? 2 : 0;
00388 
00389     int step_index_shape = static_cast<int>(floor (binDistanceShape[i_idx] +0.5));
00390     int step_index_color = static_cast<int>(floor (binDistanceColor[i_idx] +0.5));
00391 
00392     int volume_index_shape = desc_index * (nr_bins_shape+1);
00393     int volume_index_color = shapeToColorStride + desc_index * (nr_bins_color+1);
00394 
00395     //Interpolation on the cosine (adjacent bins in the histrogram)
00396     binDistanceShape[i_idx] -= step_index_shape;
00397     binDistanceColor[i_idx] -= step_index_color;
00398 
00399     double intWeightShape = (1- fabs (binDistanceShape[i_idx]));
00400     double intWeightColor = (1- fabs (binDistanceColor[i_idx]));
00401 
00402     if (binDistanceShape[i_idx] > 0)
00403       shot[volume_index_shape + ((step_index_shape + 1) % nr_bins_shape)] += binDistanceShape[i_idx];
00404     else
00405       shot[volume_index_shape + ((step_index_shape - 1 + nr_bins_shape) % nr_bins_shape)] -= binDistanceShape[i_idx];
00406 
00407     if (binDistanceColor[i_idx] > 0)
00408       shot[volume_index_color + ((step_index_color+1) % nr_bins_color)] += binDistanceColor[i_idx];
00409     else
00410       shot[volume_index_color + ((step_index_color - 1 + nr_bins_color) % nr_bins_color)] -= binDistanceColor[i_idx];
00411 
00412     //Interpolation on the distance (adjacent husks)
00413    
00414     if (distance > radius1_2_)   //external sphere
00415     {
00416       double radiusDistance = (distance - radius3_4_) / radius1_2_;
00417 
00418       if (distance > radius3_4_) //most external sector, votes only for itself
00419       {
00420         intWeightShape += 1 - radiusDistance; //weight=1-d
00421         intWeightColor += 1 - radiusDistance; //weight=1-d
00422       }
00423       else  //3/4 of radius, votes also for the internal sphere
00424       {
00425         intWeightShape += 1 + radiusDistance;
00426         intWeightColor += 1 + radiusDistance;
00427         shot[(desc_index - 2) * (nr_bins_shape+1) + step_index_shape] -= radiusDistance;
00428         shot[shapeToColorStride + (desc_index - 2) * (nr_bins_color+1) + step_index_color] -= radiusDistance;
00429       }
00430     }
00431     else    //internal sphere
00432     {
00433       double radiusDistance = (distance - radius1_4_) / radius1_2_;
00434 
00435       if (distance < radius1_4_) //most internal sector, votes only for itself
00436       {
00437         intWeightShape += 1 + radiusDistance;
00438         intWeightColor += 1 + radiusDistance; //weight=1-d
00439       }
00440       else  //3/4 of radius, votes also for the external sphere
00441       {
00442         intWeightShape += 1 - radiusDistance; //weight=1-d
00443         intWeightColor += 1 - radiusDistance; //weight=1-d
00444         shot[(desc_index + 2) * (nr_bins_shape+1) + step_index_shape] += radiusDistance;
00445         shot[shapeToColorStride + (desc_index + 2) * (nr_bins_color+1) + step_index_color] += radiusDistance;
00446       }
00447     }
00448 
00449     //Interpolation on the inclination (adjacent vertical volumes)
00450     double inclinationCos = zInFeatRef / distance;
00451     if (inclinationCos < - 1.0)
00452       inclinationCos = - 1.0;
00453     if (inclinationCos > 1.0)
00454       inclinationCos = 1.0;
00455 
00456     double inclination = acos (inclinationCos);
00457 
00458     assert (inclination >= 0.0 && inclination <= PST_RAD_180);
00459 
00460     if (inclination > PST_RAD_90 || (fabs (inclination - PST_RAD_90) < 1e-30 && zInFeatRef <= 0))
00461     {
00462       double inclinationDistance = (inclination - PST_RAD_135) / PST_RAD_90;
00463       if (inclination > PST_RAD_135)
00464       {
00465         intWeightShape += 1 - inclinationDistance;
00466         intWeightColor += 1 - inclinationDistance;
00467       }
00468       else
00469       {
00470         intWeightShape += 1 + inclinationDistance;
00471         intWeightColor += 1 + inclinationDistance;
00472         assert ((desc_index + 1) * (nr_bins_shape+1) + step_index_shape >= 0 && (desc_index + 1) * (nr_bins_shape+1) + step_index_shape < descLength_);
00473         assert (shapeToColorStride + (desc_index + 1) * (nr_bins_color+ 1) + step_index_color >= 0 && shapeToColorStride + (desc_index + 1) * (nr_bins_color+1) + step_index_color < descLength_);
00474         shot[(desc_index + 1) * (nr_bins_shape+1) + step_index_shape] -= inclinationDistance;
00475         shot[shapeToColorStride + (desc_index + 1) * (nr_bins_color+1) + step_index_color] -= inclinationDistance;
00476       }
00477     }
00478     else
00479     {
00480       double inclinationDistance = (inclination - PST_RAD_45) / PST_RAD_90;
00481       if (inclination < PST_RAD_45)
00482       {
00483         intWeightShape += 1 + inclinationDistance;
00484         intWeightColor += 1 + inclinationDistance;
00485       }
00486       else
00487       {
00488         intWeightShape += 1 - inclinationDistance;
00489         intWeightColor += 1 - inclinationDistance;
00490         assert ((desc_index - 1) * (nr_bins_shape+1) + step_index_shape >= 0 && (desc_index - 1) * (nr_bins_shape+1) + step_index_shape < descLength_);
00491         assert (shapeToColorStride + (desc_index - 1) * (nr_bins_color+ 1) + step_index_color >= 0 && shapeToColorStride + (desc_index - 1) * (nr_bins_color+1) + step_index_color < descLength_);
00492         shot[(desc_index - 1) * (nr_bins_shape+1) + step_index_shape] += inclinationDistance;
00493         shot[shapeToColorStride + (desc_index - 1) * (nr_bins_color+1) + step_index_color] += inclinationDistance;
00494       }
00495     }
00496 
00497     if (yInFeatRef != 0.0 || xInFeatRef != 0.0)
00498     {
00499       //Interpolation on the azimuth (adjacent horizontal volumes)
00500       double azimuth = atan2 (yInFeatRef, xInFeatRef);
00501 
00502       int sel = desc_index >> 2;
00503       double angularSectorSpan = PST_RAD_45;
00504       double angularSectorStart = - PST_RAD_PI_7_8;
00505 
00506       double azimuthDistance = (azimuth - (angularSectorStart + angularSectorSpan*sel)) / angularSectorSpan;
00507       assert ((azimuthDistance < 0.5 || areEquals (azimuthDistance, 0.5)) && (azimuthDistance > - 0.5 || areEquals (azimuthDistance, - 0.5)));
00508       azimuthDistance = (std::max)(- 0.5, std::min (azimuthDistance, 0.5));
00509 
00510       if (azimuthDistance > 0)
00511       {
00512         intWeightShape += 1 - azimuthDistance;
00513         intWeightColor += 1 - azimuthDistance;
00514         int interp_index = (desc_index + 4) % maxAngularSectors_;
00515         assert (interp_index * (nr_bins_shape+1) + step_index_shape >= 0 && interp_index * (nr_bins_shape+1) + step_index_shape < descLength_);
00516         assert (shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color >= 0 && shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color < descLength_);
00517         shot[interp_index * (nr_bins_shape+1) + step_index_shape] += azimuthDistance;
00518         shot[shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color] += azimuthDistance;
00519       }
00520       else
00521       {
00522         int interp_index = (desc_index - 4 + maxAngularSectors_) % maxAngularSectors_;
00523         intWeightShape += 1 + azimuthDistance;
00524         intWeightColor += 1 + azimuthDistance;
00525         assert (interp_index * (nr_bins_shape+1) + step_index_shape >= 0 && interp_index * (nr_bins_shape+1) + step_index_shape < descLength_);
00526         assert (shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color >= 0 && shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color < descLength_);
00527         shot[interp_index * (nr_bins_shape+1) + step_index_shape] -= azimuthDistance;
00528         shot[shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color] -= azimuthDistance;
00529       }
00530     }
00531 
00532     assert (volume_index_shape + step_index_shape >= 0 &&  volume_index_shape + step_index_shape < descLength_);
00533     assert (volume_index_color + step_index_color >= 0 &&  volume_index_color + step_index_color < descLength_);
00534     shot[volume_index_shape + step_index_shape] += intWeightShape;
00535     shot[volume_index_color + step_index_color] += intWeightColor;
00536   }
00537 }
00538 
00539 
00541 template <typename PointNT, typename PointOutT> void
00542 pcl::SHOTEstimation<pcl::PointXYZRGBA, PointNT, PointOutT>::computePointSHOT (
00543   const int index, const std::vector<int> &indices, const std::vector<float> &dists, Eigen::VectorXf &shot,
00544   std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &rf)
00545 {
00546   if (rf.size () != 3)
00547     rf.resize (3);
00548 
00549   // Clear the resultant shot
00550   shot.setZero ();
00551 
00552   std::vector<double> binDistanceShape;
00553   std::vector<double> binDistanceColor;
00554 
00555   int nNeighbors = indices.size ();
00556 
00557   //Skip the current feature if the number of its neighbors is not sufficient for its description
00558   if (nNeighbors < 5)
00559   {
00560     PCL_WARN ("[pcl::%s::computePointSHOT] Warning! Neighborhood has less than 5 vertexes. Aborting description of point with index %d\n", getClassName ().c_str (), index);
00561     return;
00562   }
00563 
00564   //Compute the local Reference Frame for the current 3D point
00565   Eigen::Vector4f central_point = input_->points[index].getVector4fMap ();
00566   central_point[3] = 0;
00567   if (pcl::getLocalRF (*surface_, search_radius_, central_point, indices, dists, rf))
00568     return;
00569 
00570   //If shape description is enabled, compute the bins activated by each neighbor of the current feature in the shape histogram
00571   if (b_describe_shape_)
00572   {
00573     binDistanceShape.resize (nNeighbors);
00574 
00575     for (size_t i_idx = 0; i_idx < indices.size (); ++i_idx)
00576     {
00577       double cosineDesc = normals_->points[indices[i_idx]].getNormalVector4fMap ().dot (rf[2]); //feat[i].rf[6]*normal[0] + feat[i].rf[7]*normal[1] + feat[i].rf[8]*normal[2];
00578 
00579       if (cosineDesc > 1.0)
00580         cosineDesc = 1.0;
00581       if (cosineDesc < - 1.0)
00582         cosineDesc = - 1.0;
00583 
00584       binDistanceShape[i_idx] = ((1.0 + cosineDesc) * nr_shape_bins_) / 2;
00585     }
00586   }
00587 
00588   //If color description is enabled, compute the bins activated by each neighbor of the current feature in the color histogram
00589   if (b_describe_color_)
00590   {
00591     binDistanceColor.resize (nNeighbors);
00592 
00593     unsigned char redRef = input_->points[index].rgba >> 16 & 0xFF;
00594     unsigned char greenRef = input_->points[index].rgba >> 8& 0xFF;
00595     unsigned char blueRef = input_->points[index].rgba & 0xFF;
00596 
00597     float LRef, aRef, bRef;
00598 
00599     RGB2CIELAB (redRef, greenRef, blueRef, LRef, aRef, bRef);
00600     LRef /= 100.0;
00601     aRef /= 120.0;
00602     bRef /= 120.0;    //normalized LAB components (0<L<1, -1<a<1, -1<b<1)
00603 
00604     for (size_t i_idx = 0; i_idx < indices.size (); ++i_idx)
00605     {
00606       unsigned char red = surface_->points[indices[i_idx]].rgba >> 16 & 0xFF;
00607       unsigned char green = surface_->points[indices[i_idx]].rgba >> 8 & 0xFF;
00608       unsigned char blue = surface_->points[indices[i_idx]].rgba & 0xFF;
00609 
00610       float L, a, b;
00611 
00612       RGB2CIELAB (red, green, blue, L, a, b);
00613       L /= 100.0;
00614       a /= 120.0;
00615       b /= 120.0;   //normalized LAB components (0<L<1, -1<a<1, -1<b<1)
00616 
00617       double colorDistance = (fabs (LRef - L) + ((fabs (aRef - a) + fabs (bRef - b)) / 2)) /3;
00618 
00619       if (colorDistance > 1.0)
00620         colorDistance = 1.0;
00621       if (colorDistance < 0.0)
00622         colorDistance = 0.0;
00623 
00624       binDistanceColor[i_idx] = colorDistance * nr_color_bins_;
00625     }
00626   }
00627 
00628   //Apply quadrilinear interpolation on the activated bins in the shape and/or color histogram(s)
00629 
00630   if (b_describe_shape_ && b_describe_color_)
00631     interpolateDoubleChannel (indices, dists, input_->points[index].getVector4fMap (), rf, binDistanceShape, binDistanceColor,
00632                               nr_shape_bins_, nr_color_bins_,
00633                               shot);
00634   else if (b_describe_color_)
00635     interpolateSingleChannel (indices, dists, input_->points[index].getVector4fMap (), rf, binDistanceColor, nr_color_bins_, shot);
00636   else
00637     interpolateSingleChannel (indices, dists, input_->points[index].getVector4fMap (), rf, binDistanceShape, nr_shape_bins_, shot);
00638 
00639   //Normalize the final histogram
00640   double accNorm = 0;
00641   for (int j=0; j< descLength_; j++)
00642     accNorm += shot[j]*shot[j];
00643 
00644   accNorm = sqrt (accNorm);
00645 
00646   for (int j=0; j< descLength_; j++)
00647     shot[j] /= accNorm;
00648 }
00649 
00650 
00652 template <typename PointInT, typename PointNT, typename PointOutT> void
00653 pcl::SHOTEstimation<PointInT, PointNT, PointOutT>::computePointSHOT (
00654   const int index, const std::vector<int> &indices, const std::vector<float> &dists, Eigen::VectorXf &shot,
00655   std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &rf)
00656 {
00657   if (rf.size () != 3)
00658     rf.resize (3);
00659 
00660    // Clear the resultant shot
00661   shot.setZero ();
00662 
00663   std::vector<double> binDistanceShape;
00664 
00665   int nNeighbors = indices.size ();
00666 
00667   if (nNeighbors < 5)
00668   {
00669     PCL_WARN ("[pcl::%s::computePointSHOT] Warning! Neighborhood has less than 5 vertexes. Aborting description of point with index %d\n", getClassName ().c_str (), index);
00670     return;
00671   }
00672 
00673   Eigen::Vector4f central_point = input_->points[index].getVector4fMap ();
00674   central_point[3] = 0;
00675   if (pcl::getLocalRF (*surface_, search_radius_, central_point, indices, dists, rf))
00676     return;
00677 
00678 
00679   binDistanceShape.resize (nNeighbors);
00680 
00681   for (size_t i_idx = 0; i_idx < indices.size (); ++i_idx)
00682   {
00683     double cosineDesc = normals_->points[indices[i_idx]].getNormalVector4fMap ().dot (rf[2]); //feat[i].rf[6]*normal[0] + feat[i].rf[7]*normal[1] + feat[i].rf[8]*normal[2];
00684 
00685     if (cosineDesc > 1.0)
00686       cosineDesc = 1.0;
00687     if (cosineDesc < - 1.0)
00688       cosineDesc = - 1.0;
00689 
00690     binDistanceShape[i_idx] = ((1.0 + cosineDesc) * nr_shape_bins_) / 2;
00691 
00692   }
00693 
00694   interpolateSingleChannel (indices, dists, input_->points[index].getVector4fMap (), rf, binDistanceShape, nr_shape_bins_, shot);
00695 
00696   double accNorm = 0;
00697   for (int j=0; j< descLength_; j++)
00698     accNorm += shot[j]*shot[j];
00699 
00700   accNorm = sqrt (accNorm);
00701 
00702   for (int j=0; j< descLength_; j++)
00703     shot[j] /= accNorm;
00704 
00705 }
00706 
00707 
00709 template <typename PointNT, typename PointOutT> void
00710 pcl::SHOTEstimation<pcl::PointXYZRGBA, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
00711 {
00712   if (rf_.size () != 3)
00713     rf_.resize (3);
00714 
00715   // Compute the current length of the descriptor
00716   descLength_ = (b_describe_shape_) ? nr_grid_sector_*(nr_shape_bins_+1) : 0;
00717   descLength_ +=   (b_describe_color_) ? nr_grid_sector_*(nr_color_bins_+1) : 0;
00718 
00719   // Useful values
00720   sqradius_ = search_radius_*search_radius_;
00721   radius3_4_ = (search_radius_*3) / 4;
00722   radius1_4_ = search_radius_ / 4;
00723   radius1_2_ = search_radius_ / 2;
00724 
00725   shot_.setZero (descLength_);
00726   rf_[0].setZero ();
00727   rf_[1].setZero ();
00728   rf_[2].setZero ();
00729 
00730   if (output.points[0].descriptor.size () != (size_t)descLength_)
00731     for (size_t idx = 0; idx < indices_->size (); ++idx)
00732       output.points[idx].descriptor.resize (descLength_);
00733 //  if (output.points[0].size != (size_t)descLength_)
00734 //  {
00735 //    PCL_ERROR ("[pcl::%s::computeFeature] The desired descriptor size (%lu) does not match the output point type feature (%lu)! Aborting...\n", getClassName ().c_str (), descLength_, (unsigned long) output.points[0].size);
00736 //    return;
00737 //  }
00738 
00739   // Allocate enough space to hold the results
00740   // \note This resize is irrelevant for a radiusSearch ().
00741   std::vector<int> nn_indices (k_);
00742   std::vector<float> nn_dists (k_);
00743 
00744   // Iterating over the entire index vector
00745   for (size_t idx = 0; idx < indices_->size (); ++idx)
00746   {
00747     this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists);
00748 
00749     // Compute the SHOT descriptor for the current 3D feature
00750     computePointSHOT ((*indices_)[idx], nn_indices, nn_dists, shot_, rf_);
00751 
00752     // Copy into the resultant cloud
00753     for (int d = 0; d < shot_.size (); ++d)
00754       output.points[idx].descriptor[d] = shot_[d];
00755     for (int d = 0; d < 9; ++d)
00756       output.points[idx].rf[d] = rf_[d/3][d%3];
00757   }
00758 }
00759 
00760 
00762 template <typename PointInT, typename PointNT, typename PointOutT> void
00763 pcl::SHOTEstimationBase<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
00764 {
00765   if (rf_.size () != 3)
00766     rf_.resize (3);
00767 
00768   descLength_ = nr_grid_sector_ * (nr_shape_bins_+1);
00769 
00770   sqradius_ = search_radius_ * search_radius_;
00771   radius3_4_ = (search_radius_*3) / 4;
00772   radius1_4_ = search_radius_ / 4;
00773   radius1_2_ = search_radius_ / 2;
00774 
00775   shot_.setZero (descLength_);
00776   rf_[0].setZero ();
00777   rf_[1].setZero ();
00778   rf_[2].setZero ();
00779 
00780   if (output.points[0].descriptor.size () != (size_t)descLength_)
00781     for (size_t idx = 0; idx < indices_->size (); ++idx)
00782       output.points[idx].descriptor.resize (descLength_);
00783 //  if (output.points[0].size != (size_t)descLength_)
00784 //  {
00785 //    PCL_ERROR ("[pcl::%s::computeFeature] The desired descriptor size (%lu) does not match the output point type feature (%lu)! Aborting...\n", getClassName ().c_str (), descLength_, (unsigned long) output.points[0].size);
00786 //    return;
00787 //  }
00788 
00789   // Allocate enough space to hold the results
00790   // \note This resize is irrelevant for a radiusSearch ().
00791   std::vector<int> nn_indices (k_);
00792   std::vector<float> nn_dists (k_);
00793 
00794   // Iterating over the entire index vector
00795   for (size_t idx = 0; idx < indices_->size (); ++idx)
00796   {
00797     this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists);
00798 
00799     // Estimate the SHOT at each patch
00800     computePointSHOT ((*indices_)[idx], nn_indices, nn_dists, shot_, rf_);
00801 
00802     // Copy into the resultant cloud
00803     for (int d = 0; d < shot_.size (); ++d)
00804       output.points[idx].descriptor[d] = shot_[d];
00805     for (int d = 0; d < 9; ++d)
00806       output.points[idx].rf[d] = rf_[d/3][d%3];
00807   }
00808 }
00809 
00810 #define PCL_INSTANTIATE_SHOTEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::SHOTEstimation<T,NT,OutT>;
00811 
00812 #endif    // PCL_FEATURES_IMPL_SHOT_H_
00813 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines