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