Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2011, Alexandru-Eugen Ichim 00005 * Willow Garage, Inc 00006 * All rights reserved. 00007 * 00008 * Redistribution and use in source and binary forms, with or without 00009 * modification, are permitted provided that the following conditions 00010 * are met: 00011 * 00012 * * Redistributions of source code must retain the above copyright 00013 * notice, this list of conditions and the following disclaimer. 00014 * * Redistributions in binary form must reproduce the above 00015 * copyright notice, this list of conditions and the following 00016 * disclaimer in the documentation and/or other materials provided 00017 * with the distribution. 00018 * * Neither the name of Willow Garage, Inc. nor the names of its 00019 * contributors may be used to endorse or promote products derived 00020 * from this software without specific prior written permission. 00021 * 00022 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00023 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00024 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00025 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00026 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00027 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00028 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00029 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00030 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00031 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00032 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00033 * POSSIBILITY OF SUCH DAMAGE. 00034 * 00035 * $Id$ 00036 */ 00037 00038 #ifndef PCL_SURFACE_IMPL_SURFEL_SMOOTHING_H_ 00039 #define PCL_SURFACE_IMPL_SURFEL_SMOOTHING_H_ 00040 00041 #include <pcl/surface/surfel_smoothing.h> 00042 #include <pcl/common/distances.h> 00043 00044 ////////////////////////////////////////////////////////////////////////////////////////////// 00045 template <typename PointT, typename PointNT> bool 00046 pcl::SurfelSmoothing<PointT, PointNT>::initCompute () 00047 { 00048 if (!PCLBase<PointT>::initCompute ()) 00049 return false; 00050 00051 if (!normals_) 00052 { 00053 PCL_ERROR ("SurfelSmoothing: normal cloud not set\n"); 00054 return false; 00055 } 00056 00057 if (input_->points.size () != normals_->points.size ()) 00058 { 00059 PCL_ERROR ("SurfelSmoothing: number of input points different from the number of given normals\n"); 00060 return false; 00061 } 00062 00063 // Initialize the spatial locator 00064 if (!tree_) 00065 { 00066 if (input_->isOrganized ()) 00067 tree_.reset (new pcl::search::OrganizedNeighbor<PointT> ()); 00068 else 00069 tree_.reset (new pcl::search::KdTree<PointT> (false)); 00070 } 00071 00072 // create internal copies of the input - these will be modified 00073 interm_cloud_ = PointCloudInPtr (new PointCloudIn (*input_)); 00074 interm_normals_ = NormalCloudPtr (new NormalCloud (*normals_)); 00075 00076 return (true); 00077 } 00078 00079 00080 ////////////////////////////////////////////////////////////////////////////////////////////// 00081 template <typename PointT, typename PointNT> float 00082 pcl::SurfelSmoothing<PointT, PointNT>::smoothCloudIteration (PointCloudInPtr &output_positions, 00083 NormalCloudPtr &output_normals) 00084 { 00085 // PCL_INFO ("SurfelSmoothing: cloud smoothing iteration starting ...\n"); 00086 00087 output_positions = PointCloudInPtr (new PointCloudIn); 00088 output_positions->points.resize (interm_cloud_->points.size ()); 00089 output_normals = NormalCloudPtr (new NormalCloud); 00090 output_normals->points.resize (interm_cloud_->points.size ()); 00091 00092 std::vector<int> nn_indices; 00093 std::vector<float> nn_distances; 00094 00095 std::vector<float> diffs (interm_cloud_->points.size ()); 00096 float total_residual = 0.0f; 00097 00098 for (size_t i = 0; i < interm_cloud_->points.size (); ++i) 00099 { 00100 Eigen::Vector4f smoothed_point = Eigen::Vector4f::Zero (); 00101 Eigen::Vector4f smoothed_normal = Eigen::Vector4f::Zero (); 00102 00103 // get neighbors 00104 // @todo using 5x the scale for searching instead of all the points to avoid O(N^2) 00105 tree_->radiusSearch (interm_cloud_->points[i], 5*scale_, nn_indices, nn_distances); 00106 00107 float theta_normalization_factor = 0.0; 00108 std::vector<float> theta (nn_indices.size ()); 00109 for (size_t nn_index_i = 0; nn_index_i < nn_indices.size (); ++nn_index_i) 00110 { 00111 float dist = pcl::squaredEuclideanDistance (interm_cloud_->points[i], input_->points[nn_indices[nn_index_i]]);//interm_cloud_->points[nn_indices[nn_index_i]]); 00112 float theta_i = expf ( (-1) * dist / scale_squared_); 00113 theta_normalization_factor += theta_i; 00114 00115 smoothed_normal += theta_i * interm_normals_->points[nn_indices[nn_index_i]].getNormalVector4fMap (); 00116 00117 theta[nn_index_i] = theta_i; 00118 } 00119 00120 smoothed_normal /= theta_normalization_factor; 00121 smoothed_normal(3) = 0.0f; 00122 smoothed_normal.normalize (); 00123 00124 00125 // find minimum along the normal 00126 float e_residual; 00127 smoothed_point = interm_cloud_->points[i].getVector4fMap (); 00128 while (1) 00129 { 00130 e_residual = 0.0f; 00131 smoothed_point(3) = 0.0f; 00132 for (size_t nn_index_i = 0; nn_index_i < nn_indices.size (); ++nn_index_i) 00133 { 00134 Eigen::Vector4f neighbor = input_->points[nn_indices[nn_index_i]].getVector4fMap ();//interm_cloud_->points[nn_indices[nn_index_i]].getVector4fMap (); 00135 neighbor(3) = 0.0f; 00136 float dot_product = smoothed_normal.dot (neighbor - smoothed_point); 00137 e_residual += theta[nn_index_i] * dot_product;// * dot_product; 00138 } 00139 e_residual /= theta_normalization_factor; 00140 if (e_residual < 1e-5) break; 00141 00142 smoothed_point = smoothed_point + e_residual * smoothed_normal; 00143 } 00144 00145 total_residual += e_residual; 00146 00147 output_positions->points[i].getVector4fMap () = smoothed_point; 00148 output_normals->points[i].getNormalVector4fMap () = normals_->points[i].getNormalVector4fMap ();//smoothed_normal; 00149 } 00150 00151 // std::cerr << "Total residual after iteration: " << total_residual << std::endl; 00152 // PCL_INFO("SurfelSmoothing done iteration\n"); 00153 return total_residual; 00154 } 00155 00156 00157 template <typename PointT, typename PointNT> void 00158 pcl::SurfelSmoothing<PointT, PointNT>::smoothPoint (size_t &point_index, 00159 PointT &output_point, 00160 PointNT &output_normal) 00161 { 00162 Eigen::Vector4f average_normal = Eigen::Vector4f::Zero (); 00163 Eigen::Vector4f result_point = input_->points[point_index].getVector4fMap (); 00164 result_point(3) = 0.0f; 00165 00166 // @todo parameter 00167 float error_residual_threshold_ = 1e-3f; 00168 float error_residual = error_residual_threshold_ + 1; 00169 float last_error_residual = error_residual + 100.0f; 00170 00171 std::vector<int> nn_indices; 00172 std::vector<float> nn_distances; 00173 00174 int big_iterations = 0; 00175 int max_big_iterations = 500; 00176 00177 while (fabs (error_residual) < fabs (last_error_residual) -error_residual_threshold_ && 00178 big_iterations < max_big_iterations) 00179 { 00180 average_normal = Eigen::Vector4f::Zero (); 00181 big_iterations ++; 00182 PointT aux_point; aux_point.x = result_point(0); aux_point.y = result_point(1); aux_point.z = result_point(2); 00183 tree_->radiusSearch (aux_point, 5*scale_, nn_indices, nn_distances); 00184 00185 float theta_normalization_factor = 0.0; 00186 std::vector<float> theta (nn_indices.size ()); 00187 for (size_t nn_index_i = 0; nn_index_i < nn_indices.size (); ++nn_index_i) 00188 { 00189 float dist = nn_distances[nn_index_i]; 00190 float theta_i = expf ( (-1) * dist / scale_squared_); 00191 theta_normalization_factor += theta_i; 00192 00193 average_normal += theta_i * normals_->points[nn_indices[nn_index_i]].getNormalVector4fMap (); 00194 theta[nn_index_i] = theta_i; 00195 } 00196 average_normal /= theta_normalization_factor; 00197 average_normal(3) = 0.0f; 00198 average_normal.normalize (); 00199 00200 // find minimum along the normal 00201 float e_residual_along_normal = 2, last_e_residual_along_normal = 3; 00202 int small_iterations = 0; 00203 int max_small_iterations = 10; 00204 while ( fabs (e_residual_along_normal) < fabs (last_e_residual_along_normal) && 00205 small_iterations < max_small_iterations) 00206 { 00207 small_iterations ++; 00208 00209 e_residual_along_normal = 0.0f; 00210 for (size_t nn_index_i = 0; nn_index_i < nn_indices.size (); ++nn_index_i) 00211 { 00212 Eigen::Vector4f neighbor = input_->points[nn_indices[nn_index_i]].getVector4fMap (); 00213 neighbor(3) = 0.0f; 00214 float dot_product = average_normal.dot (neighbor - result_point); 00215 e_residual_along_normal += theta[nn_index_i] * dot_product; 00216 } 00217 e_residual_along_normal /= theta_normalization_factor; 00218 if (e_residual_along_normal < 1e-3) break; 00219 00220 result_point = result_point + e_residual_along_normal * average_normal; 00221 } 00222 00223 // if (small_iterations == max_small_iterations) 00224 // PCL_INFO ("passed the number of small iterations %d\n", small_iterations); 00225 00226 last_error_residual = error_residual; 00227 error_residual = e_residual_along_normal; 00228 00229 // PCL_INFO ("last %f current %f\n", last_error_residual, error_residual); 00230 } 00231 00232 output_point.x = result_point(0); 00233 output_point.y = result_point(1); 00234 output_point.z = result_point(2); 00235 output_normal = normals_->points[point_index]; 00236 00237 if (big_iterations == max_big_iterations) 00238 PCL_DEBUG ("[pcl::SurfelSmoothing::smoothPoint] Passed the number of BIG iterations: %d\n", big_iterations); 00239 } 00240 00241 00242 ////////////////////////////////////////////////////////////////////////////////////////////// 00243 template <typename PointT, typename PointNT> void 00244 pcl::SurfelSmoothing<PointT, PointNT>::computeSmoothedCloud (PointCloudInPtr &output_positions, 00245 NormalCloudPtr &output_normals) 00246 { 00247 if (!initCompute ()) 00248 { 00249 PCL_ERROR ("[pcl::SurfelSmoothing::computeSmoothedCloud]: SurfelSmoothing not initialized properly, skipping computeSmoothedCloud ().\n"); 00250 return; 00251 } 00252 00253 tree_->setInputCloud (input_); 00254 00255 output_positions->header = input_->header; 00256 output_positions->height = input_->height; 00257 output_positions->width = input_->width; 00258 00259 output_normals->header = input_->header; 00260 output_normals->height = input_->height; 00261 output_normals->width = input_->width; 00262 00263 output_positions->points.resize (input_->points.size ()); 00264 output_normals->points.resize (input_->points.size ()); 00265 for (size_t i = 0; i < input_->points.size (); ++i) 00266 { 00267 smoothPoint (i, output_positions->points[i], output_normals->points[i]); 00268 } 00269 } 00270 00271 ////////////////////////////////////////////////////////////////////////////////////////////// 00272 template <typename PointT, typename PointNT> void 00273 pcl::SurfelSmoothing<PointT, PointNT>::extractSalientFeaturesBetweenScales (PointCloudInPtr &cloud2, 00274 NormalCloudPtr &cloud2_normals, 00275 boost::shared_ptr<std::vector<int> > &output_features) 00276 { 00277 if (interm_cloud_->points.size () != cloud2->points.size () || 00278 cloud2->points.size () != cloud2_normals->points.size ()) 00279 { 00280 PCL_ERROR ("[pcl::SurfelSmoothing::extractSalientFeaturesBetweenScales]: Number of points in the clouds does not match.\n"); 00281 return; 00282 } 00283 00284 std::vector<float> diffs (cloud2->points.size ()); 00285 for (size_t i = 0; i < cloud2->points.size (); ++i) 00286 diffs[i] = cloud2_normals->points[i].getNormalVector4fMap ().dot (cloud2->points[i].getVector4fMap () - 00287 interm_cloud_->points[i].getVector4fMap ()); 00288 00289 std::vector<int> nn_indices; 00290 std::vector<float> nn_distances; 00291 00292 output_features->resize (cloud2->points.size ()); 00293 for (int point_i = 0; point_i < static_cast<int> (cloud2->points.size ()); ++point_i) 00294 { 00295 // Get neighbors 00296 tree_->radiusSearch (point_i, scale_, nn_indices, nn_distances); 00297 00298 bool largest = true; 00299 bool smallest = true; 00300 for (std::vector<int>::iterator nn_index_it = nn_indices.begin (); nn_index_it != nn_indices.end (); ++nn_index_it) 00301 { 00302 if (diffs[point_i] < diffs[*nn_index_it]) 00303 largest = false; 00304 else 00305 smallest = false; 00306 } 00307 00308 if (largest == true || smallest == true) 00309 (*output_features)[point_i] = point_i; 00310 } 00311 } 00312 00313 00314 00315 #define PCL_INSTANTIATE_SurfelSmoothing(PointT,PointNT) template class PCL_EXPORTS pcl::SurfelSmoothing<PointT, PointNT>; 00316 00317 #endif /* PCL_SURFACE_IMPL_SURFEL_SMOOTHING_H_ */