Point Cloud Library (PCL)
1.3.1
|
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 ¢ral_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 ¢ral_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