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-2012, 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 * $Id$ 00038 * 00039 */ 00040 00041 #ifndef PCL_FEATURES_IMPL_SPIN_IMAGE_H_ 00042 #define PCL_FEATURES_IMPL_SPIN_IMAGE_H_ 00043 00044 #include <limits> 00045 #include <pcl/point_cloud.h> 00046 #include <pcl/point_types.h> 00047 #include <pcl/exceptions.h> 00048 #include <pcl/kdtree/kdtree_flann.h> 00049 #include <pcl/features/spin_image.h> 00050 #include <cmath> 00051 00052 ////////////////////////////////////////////////////////////////////////////////////////////// 00053 template <typename PointInT, typename PointNT, typename PointOutT> 00054 pcl::SpinImageEstimation<PointInT, PointNT, PointOutT>::SpinImageEstimation ( 00055 unsigned int image_width, double support_angle_cos, unsigned int min_pts_neighb) : 00056 input_normals_ (), rotation_axes_cloud_ (), 00057 is_angular_ (false), rotation_axis_ (), use_custom_axis_(false), use_custom_axes_cloud_ (false), 00058 is_radial_ (false), image_width_ (image_width), support_angle_cos_ (support_angle_cos), 00059 min_pts_neighb_ (min_pts_neighb) 00060 { 00061 assert (support_angle_cos_ <= 1.0 && support_angle_cos_ >= 0.0); // may be permit negative cosine? 00062 00063 feature_name_ = "SpinImageEstimation"; 00064 } 00065 00066 00067 ////////////////////////////////////////////////////////////////////////////////////////////// 00068 template <typename PointInT, typename PointNT, typename PointOutT> Eigen::ArrayXXd 00069 pcl::SpinImageEstimation<PointInT, PointNT, PointOutT>::computeSiForPoint (int index) const 00070 { 00071 assert (image_width_ > 0); 00072 assert (support_angle_cos_ <= 1.0 && support_angle_cos_ >= 0.0); // may be permit negative cosine? 00073 00074 const Eigen::Vector3f origin_point (input_->points[index].getVector3fMap ()); 00075 00076 Eigen::Vector3f origin_normal; 00077 origin_normal = 00078 input_normals_ ? 00079 input_normals_->points[index].getNormalVector3fMap () : 00080 Eigen::Vector3f (); // just a placeholder; should never be used! 00081 00082 const Eigen::Vector3f rotation_axis = use_custom_axis_ ? 00083 rotation_axis_.getNormalVector3fMap () : 00084 use_custom_axes_cloud_ ? 00085 rotation_axes_cloud_->points[index].getNormalVector3fMap () : 00086 origin_normal; 00087 00088 Eigen::ArrayXXd m_matrix (Eigen::ArrayXXd::Zero (image_width_+1, 2*image_width_+1)); 00089 Eigen::ArrayXXd m_averAngles (Eigen::ArrayXXd::Zero (image_width_+1, 2*image_width_+1)); 00090 00091 // OK, we are interested in the points of the cylinder of height 2*r and 00092 // base radius r, where r = m_dBinSize * in_iImageWidth 00093 // it can be embedded to the sphere of radius sqrt(2) * m_dBinSize * in_iImageWidth 00094 // suppose that points are uniformly distributed, so we lose ~40% 00095 // according to the volumes ratio 00096 double bin_size = 0.0; 00097 if (is_radial_) 00098 bin_size = search_radius_ / image_width_; 00099 else 00100 bin_size = search_radius_ / image_width_ / sqrt(2.0); 00101 00102 std::vector<int> nn_indices; 00103 std::vector<float> nn_sqr_dists; 00104 const int neighb_cnt = this->searchForNeighbors (index, search_radius_, nn_indices, nn_sqr_dists); 00105 if (neighb_cnt < static_cast<int> (min_pts_neighb_)) 00106 { 00107 throw PCLException ( 00108 "Too few points for spin image, use setMinPointCountInNeighbourhood() to decrease the threshold or use larger feature radius", 00109 "spin_image.hpp", "computeSiForPoint"); 00110 } 00111 00112 // for all neighbor points 00113 for (int i_neigh = 0; i_neigh < neighb_cnt ; i_neigh++) 00114 { 00115 // first, skip the points with distant normals 00116 double cos_between_normals = -2.0; // should be initialized if used 00117 if (support_angle_cos_ > 0.0 || is_angular_) // not bogus 00118 { 00119 cos_between_normals = origin_normal.dot (input_normals_->points[nn_indices[i_neigh]].getNormalVector3fMap ()); 00120 if (fabs (cos_between_normals) > (1.0 + 10*std::numeric_limits<float>::epsilon ())) // should be okay for numeric stability 00121 { 00122 PCL_ERROR ("[pcl::%s::computeSiForPoint] Normal for the point %d and/or the point %d are not normalized, dot ptoduct is %f.\n", 00123 getClassName ().c_str (), nn_indices[i_neigh], index, cos_between_normals); 00124 throw PCLException ("Some normals are not normalized", 00125 "spin_image.hpp", "computeSiForPoint"); 00126 } 00127 cos_between_normals = std::max (-1.0, std::min (1.0, cos_between_normals)); 00128 00129 if (fabs (cos_between_normals) < support_angle_cos_ ) // allow counter-directed normals 00130 { 00131 continue; 00132 } 00133 00134 if (cos_between_normals < 0.0) 00135 { 00136 cos_between_normals = -cos_between_normals; // the normal is not used explicitly from now 00137 } 00138 } 00139 00140 // now compute the coordinate in cylindric coordinate system associated with the origin point 00141 const Eigen::Vector3f direction ( 00142 surface_->points[nn_indices[i_neigh]].getVector3fMap () - origin_point); 00143 const double direction_norm = direction.norm (); 00144 if (fabs(direction_norm) < 10*std::numeric_limits<double>::epsilon ()) 00145 continue; // ignore the point itself; it does not contribute really 00146 assert (direction_norm > 0.0); 00147 00148 // the angle between the normal vector and the direction to the point 00149 double cos_dir_axis = direction.dot(rotation_axis) / direction_norm; 00150 if (fabs(cos_dir_axis) > (1.0 + 10*std::numeric_limits<float>::epsilon())) // should be okay for numeric stability 00151 { 00152 PCL_ERROR ("[pcl::%s::computeSiForPoint] Rotation axis for the point %d are not normalized, dot ptoduct is %f.\n", 00153 getClassName ().c_str (), index, cos_dir_axis); 00154 throw PCLException ("Some rotation axis is not normalized", 00155 "spin_image.hpp", "computeSiForPoint"); 00156 } 00157 cos_dir_axis = std::max (-1.0, std::min (1.0, cos_dir_axis)); 00158 00159 // compute coordinates w.r.t. the reference frame 00160 double beta = std::numeric_limits<double>::signaling_NaN (); 00161 double alpha = std::numeric_limits<double>::signaling_NaN (); 00162 if (is_radial_) // radial spin image structure 00163 { 00164 beta = asin (cos_dir_axis); // yes, arc sine! to get the angle against tangent, not normal! 00165 alpha = direction_norm; 00166 } 00167 else // rectangular spin-image structure 00168 { 00169 beta = direction_norm * cos_dir_axis; 00170 alpha = direction_norm * sqrt (1.0 - cos_dir_axis*cos_dir_axis); 00171 00172 if (fabs (beta) >= bin_size * image_width_ || alpha >= bin_size * image_width_) 00173 { 00174 continue; // outside the cylinder 00175 } 00176 } 00177 00178 assert (alpha >= 0.0); 00179 assert (alpha <= bin_size * image_width_ + 20 * std::numeric_limits<float>::epsilon () ); 00180 00181 00182 // bilinear interpolation 00183 double beta_bin_size = is_radial_ ? (M_PI / 2 / image_width_) : bin_size; 00184 int beta_bin = int(std::floor (beta / beta_bin_size)) + int(image_width_); 00185 assert (0 <= beta_bin && beta_bin < m_matrix.cols ()); 00186 int alpha_bin = int(std::floor (alpha / bin_size)); 00187 assert (0 <= alpha_bin && alpha_bin < m_matrix.rows ()); 00188 00189 if (alpha_bin == static_cast<int> (image_width_)) // border points 00190 { 00191 alpha_bin--; 00192 // HACK: to prevent a > 1 00193 alpha = bin_size * (alpha_bin + 1) - std::numeric_limits<double>::epsilon (); 00194 } 00195 if (beta_bin == int(2*image_width_) ) // border points 00196 { 00197 beta_bin--; 00198 // HACK: to prevent b > 1 00199 beta = beta_bin_size * (beta_bin - int(image_width_) + 1) - std::numeric_limits<double>::epsilon (); 00200 } 00201 00202 double a = alpha/bin_size - double(alpha_bin); 00203 double b = beta/beta_bin_size - double(beta_bin-int(image_width_)); 00204 00205 assert (0 <= a && a <= 1); 00206 assert (0 <= b && b <= 1); 00207 00208 m_matrix (alpha_bin, beta_bin) += (1-a) * (1-b); 00209 m_matrix (alpha_bin+1, beta_bin) += a * (1-b); 00210 m_matrix (alpha_bin, beta_bin+1) += (1-a) * b; 00211 m_matrix (alpha_bin+1, beta_bin+1) += a * b; 00212 00213 if (is_angular_) 00214 { 00215 m_averAngles (alpha_bin, beta_bin) += (1-a) * (1-b) * acos (cos_between_normals); 00216 m_averAngles (alpha_bin+1, beta_bin) += a * (1-b) * acos (cos_between_normals); 00217 m_averAngles (alpha_bin, beta_bin+1) += (1-a) * b * acos (cos_between_normals); 00218 m_averAngles (alpha_bin+1, beta_bin+1) += a * b * acos (cos_between_normals); 00219 } 00220 } 00221 00222 if (is_angular_) 00223 { 00224 // transform sum to average 00225 m_matrix = m_averAngles / (m_matrix + std::numeric_limits<double>::epsilon ()); // +eps to avoid division by zero 00226 } 00227 else if (neighb_cnt > 1) // to avoid division by zero, also no need to divide by 1 00228 { 00229 // normalization 00230 m_matrix /= m_matrix.sum(); 00231 } 00232 00233 return m_matrix; 00234 } 00235 00236 00237 ////////////////////////////////////////////////////////////////////////////////////////////// 00238 template <typename PointInT, typename PointNT, typename PointOutT> bool 00239 pcl::SpinImageEstimation<PointInT, PointNT, PointOutT>::initCompute () 00240 { 00241 if (!Feature<PointInT, PointOutT>::initCompute ()) 00242 { 00243 PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ()); 00244 return (false); 00245 } 00246 00247 // Check if input normals are set 00248 if (!input_normals_) 00249 { 00250 PCL_ERROR ("[pcl::%s::initCompute] No input dataset containing normals was given!\n", getClassName ().c_str ()); 00251 Feature<PointInT, PointOutT>::deinitCompute (); 00252 return (false); 00253 } 00254 00255 // Check if the size of normals is the same as the size of the surface 00256 if (input_normals_->points.size () != input_->points.size ()) 00257 { 00258 PCL_ERROR ("[pcl::%s::initCompute] ", getClassName ().c_str ()); 00259 PCL_ERROR ("The number of points in the input dataset differs from "); 00260 PCL_ERROR ("the number of points in the dataset containing the normals!\n"); 00261 Feature<PointInT, PointOutT>::deinitCompute (); 00262 return (false); 00263 } 00264 00265 // We need a positive definite search radius to continue 00266 if (search_radius_ == 0) 00267 { 00268 PCL_ERROR ("[pcl::%s::initCompute] Need a search radius different than 0!\n", getClassName ().c_str ()); 00269 Feature<PointInT, PointOutT>::deinitCompute (); 00270 return (false); 00271 } 00272 if (k_ != 0) 00273 { 00274 PCL_ERROR ("[pcl::%s::initCompute] K-nearest neighbor search for spin images not implemented. Used a search radius instead!\n", getClassName ().c_str ()); 00275 Feature<PointInT, PointOutT>::deinitCompute (); 00276 return (false); 00277 } 00278 // If the surface won't be set, make fake surface and fake surface normals 00279 // if we wouldn't do it here, the following method would alarm that no surface normals is given 00280 if (!surface_) 00281 { 00282 surface_ = input_; 00283 fake_surface_ = true; 00284 } 00285 00286 //if (fake_surface_ && !input_normals_) 00287 // input_normals_ = normals_; // normals_ is set, as checked earlier 00288 00289 assert(!(use_custom_axis_ && use_custom_axes_cloud_)); 00290 00291 if (!use_custom_axis_ && !use_custom_axes_cloud_ // use input normals as rotation axes 00292 && !input_normals_) 00293 { 00294 PCL_ERROR ("[pcl::%s::initCompute] No normals for input cloud were given!\n", getClassName ().c_str ()); 00295 // Cleanup 00296 Feature<PointInT, PointOutT>::deinitCompute (); 00297 return (false); 00298 } 00299 00300 if ((is_angular_ || support_angle_cos_ > 0.0) // support angle is not bogus NOTE this is for randomly-flipped normals 00301 && !input_normals_) 00302 { 00303 PCL_ERROR ("[pcl::%s::initCompute] No normals for input cloud were given!\n", getClassName ().c_str ()); 00304 // Cleanup 00305 Feature<PointInT, PointOutT>::deinitCompute (); 00306 return (false); 00307 } 00308 00309 if (use_custom_axes_cloud_ 00310 && rotation_axes_cloud_->size () == input_->size ()) 00311 { 00312 PCL_ERROR ("[pcl::%s::initCompute] Rotation axis cloud have different size from input!\n", getClassName ().c_str ()); 00313 // Cleanup 00314 Feature<PointInT, PointOutT>::deinitCompute (); 00315 return (false); 00316 } 00317 00318 return (true); 00319 } 00320 00321 00322 ////////////////////////////////////////////////////////////////////////////////////////////// 00323 template <typename PointInT, typename PointNT, typename PointOutT> void 00324 pcl::SpinImageEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output) 00325 { 00326 for (int i_input = 0; i_input < static_cast<int> (indices_->size ()); ++i_input) 00327 { 00328 Eigen::ArrayXXd res = computeSiForPoint (indices_->at (i_input)); 00329 00330 // Copy into the resultant cloud 00331 for (int iRow = 0; iRow < res.rows () ; iRow++) 00332 { 00333 for (int iCol = 0; iCol < res.cols () ; iCol++) 00334 { 00335 output.points[i_input].histogram[ iRow*res.cols () + iCol ] = static_cast<float> (res (iRow, iCol)); 00336 } 00337 } 00338 } 00339 } 00340 00341 #define PCL_INSTANTIATE_SpinImageEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::SpinImageEstimation<T,NT,OutT>; 00342 00343 #endif // PCL_FEATURES_IMPL_SPIN_IMAGE_H_ 00344