Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2010-2011, Willow Garage, Inc. 00006 * Copyright (c) 2012-, Open Perception, Inc. 00007 * 00008 * All rights reserved. 00009 * 00010 * Redistribution and use in source and binary forms, with or without 00011 * modification, are permitted provided that the following conditions 00012 * are met: 00013 * 00014 * * Redistributions of source code must retain the above copyright 00015 * notice, this list of conditions and the following disclaimer. 00016 * * Redistributions in binary form must reproduce the above 00017 * copyright notice, this list of conditions and the following 00018 * disclaimer in the documentation and/or other materials provided 00019 * with the distribution. 00020 * * Neither the name of the copyright holder(s) nor the names of its 00021 * contributors may be used to endorse or promote products derived 00022 * from this software without specific prior written permission. 00023 * 00024 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00025 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00026 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00027 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00028 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00029 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00030 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00031 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00032 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00033 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00034 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00035 * POSSIBILITY OF SUCH DAMAGE. 00036 * 00037 * 00038 */ 00039 00040 #ifndef PCL_FEATURES_IMPL_SHOT_H_ 00041 #define PCL_FEATURES_IMPL_SHOT_H_ 00042 00043 #include <pcl/features/shot.h> 00044 #include <pcl/features/shot_lrf.h> 00045 #include <utility> 00046 00047 // Useful constants. 00048 #define PST_PI 3.1415926535897932384626433832795 00049 #define PST_RAD_45 0.78539816339744830961566084581988 00050 #define PST_RAD_90 1.5707963267948966192313216916398 00051 #define PST_RAD_135 2.3561944901923449288469825374596 00052 #define PST_RAD_180 PST_PI 00053 #define PST_RAD_360 6.283185307179586476925286766558 00054 #define PST_RAD_PI_7_8 2.7488935718910690836548129603691 00055 00056 const double zeroDoubleEps15 = 1E-15; 00057 const float zeroFloatEps8 = 1E-8f; 00058 00059 ////////////////////////////////////////////////////////////////////////////////////////////// 00060 /** \brief Check if val1 and val2 are equals. 00061 * 00062 * \param[in] val1 first number to check. 00063 * \param[in] val2 second number to check. 00064 * \param[in] zeroDoubleEps epsilon 00065 * \return true if val1 is equal to val2, false otherwise. 00066 */ 00067 inline bool 00068 areEquals (double val1, double val2, double zeroDoubleEps = zeroDoubleEps15) 00069 { 00070 return (fabs (val1 - val2)<zeroDoubleEps); 00071 } 00072 00073 ////////////////////////////////////////////////////////////////////////////////////////////// 00074 /** \brief Check if val1 and val2 are equals. 00075 * 00076 * \param[in] val1 first number to check. 00077 * \param[in] val2 second number to check. 00078 * \param[in] zeroFloatEps epsilon 00079 * \return true if val1 is equal to val2, false otherwise. 00080 */ 00081 inline bool 00082 areEquals (float val1, float val2, float zeroFloatEps = zeroFloatEps8) 00083 { 00084 return (fabs (val1 - val2)<zeroFloatEps); 00085 } 00086 00087 ////////////////////////////////////////////////////////////////////////////////////////////// 00088 template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> float 00089 pcl::SHOTColorEstimation<PointInT, PointNT, PointOutT, PointRFT>::sRGB_LUT[256] = {- 1}; 00090 00091 ////////////////////////////////////////////////////////////////////////////////////////////// 00092 template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> float 00093 pcl::SHOTColorEstimation<PointInT, PointNT, PointOutT, PointRFT>::sXYZ_LUT[4000] = {- 1}; 00094 00095 ////////////////////////////////////////////////////////////////////////////////////////////// 00096 template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void 00097 pcl::SHOTColorEstimation<PointInT, PointNT, PointOutT, PointRFT>::RGB2CIELAB (unsigned char R, unsigned char G, 00098 unsigned char B, float &L, float &A, 00099 float &B2) 00100 { 00101 if (sRGB_LUT[0] < 0) 00102 { 00103 for (int i = 0; i < 256; i++) 00104 { 00105 float f = static_cast<float> (i) / 255.0f; 00106 if (f > 0.04045) 00107 sRGB_LUT[i] = powf ((f + 0.055f) / 1.055f, 2.4f); 00108 else 00109 sRGB_LUT[i] = f / 12.92f; 00110 } 00111 00112 for (int i = 0; i < 4000; i++) 00113 { 00114 float f = static_cast<float> (i) / 4000.0f; 00115 if (f > 0.008856) 00116 sXYZ_LUT[i] = static_cast<float> (powf (f, 0.3333f)); 00117 else 00118 sXYZ_LUT[i] = static_cast<float>((7.787 * f) + (16.0 / 116.0)); 00119 } 00120 } 00121 00122 float fr = sRGB_LUT[R]; 00123 float fg = sRGB_LUT[G]; 00124 float fb = sRGB_LUT[B]; 00125 00126 // Use white = D65 00127 const float x = fr * 0.412453f + fg * 0.357580f + fb * 0.180423f; 00128 const float y = fr * 0.212671f + fg * 0.715160f + fb * 0.072169f; 00129 const float z = fr * 0.019334f + fg * 0.119193f + fb * 0.950227f; 00130 00131 float vx = x / 0.95047f; 00132 float vy = y; 00133 float vz = z / 1.08883f; 00134 00135 vx = sXYZ_LUT[int(vx*4000)]; 00136 vy = sXYZ_LUT[int(vy*4000)]; 00137 vz = sXYZ_LUT[int(vz*4000)]; 00138 00139 L = 116.0f * vy - 16.0f; 00140 if (L > 100) 00141 L = 100.0f; 00142 00143 A = 500.0f * (vx - vy); 00144 if (A > 120) 00145 A = 120.0f; 00146 else if (A <- 120) 00147 A = -120.0f; 00148 00149 B2 = 200.0f * (vy - vz); 00150 if (B2 > 120) 00151 B2 = 120.0f; 00152 else if (B2<- 120) 00153 B2 = -120.0f; 00154 } 00155 00156 ////////////////////////////////////////////////////////////////////////////////////////////// 00157 template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> bool 00158 pcl::SHOTEstimationBase<PointInT, PointNT, PointOutT, PointRFT>::initCompute () 00159 { 00160 if (!FeatureFromNormals<PointInT, PointNT, PointOutT>::initCompute ()) 00161 { 00162 PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ()); 00163 return (false); 00164 } 00165 00166 // SHOT cannot work with k-search 00167 if (this->getKSearch () != 0) 00168 { 00169 PCL_ERROR( 00170 "[pcl::%s::initCompute] Error! Search method set to k-neighborhood. Call setKSearch(0) and setRadiusSearch( radius ) to use this class.\n", 00171 getClassName().c_str ()); 00172 return (false); 00173 } 00174 00175 // Default LRF estimation alg: SHOTLocalReferenceFrameEstimation 00176 typename SHOTLocalReferenceFrameEstimation<PointInT, PointRFT>::Ptr lrf_estimator(new SHOTLocalReferenceFrameEstimation<PointInT, PointRFT>()); 00177 lrf_estimator->setRadiusSearch ((lrf_radius_ > 0 ? lrf_radius_ : search_radius_)); 00178 lrf_estimator->setInputCloud (input_); 00179 lrf_estimator->setIndices (indices_); 00180 if (!fake_surface_) 00181 lrf_estimator->setSearchSurface(surface_); 00182 00183 if (!FeatureWithLocalReferenceFrames<PointInT, PointRFT>::initLocalReferenceFrames (indices_->size (), lrf_estimator)) 00184 { 00185 PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ()); 00186 return (false); 00187 } 00188 00189 return (true); 00190 } 00191 00192 ////////////////////////////////////////////////////////////////////////////////////////////// 00193 template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void 00194 pcl::SHOTEstimationBase<PointInT, PointNT, PointOutT, PointRFT>::createBinDistanceShape ( 00195 int index, 00196 const std::vector<int> &indices, 00197 std::vector<double> &bin_distance_shape) 00198 { 00199 bin_distance_shape.resize (indices.size ()); 00200 00201 const PointRFT& current_frame = frames_->points[index]; 00202 //if (!pcl_isfinite (current_frame.rf[0]) || !pcl_isfinite (current_frame.rf[4]) || !pcl_isfinite (current_frame.rf[11])) 00203 //return; 00204 00205 Eigen::Vector4f current_frame_z (current_frame.z_axis[0], current_frame.z_axis[1], current_frame.z_axis[2], 0); 00206 00207 unsigned nan_counter = 0; 00208 for (size_t i_idx = 0; i_idx < indices.size (); ++i_idx) 00209 { 00210 // check NaN normal 00211 const Eigen::Vector4f& normal_vec = normals_->points[indices[i_idx]].getNormalVector4fMap (); 00212 if (!pcl_isfinite (normal_vec[0]) || 00213 !pcl_isfinite (normal_vec[1]) || 00214 !pcl_isfinite (normal_vec[2])) 00215 { 00216 bin_distance_shape[i_idx] = std::numeric_limits<double>::quiet_NaN (); 00217 ++nan_counter; 00218 } else 00219 { 00220 //double cosineDesc = feat[i].rf[6]*normal[0] + feat[i].rf[7]*normal[1] + feat[i].rf[8]*normal[2]; 00221 double cosineDesc = normal_vec.dot (current_frame_z); 00222 00223 if (cosineDesc > 1.0) 00224 cosineDesc = 1.0; 00225 if (cosineDesc < - 1.0) 00226 cosineDesc = - 1.0; 00227 00228 bin_distance_shape[i_idx] = ((1.0 + cosineDesc) * nr_shape_bins_) / 2; 00229 } 00230 } 00231 if (nan_counter > 0) 00232 PCL_WARN ("[pcl::%s::createBinDistanceShape] Point %d has %d (%f%%) NaN normals in its neighbourhood\n", 00233 getClassName ().c_str (), index, nan_counter, (static_cast<float>(nan_counter)*100.f/static_cast<float>(indices.size ()))); 00234 } 00235 00236 ////////////////////////////////////////////////////////////////////////////////////////////// 00237 template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void 00238 pcl::SHOTEstimationBase<PointInT, PointNT, PointOutT, PointRFT>::normalizeHistogram ( 00239 Eigen::VectorXf &shot, int desc_length) 00240 { 00241 // Normalization is performed by considering the L2 norm 00242 // and not the sum of bins, as reported in the ECCV paper. 00243 // This is due to additional experiments performed by the authors after its pubblication, 00244 // where L2 normalization turned out better at handling point density variations. 00245 double acc_norm = 0; 00246 for (int j = 0; j < desc_length; j++) 00247 acc_norm += shot[j] * shot[j]; 00248 acc_norm = sqrt (acc_norm); 00249 for (int j = 0; j < desc_length; j++) 00250 shot[j] /= static_cast<float> (acc_norm); 00251 } 00252 00253 ////////////////////////////////////////////////////////////////////////////////////////////// 00254 template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void 00255 pcl::SHOTEstimationBase<PointInT, PointNT, PointOutT, PointRFT>::interpolateSingleChannel ( 00256 const std::vector<int> &indices, 00257 const std::vector<float> &sqr_dists, 00258 const int index, 00259 std::vector<double> &binDistance, 00260 const int nr_bins, 00261 Eigen::VectorXf &shot) 00262 { 00263 const Eigen::Vector4f& central_point = (*input_)[(*indices_)[index]].getVector4fMap (); 00264 const PointRFT& current_frame = (*frames_)[index]; 00265 00266 Eigen::Vector4f current_frame_x (current_frame.x_axis[0], current_frame.x_axis[1], current_frame.x_axis[2], 0); 00267 Eigen::Vector4f current_frame_y (current_frame.y_axis[0], current_frame.y_axis[1], current_frame.y_axis[2], 0); 00268 Eigen::Vector4f current_frame_z (current_frame.z_axis[0], current_frame.z_axis[1], current_frame.z_axis[2], 0); 00269 00270 for (size_t i_idx = 0; i_idx < indices.size (); ++i_idx) 00271 { 00272 if (!pcl_isfinite(binDistance[i_idx])) 00273 continue; 00274 00275 Eigen::Vector4f delta = surface_->points[indices[i_idx]].getVector4fMap () - central_point; 00276 delta[3] = 0; 00277 00278 // Compute the Euclidean norm 00279 double distance = sqrt (sqr_dists[i_idx]); 00280 00281 if (areEquals (distance, 0.0)) 00282 continue; 00283 00284 double xInFeatRef = delta.dot (current_frame_x); 00285 double yInFeatRef = delta.dot (current_frame_y); 00286 double zInFeatRef = delta.dot (current_frame_z); 00287 00288 // To avoid numerical problems afterwards 00289 if (fabs (yInFeatRef) < 1E-30) 00290 yInFeatRef = 0; 00291 if (fabs (xInFeatRef) < 1E-30) 00292 xInFeatRef = 0; 00293 if (fabs (zInFeatRef) < 1E-30) 00294 zInFeatRef = 0; 00295 00296 00297 unsigned char bit4 = ((yInFeatRef > 0) || ((yInFeatRef == 0.0) && (xInFeatRef < 0))) ? 1 : 0; 00298 unsigned char bit3 = static_cast<unsigned char> (((xInFeatRef > 0) || ((xInFeatRef == 0.0) && (yInFeatRef > 0))) ? !bit4 : bit4); 00299 00300 assert (bit3 == 0 || bit3 == 1); 00301 00302 int desc_index = (bit4<<3) + (bit3<<2); 00303 00304 desc_index = desc_index << 1; 00305 00306 if ((xInFeatRef * yInFeatRef > 0) || (xInFeatRef == 0.0)) 00307 desc_index += (fabs (xInFeatRef) >= fabs (yInFeatRef)) ? 0 : 4; 00308 else 00309 desc_index += (fabs (xInFeatRef) > fabs (yInFeatRef)) ? 4 : 0; 00310 00311 desc_index += zInFeatRef > 0 ? 1 : 0; 00312 00313 // 2 RADII 00314 desc_index += (distance > radius1_2_) ? 2 : 0; 00315 00316 int step_index = static_cast<int>(floor (binDistance[i_idx] +0.5)); 00317 int volume_index = desc_index * (nr_bins+1); 00318 00319 //Interpolation on the cosine (adjacent bins in the histogram) 00320 binDistance[i_idx] -= step_index; 00321 double intWeight = (1- fabs (binDistance[i_idx])); 00322 00323 if (binDistance[i_idx] > 0) 00324 shot[volume_index + ((step_index+1) % nr_bins)] += static_cast<float> (binDistance[i_idx]); 00325 else 00326 shot[volume_index + ((step_index - 1 + nr_bins) % nr_bins)] += - static_cast<float> (binDistance[i_idx]); 00327 00328 //Interpolation on the distance (adjacent husks) 00329 00330 if (distance > radius1_2_) //external sphere 00331 { 00332 double radiusDistance = (distance - radius3_4_) / radius1_2_; 00333 00334 if (distance > radius3_4_) //most external sector, votes only for itself 00335 intWeight += 1 - radiusDistance; //peso=1-d 00336 else //3/4 of radius, votes also for the internal sphere 00337 { 00338 intWeight += 1 + radiusDistance; 00339 shot[(desc_index - 2) * (nr_bins+1) + step_index] -= static_cast<float> (radiusDistance); 00340 } 00341 } 00342 else //internal sphere 00343 { 00344 double radiusDistance = (distance - radius1_4_) / radius1_2_; 00345 00346 if (distance < radius1_4_) //most internal sector, votes only for itself 00347 intWeight += 1 + radiusDistance; //weight=1-d 00348 else //3/4 of radius, votes also for the external sphere 00349 { 00350 intWeight += 1 - radiusDistance; 00351 shot[(desc_index + 2) * (nr_bins+1) + step_index] += static_cast<float> (radiusDistance); 00352 } 00353 } 00354 00355 //Interpolation on the inclination (adjacent vertical volumes) 00356 double inclinationCos = zInFeatRef / distance; 00357 if (inclinationCos < - 1.0) 00358 inclinationCos = - 1.0; 00359 if (inclinationCos > 1.0) 00360 inclinationCos = 1.0; 00361 00362 double inclination = acos (inclinationCos); 00363 00364 assert (inclination >= 0.0 && inclination <= PST_RAD_180); 00365 00366 if (inclination > PST_RAD_90 || (fabs (inclination - PST_RAD_90) < 1e-30 && zInFeatRef <= 0)) 00367 { 00368 double inclinationDistance = (inclination - PST_RAD_135) / PST_RAD_90; 00369 if (inclination > PST_RAD_135) 00370 intWeight += 1 - inclinationDistance; 00371 else 00372 { 00373 intWeight += 1 + inclinationDistance; 00374 assert ((desc_index + 1) * (nr_bins+1) + step_index >= 0 && (desc_index + 1) * (nr_bins+1) + step_index < descLength_); 00375 shot[(desc_index + 1) * (nr_bins+1) + step_index] -= static_cast<float> (inclinationDistance); 00376 } 00377 } 00378 else 00379 { 00380 double inclinationDistance = (inclination - PST_RAD_45) / PST_RAD_90; 00381 if (inclination < PST_RAD_45) 00382 intWeight += 1 + inclinationDistance; 00383 else 00384 { 00385 intWeight += 1 - inclinationDistance; 00386 assert ((desc_index - 1) * (nr_bins+1) + step_index >= 0 && (desc_index - 1) * (nr_bins+1) + step_index < descLength_); 00387 shot[(desc_index - 1) * (nr_bins+1) + step_index] += static_cast<float> (inclinationDistance); 00388 } 00389 } 00390 00391 if (yInFeatRef != 0.0 || xInFeatRef != 0.0) 00392 { 00393 //Interpolation on the azimuth (adjacent horizontal volumes) 00394 double azimuth = atan2 (yInFeatRef, xInFeatRef); 00395 00396 int sel = desc_index >> 2; 00397 double angularSectorSpan = PST_RAD_45; 00398 double angularSectorStart = - PST_RAD_PI_7_8; 00399 00400 double azimuthDistance = (azimuth - (angularSectorStart + angularSectorSpan*sel)) / angularSectorSpan; 00401 00402 assert ((azimuthDistance < 0.5 || areEquals (azimuthDistance, 0.5)) && (azimuthDistance > - 0.5 || areEquals (azimuthDistance, - 0.5))); 00403 00404 azimuthDistance = (std::max)(- 0.5, std::min (azimuthDistance, 0.5)); 00405 00406 if (azimuthDistance > 0) 00407 { 00408 intWeight += 1 - azimuthDistance; 00409 int interp_index = (desc_index + 4) % maxAngularSectors_; 00410 assert (interp_index * (nr_bins+1) + step_index >= 0 && interp_index * (nr_bins+1) + step_index < descLength_); 00411 shot[interp_index * (nr_bins+1) + step_index] += static_cast<float> (azimuthDistance); 00412 } 00413 else 00414 { 00415 int interp_index = (desc_index - 4 + maxAngularSectors_) % maxAngularSectors_; 00416 assert (interp_index * (nr_bins+1) + step_index >= 0 && interp_index * (nr_bins+1) + step_index < descLength_); 00417 intWeight += 1 + azimuthDistance; 00418 shot[interp_index * (nr_bins+1) + step_index] -= static_cast<float> (azimuthDistance); 00419 } 00420 00421 } 00422 00423 assert (volume_index + step_index >= 0 && volume_index + step_index < descLength_); 00424 shot[volume_index + step_index] += static_cast<float> (intWeight); 00425 } 00426 } 00427 00428 ////////////////////////////////////////////////////////////////////////////////////////////// 00429 template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void 00430 pcl::SHOTColorEstimation<PointInT, PointNT, PointOutT, PointRFT>::interpolateDoubleChannel ( 00431 const std::vector<int> &indices, 00432 const std::vector<float> &sqr_dists, 00433 const int index, 00434 std::vector<double> &binDistanceShape, 00435 std::vector<double> &binDistanceColor, 00436 const int nr_bins_shape, 00437 const int nr_bins_color, 00438 Eigen::VectorXf &shot) 00439 { 00440 const Eigen::Vector4f ¢ral_point = (*input_)[(*indices_)[index]].getVector4fMap (); 00441 const PointRFT& current_frame = (*frames_)[index]; 00442 00443 int shapeToColorStride = nr_grid_sector_*(nr_bins_shape+1); 00444 00445 Eigen::Vector4f current_frame_x (current_frame.x_axis[0], current_frame.x_axis[1], current_frame.x_axis[2], 0); 00446 Eigen::Vector4f current_frame_y (current_frame.y_axis[0], current_frame.y_axis[1], current_frame.y_axis[2], 0); 00447 Eigen::Vector4f current_frame_z (current_frame.z_axis[0], current_frame.z_axis[1], current_frame.z_axis[2], 0); 00448 00449 for (size_t i_idx = 0; i_idx < indices.size (); ++i_idx) 00450 { 00451 if (!pcl_isfinite(binDistanceShape[i_idx])) 00452 continue; 00453 00454 Eigen::Vector4f delta = surface_->points[indices[i_idx]].getVector4fMap () - central_point; 00455 delta[3] = 0; 00456 00457 // Compute the Euclidean norm 00458 double distance = sqrt (sqr_dists[i_idx]); 00459 00460 if (areEquals (distance, 0.0)) 00461 continue; 00462 00463 double xInFeatRef = delta.dot (current_frame_x); 00464 double yInFeatRef = delta.dot (current_frame_y); 00465 double zInFeatRef = delta.dot (current_frame_z); 00466 00467 // To avoid numerical problems afterwards 00468 if (fabs (yInFeatRef) < 1E-30) 00469 yInFeatRef = 0; 00470 if (fabs (xInFeatRef) < 1E-30) 00471 xInFeatRef = 0; 00472 if (fabs (zInFeatRef) < 1E-30) 00473 zInFeatRef = 0; 00474 00475 unsigned char bit4 = ((yInFeatRef > 0) || ((yInFeatRef == 0.0) && (xInFeatRef < 0))) ? 1 : 0; 00476 unsigned char bit3 = static_cast<unsigned char> (((xInFeatRef > 0) || ((xInFeatRef == 0.0) && (yInFeatRef > 0))) ? !bit4 : bit4); 00477 00478 assert (bit3 == 0 || bit3 == 1); 00479 00480 int desc_index = (bit4<<3) + (bit3<<2); 00481 00482 desc_index = desc_index << 1; 00483 00484 if ((xInFeatRef * yInFeatRef > 0) || (xInFeatRef == 0.0)) 00485 desc_index += (fabs (xInFeatRef) >= fabs (yInFeatRef)) ? 0 : 4; 00486 else 00487 desc_index += (fabs (xInFeatRef) > fabs (yInFeatRef)) ? 4 : 0; 00488 00489 desc_index += zInFeatRef > 0 ? 1 : 0; 00490 00491 // 2 RADII 00492 desc_index += (distance > radius1_2_) ? 2 : 0; 00493 00494 int step_index_shape = static_cast<int>(floor (binDistanceShape[i_idx] +0.5)); 00495 int step_index_color = static_cast<int>(floor (binDistanceColor[i_idx] +0.5)); 00496 00497 int volume_index_shape = desc_index * (nr_bins_shape+1); 00498 int volume_index_color = shapeToColorStride + desc_index * (nr_bins_color+1); 00499 00500 //Interpolation on the cosine (adjacent bins in the histrogram) 00501 binDistanceShape[i_idx] -= step_index_shape; 00502 binDistanceColor[i_idx] -= step_index_color; 00503 00504 double intWeightShape = (1- fabs (binDistanceShape[i_idx])); 00505 double intWeightColor = (1- fabs (binDistanceColor[i_idx])); 00506 00507 if (binDistanceShape[i_idx] > 0) 00508 shot[volume_index_shape + ((step_index_shape + 1) % nr_bins_shape)] += static_cast<float> (binDistanceShape[i_idx]); 00509 else 00510 shot[volume_index_shape + ((step_index_shape - 1 + nr_bins_shape) % nr_bins_shape)] -= static_cast<float> (binDistanceShape[i_idx]); 00511 00512 if (binDistanceColor[i_idx] > 0) 00513 shot[volume_index_color + ((step_index_color+1) % nr_bins_color)] += static_cast<float> (binDistanceColor[i_idx]); 00514 else 00515 shot[volume_index_color + ((step_index_color - 1 + nr_bins_color) % nr_bins_color)] -= static_cast<float> (binDistanceColor[i_idx]); 00516 00517 //Interpolation on the distance (adjacent husks) 00518 00519 if (distance > radius1_2_) //external sphere 00520 { 00521 double radiusDistance = (distance - radius3_4_) / radius1_2_; 00522 00523 if (distance > radius3_4_) //most external sector, votes only for itself 00524 { 00525 intWeightShape += 1 - radiusDistance; //weight=1-d 00526 intWeightColor += 1 - radiusDistance; //weight=1-d 00527 } 00528 else //3/4 of radius, votes also for the internal sphere 00529 { 00530 intWeightShape += 1 + radiusDistance; 00531 intWeightColor += 1 + radiusDistance; 00532 shot[(desc_index - 2) * (nr_bins_shape+1) + step_index_shape] -= static_cast<float> (radiusDistance); 00533 shot[shapeToColorStride + (desc_index - 2) * (nr_bins_color+1) + step_index_color] -= static_cast<float> (radiusDistance); 00534 } 00535 } 00536 else //internal sphere 00537 { 00538 double radiusDistance = (distance - radius1_4_) / radius1_2_; 00539 00540 if (distance < radius1_4_) //most internal sector, votes only for itself 00541 { 00542 intWeightShape += 1 + radiusDistance; 00543 intWeightColor += 1 + radiusDistance; //weight=1-d 00544 } 00545 else //3/4 of radius, votes also for the external sphere 00546 { 00547 intWeightShape += 1 - radiusDistance; //weight=1-d 00548 intWeightColor += 1 - radiusDistance; //weight=1-d 00549 shot[(desc_index + 2) * (nr_bins_shape+1) + step_index_shape] += static_cast<float> (radiusDistance); 00550 shot[shapeToColorStride + (desc_index + 2) * (nr_bins_color+1) + step_index_color] += static_cast<float> (radiusDistance); 00551 } 00552 } 00553 00554 //Interpolation on the inclination (adjacent vertical volumes) 00555 double inclinationCos = zInFeatRef / distance; 00556 if (inclinationCos < - 1.0) 00557 inclinationCos = - 1.0; 00558 if (inclinationCos > 1.0) 00559 inclinationCos = 1.0; 00560 00561 double inclination = acos (inclinationCos); 00562 00563 assert (inclination >= 0.0 && inclination <= PST_RAD_180); 00564 00565 if (inclination > PST_RAD_90 || (fabs (inclination - PST_RAD_90) < 1e-30 && zInFeatRef <= 0)) 00566 { 00567 double inclinationDistance = (inclination - PST_RAD_135) / PST_RAD_90; 00568 if (inclination > PST_RAD_135) 00569 { 00570 intWeightShape += 1 - inclinationDistance; 00571 intWeightColor += 1 - inclinationDistance; 00572 } 00573 else 00574 { 00575 intWeightShape += 1 + inclinationDistance; 00576 intWeightColor += 1 + inclinationDistance; 00577 assert ((desc_index + 1) * (nr_bins_shape+1) + step_index_shape >= 0 && (desc_index + 1) * (nr_bins_shape+1) + step_index_shape < descLength_); 00578 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_); 00579 shot[(desc_index + 1) * (nr_bins_shape+1) + step_index_shape] -= static_cast<float> (inclinationDistance); 00580 shot[shapeToColorStride + (desc_index + 1) * (nr_bins_color+1) + step_index_color] -= static_cast<float> (inclinationDistance); 00581 } 00582 } 00583 else 00584 { 00585 double inclinationDistance = (inclination - PST_RAD_45) / PST_RAD_90; 00586 if (inclination < PST_RAD_45) 00587 { 00588 intWeightShape += 1 + inclinationDistance; 00589 intWeightColor += 1 + inclinationDistance; 00590 } 00591 else 00592 { 00593 intWeightShape += 1 - inclinationDistance; 00594 intWeightColor += 1 - inclinationDistance; 00595 assert ((desc_index - 1) * (nr_bins_shape+1) + step_index_shape >= 0 && (desc_index - 1) * (nr_bins_shape+1) + step_index_shape < descLength_); 00596 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_); 00597 shot[(desc_index - 1) * (nr_bins_shape+1) + step_index_shape] += static_cast<float> (inclinationDistance); 00598 shot[shapeToColorStride + (desc_index - 1) * (nr_bins_color+1) + step_index_color] += static_cast<float> (inclinationDistance); 00599 } 00600 } 00601 00602 if (yInFeatRef != 0.0 || xInFeatRef != 0.0) 00603 { 00604 //Interpolation on the azimuth (adjacent horizontal volumes) 00605 double azimuth = atan2 (yInFeatRef, xInFeatRef); 00606 00607 int sel = desc_index >> 2; 00608 double angularSectorSpan = PST_RAD_45; 00609 double angularSectorStart = - PST_RAD_PI_7_8; 00610 00611 double azimuthDistance = (azimuth - (angularSectorStart + angularSectorSpan*sel)) / angularSectorSpan; 00612 assert ((azimuthDistance < 0.5 || areEquals (azimuthDistance, 0.5)) && (azimuthDistance > - 0.5 || areEquals (azimuthDistance, - 0.5))); 00613 azimuthDistance = (std::max)(- 0.5, std::min (azimuthDistance, 0.5)); 00614 00615 if (azimuthDistance > 0) 00616 { 00617 intWeightShape += 1 - azimuthDistance; 00618 intWeightColor += 1 - azimuthDistance; 00619 int interp_index = (desc_index + 4) % maxAngularSectors_; 00620 assert (interp_index * (nr_bins_shape+1) + step_index_shape >= 0 && interp_index * (nr_bins_shape+1) + step_index_shape < descLength_); 00621 assert (shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color >= 0 && shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color < descLength_); 00622 shot[interp_index * (nr_bins_shape+1) + step_index_shape] += static_cast<float> (azimuthDistance); 00623 shot[shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color] += static_cast<float> (azimuthDistance); 00624 } 00625 else 00626 { 00627 int interp_index = (desc_index - 4 + maxAngularSectors_) % maxAngularSectors_; 00628 intWeightShape += 1 + azimuthDistance; 00629 intWeightColor += 1 + azimuthDistance; 00630 assert (interp_index * (nr_bins_shape+1) + step_index_shape >= 0 && interp_index * (nr_bins_shape+1) + step_index_shape < descLength_); 00631 assert (shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color >= 0 && shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color < descLength_); 00632 shot[interp_index * (nr_bins_shape+1) + step_index_shape] -= static_cast<float> (azimuthDistance); 00633 shot[shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color] -= static_cast<float> (azimuthDistance); 00634 } 00635 } 00636 00637 assert (volume_index_shape + step_index_shape >= 0 && volume_index_shape + step_index_shape < descLength_); 00638 assert (volume_index_color + step_index_color >= 0 && volume_index_color + step_index_color < descLength_); 00639 shot[volume_index_shape + step_index_shape] += static_cast<float> (intWeightShape); 00640 shot[volume_index_color + step_index_color] += static_cast<float> (intWeightColor); 00641 } 00642 } 00643 00644 ////////////////////////////////////////////////////////////////////////////////////////////// 00645 template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void 00646 pcl::SHOTColorEstimation<PointInT, PointNT, PointOutT, PointRFT>::computePointSHOT ( 00647 const int index, const std::vector<int> &indices, const std::vector<float> &sqr_dists, Eigen::VectorXf &shot) 00648 { 00649 // Clear the resultant shot 00650 shot.setZero (); 00651 std::vector<double> binDistanceShape; 00652 std::vector<double> binDistanceColor; 00653 size_t nNeighbors = indices.size (); 00654 //Skip the current feature if the number of its neighbors is not sufficient for its description 00655 if (nNeighbors < 5) 00656 { 00657 PCL_WARN ("[pcl::%s::computePointSHOT] Warning! Neighborhood has less than 5 vertexes. Aborting description of point with index %d\n", 00658 getClassName ().c_str (), (*indices_)[index]); 00659 00660 shot.setConstant(descLength_, 1, std::numeric_limits<float>::quiet_NaN () ); 00661 00662 return; 00663 } 00664 00665 //If shape description is enabled, compute the bins activated by each neighbor of the current feature in the shape histogram 00666 if (b_describe_shape_) 00667 { 00668 this->createBinDistanceShape (index, indices, binDistanceShape); 00669 } 00670 00671 //If color description is enabled, compute the bins activated by each neighbor of the current feature in the color histogram 00672 if (b_describe_color_) 00673 { 00674 binDistanceColor.resize (nNeighbors); 00675 00676 //unsigned char redRef = input_->points[(*indices_)[index]].rgba >> 16 & 0xFF; 00677 //unsigned char greenRef = input_->points[(*indices_)[index]].rgba >> 8& 0xFF; 00678 //unsigned char blueRef = input_->points[(*indices_)[index]].rgba & 0xFF; 00679 unsigned char redRef = input_->points[(*indices_)[index]].r; 00680 unsigned char greenRef = input_->points[(*indices_)[index]].g; 00681 unsigned char blueRef = input_->points[(*indices_)[index]].b; 00682 00683 float LRef, aRef, bRef; 00684 00685 RGB2CIELAB (redRef, greenRef, blueRef, LRef, aRef, bRef); 00686 LRef /= 100.0f; 00687 aRef /= 120.0f; 00688 bRef /= 120.0f; //normalized LAB components (0<L<1, -1<a<1, -1<b<1) 00689 00690 for (size_t i_idx = 0; i_idx < indices.size (); ++i_idx) 00691 { 00692 //unsigned char red = surface_->points[indices[i_idx]].rgba >> 16 & 0xFF; 00693 //unsigned char green = surface_->points[indices[i_idx]].rgba >> 8 & 0xFF; 00694 //unsigned char blue = surface_->points[indices[i_idx]].rgba & 0xFF; 00695 unsigned char red = surface_->points[indices[i_idx]].r; 00696 unsigned char green = surface_->points[indices[i_idx]].g; 00697 unsigned char blue = surface_->points[indices[i_idx]].b; 00698 00699 float L, a, b; 00700 00701 RGB2CIELAB (red, green, blue, L, a, b); 00702 L /= 100.0f; 00703 a /= 120.0f; 00704 b /= 120.0f; //normalized LAB components (0<L<1, -1<a<1, -1<b<1) 00705 00706 double colorDistance = (fabs (LRef - L) + ((fabs (aRef - a) + fabs (bRef - b)) / 2)) /3; 00707 00708 if (colorDistance > 1.0) 00709 colorDistance = 1.0; 00710 if (colorDistance < 0.0) 00711 colorDistance = 0.0; 00712 00713 binDistanceColor[i_idx] = colorDistance * nr_color_bins_; 00714 } 00715 } 00716 00717 //Apply quadrilinear interpolation on the activated bins in the shape and/or color histogram(s) 00718 00719 if (b_describe_shape_ && b_describe_color_) 00720 interpolateDoubleChannel (indices, sqr_dists, index, binDistanceShape, binDistanceColor, 00721 nr_shape_bins_, nr_color_bins_, 00722 shot); 00723 else if (b_describe_color_) 00724 interpolateSingleChannel (indices, sqr_dists, index, binDistanceColor, nr_color_bins_, shot); 00725 else 00726 interpolateSingleChannel (indices, sqr_dists, index, binDistanceShape, nr_shape_bins_, shot); 00727 00728 // Normalize the final histogram 00729 this->normalizeHistogram (shot, descLength_); 00730 } 00731 00732 ////////////////////////////////////////////////////////////////////////////////////////////// 00733 template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void 00734 pcl::SHOTEstimation<PointInT, PointNT, PointOutT, PointRFT>::computePointSHOT ( 00735 const int index, const std::vector<int> &indices, const std::vector<float> &sqr_dists, Eigen::VectorXf &shot) 00736 { 00737 //Skip the current feature if the number of its neighbors is not sufficient for its description 00738 if (indices.size () < 5) 00739 { 00740 PCL_WARN ("[pcl::%s::computePointSHOT] Warning! Neighborhood has less than 5 vertexes. Aborting description of point with index %d\n", 00741 getClassName ().c_str (), (*indices_)[index]); 00742 00743 shot.setConstant(descLength_, 1, std::numeric_limits<float>::quiet_NaN () ); 00744 00745 return; 00746 } 00747 00748 // Clear the resultant shot 00749 std::vector<double> binDistanceShape; 00750 this->createBinDistanceShape (index, indices, binDistanceShape); 00751 00752 // Interpolate 00753 shot.setZero (); 00754 interpolateSingleChannel (indices, sqr_dists, index, binDistanceShape, nr_shape_bins_, shot); 00755 00756 // Normalize the final histogram 00757 this->normalizeHistogram (shot, descLength_); 00758 } 00759 00760 ////////////////////////////////////////////////////////////////////////////////////////////// 00761 ////////////////////////////////////////////////////////////////////////////////////////////// 00762 ////////////////////////////////////////////////////////////////////////////////////////////// 00763 template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void 00764 pcl::SHOTEstimation<PointInT, PointNT, PointOutT, PointRFT>::computeFeature (pcl::PointCloud<PointOutT> &output) 00765 { 00766 descLength_ = nr_grid_sector_ * (nr_shape_bins_+1); 00767 00768 sqradius_ = search_radius_ * search_radius_; 00769 radius3_4_ = (search_radius_*3) / 4; 00770 radius1_4_ = search_radius_ / 4; 00771 radius1_2_ = search_radius_ / 2; 00772 00773 assert(descLength_ == 352); 00774 00775 shot_.setZero (descLength_); 00776 00777 // Allocate enough space to hold the results 00778 // \note This resize is irrelevant for a radiusSearch (). 00779 std::vector<int> nn_indices (k_); 00780 std::vector<float> nn_dists (k_); 00781 00782 output.is_dense = true; 00783 // Iterating over the entire index vector 00784 for (size_t idx = 0; idx < indices_->size (); ++idx) 00785 { 00786 bool lrf_is_nan = false; 00787 const PointRFT& current_frame = (*frames_)[idx]; 00788 if (!pcl_isfinite (current_frame.x_axis[0]) || 00789 !pcl_isfinite (current_frame.y_axis[0]) || 00790 !pcl_isfinite (current_frame.z_axis[0])) 00791 { 00792 PCL_WARN ("[pcl::%s::computeFeature] The local reference frame is not valid! Aborting description of point with index %d\n", 00793 getClassName ().c_str (), (*indices_)[idx]); 00794 lrf_is_nan = true; 00795 } 00796 00797 if (!isFinite ((*input_)[(*indices_)[idx]]) || 00798 lrf_is_nan || 00799 this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0) 00800 { 00801 // Copy into the resultant cloud 00802 for (int d = 0; d < descLength_; ++d) 00803 output.points[idx].descriptor[d] = std::numeric_limits<float>::quiet_NaN (); 00804 for (int d = 0; d < 9; ++d) 00805 output.points[idx].rf[d] = std::numeric_limits<float>::quiet_NaN (); 00806 00807 output.is_dense = false; 00808 continue; 00809 } 00810 00811 // Estimate the SHOT descriptor at each patch 00812 computePointSHOT (static_cast<int> (idx), nn_indices, nn_dists, shot_); 00813 00814 // Copy into the resultant cloud 00815 for (int d = 0; d < descLength_; ++d) 00816 output.points[idx].descriptor[d] = shot_[d]; 00817 for (int d = 0; d < 3; ++d) 00818 { 00819 output.points[idx].rf[d + 0] = frames_->points[idx].x_axis[d]; 00820 output.points[idx].rf[d + 3] = frames_->points[idx].y_axis[d]; 00821 output.points[idx].rf[d + 6] = frames_->points[idx].z_axis[d]; 00822 } 00823 } 00824 } 00825 00826 ////////////////////////////////////////////////////////////////////////////////////////////// 00827 ////////////////////////////////////////////////////////////////////////////////////////////// 00828 ////////////////////////////////////////////////////////////////////////////////////////////// 00829 template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void 00830 pcl::SHOTColorEstimation<PointInT, PointNT, PointOutT, PointRFT>::computeFeature (pcl::PointCloud<PointOutT> &output) 00831 { 00832 // Compute the current length of the descriptor 00833 descLength_ = (b_describe_shape_) ? nr_grid_sector_*(nr_shape_bins_+1) : 0; 00834 descLength_ += (b_describe_color_) ? nr_grid_sector_*(nr_color_bins_+1) : 0; 00835 00836 assert( (!b_describe_color_ && b_describe_shape_ && descLength_ == 352) || 00837 (b_describe_color_ && !b_describe_shape_ && descLength_ == 992) || 00838 (b_describe_color_ && b_describe_shape_ && descLength_ == 1344) 00839 ); 00840 00841 // Useful values 00842 sqradius_ = search_radius_*search_radius_; 00843 radius3_4_ = (search_radius_*3) / 4; 00844 radius1_4_ = search_radius_ / 4; 00845 radius1_2_ = search_radius_ / 2; 00846 00847 shot_.setZero (descLength_); 00848 00849 // Allocate enough space to hold the results 00850 // \note This resize is irrelevant for a radiusSearch (). 00851 std::vector<int> nn_indices (k_); 00852 std::vector<float> nn_dists (k_); 00853 00854 output.is_dense = true; 00855 // Iterating over the entire index vector 00856 for (size_t idx = 0; idx < indices_->size (); ++idx) 00857 { 00858 bool lrf_is_nan = false; 00859 const PointRFT& current_frame = (*frames_)[idx]; 00860 if (!pcl_isfinite (current_frame.x_axis[0]) || 00861 !pcl_isfinite (current_frame.y_axis[0]) || 00862 !pcl_isfinite (current_frame.z_axis[0])) 00863 { 00864 PCL_WARN ("[pcl::%s::computeFeature] The local reference frame is not valid! Aborting description of point with index %d\n", 00865 getClassName ().c_str (), (*indices_)[idx]); 00866 lrf_is_nan = true; 00867 } 00868 00869 if (!isFinite ((*input_)[(*indices_)[idx]]) || 00870 lrf_is_nan || 00871 this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0) 00872 { 00873 // Copy into the resultant cloud 00874 for (int d = 0; d < descLength_; ++d) 00875 output.points[idx].descriptor[d] = std::numeric_limits<float>::quiet_NaN (); 00876 for (int d = 0; d < 9; ++d) 00877 output.points[idx].rf[d] = std::numeric_limits<float>::quiet_NaN (); 00878 00879 output.is_dense = false; 00880 continue; 00881 } 00882 00883 // Compute the SHOT descriptor for the current 3D feature 00884 computePointSHOT (static_cast<int> (idx), nn_indices, nn_dists, shot_); 00885 00886 // Copy into the resultant cloud 00887 for (int d = 0; d < descLength_; ++d) 00888 output.points[idx].descriptor[d] = shot_[d]; 00889 for (int d = 0; d < 3; ++d) 00890 { 00891 output.points[idx].rf[d + 0] = frames_->points[idx].x_axis[d]; 00892 output.points[idx].rf[d + 3] = frames_->points[idx].y_axis[d]; 00893 output.points[idx].rf[d + 6] = frames_->points[idx].z_axis[d]; 00894 } 00895 } 00896 } 00897 00898 #define PCL_INSTANTIATE_SHOTEstimation(T,NT,OutT,RFT) template class PCL_EXPORTS pcl::SHOTEstimation<T,NT,OutT,RFT>; 00899 #define PCL_INSTANTIATE_SHOTColorEstimation(T,NT,OutT,RFT) template class PCL_EXPORTS pcl::SHOTColorEstimation<T,NT,OutT,RFT>; 00900 00901 #endif // PCL_FEATURES_IMPL_SHOT_H_