Point Cloud Library (PCL)
1.3.1
|
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 00044 template <typename PointT, typename PointNT> bool 00045 pcl::SurfelSmoothing<PointT, PointNT>::initCompute () 00046 { 00047 if (!PCLBase<PointT>::initCompute ()) 00048 return false; 00049 00050 if (!normals_) 00051 { 00052 PCL_ERROR ("SurfelSmoothing: normal cloud not set\n"); 00053 return false; 00054 } 00055 00056 if (input_->points.size () != normals_->points.size ()) 00057 { 00058 PCL_ERROR ("SurfelSmoothing: number of input points different from the number of given normals\n"); 00059 return false; 00060 } 00061 00062 // Initialize the spatial locator 00063 if (!tree_) 00064 { 00065 if (input_->isOrganized ()) 00066 tree_.reset (new pcl::search::OrganizedNeighbor<PointT> ()); 00067 else 00068 tree_.reset (new pcl::search::KdTree<PointT> (false)); 00069 } 00070 00071 // create internal copies of the input - these will be modified 00072 interm_cloud_ = PointCloudInPtr (new PointCloudIn (*input_)); 00073 interm_normals_ = NormalCloudPtr (new NormalCloud (*normals_)); 00074 00075 return (true); 00076 } 00077 00078 00080 template <typename PointT, typename PointNT> float 00081 pcl::SurfelSmoothing<PointT, PointNT>::smoothCloudIteration (PointCloudInPtr &output_positions, 00082 NormalCloudPtr &output_normals) 00083 { 00084 // PCL_INFO ("SurfelSmoothing: cloud smoothing iteration starting ...\n"); 00085 00086 output_positions = PointCloudInPtr (new PointCloudIn); 00087 output_positions->points.resize (interm_cloud_->points.size ()); 00088 output_normals = NormalCloudPtr (new NormalCloud); 00089 output_normals->points.resize (interm_cloud_->points.size ()); 00090 00091 std::vector<int> nn_indices; 00092 std::vector<float> nn_distances; 00093 00094 std::vector<float> diffs (interm_cloud_->points.size ()); 00095 float total_residual = 0.0f; 00096 00097 for (size_t i = 0; i < interm_cloud_->points.size (); ++i) 00098 { 00099 Eigen::Vector4f smoothed_point = Eigen::Vector4f::Zero (); 00100 Eigen::Vector4f smoothed_normal = Eigen::Vector4f::Zero (); 00101 00102 // get neighbors 00103 // @todo using 5x the scale for searching instead of all the points to avoid O(N^2) 00104 tree_->radiusSearch (interm_cloud_->points[i], 5*scale_, nn_indices, nn_distances); 00105 00106 float theta_normalization_factor = 0.0; 00107 std::vector<float> theta (nn_indices.size ()); 00108 for (size_t nn_index_i = 0; nn_index_i < nn_indices.size (); ++nn_index_i) 00109 { 00110 float dist = pcl::squaredEuclideanDistance (interm_cloud_->points[i], input_->points[nn_indices[nn_index_i]]);//interm_cloud_->points[nn_indices[nn_index_i]]); 00111 float theta_i = exp ( (-1) * dist / scale_squared_); 00112 theta_normalization_factor += theta_i; 00113 00114 smoothed_normal += theta_i * interm_normals_->points[nn_indices[nn_index_i]].getNormalVector4fMap (); 00115 00116 theta[nn_index_i] = theta_i; 00117 } 00118 00119 smoothed_normal /= theta_normalization_factor; 00120 smoothed_normal(3) = 0.0f; 00121 smoothed_normal.normalize (); 00122 00123 00124 // find minimum along the normal 00125 float e_residual; 00126 smoothed_point = interm_cloud_->points[i].getVector4fMap (); 00127 while (1) 00128 { 00129 e_residual = 0.0f; 00130 smoothed_point(3) = 0.0f; 00131 for (size_t nn_index_i = 0; nn_index_i < nn_indices.size (); ++nn_index_i) 00132 { 00133 Eigen::Vector4f neighbor = input_->points[nn_indices[nn_index_i]].getVector4fMap ();//interm_cloud_->points[nn_indices[nn_index_i]].getVector4fMap (); 00134 neighbor(3) = 0.0f; 00135 float dot_product = smoothed_normal.dot (neighbor - smoothed_point); 00136 e_residual += theta[nn_index_i] * dot_product;// * dot_product; 00137 } 00138 e_residual /= theta_normalization_factor; 00139 if (e_residual < 1e-5) break; 00140 00141 smoothed_point = smoothed_point + e_residual * smoothed_normal; 00142 } 00143 00144 total_residual += e_residual; 00145 00146 output_positions->points[i].getVector4fMap () = smoothed_point; 00147 output_normals->points[i].getNormalVector4fMap () = normals_->points[i].getNormalVector4fMap ();//smoothed_normal; 00148 } 00149 00150 // std::cerr << "Total residual after iteration: " << total_residual << std::endl; 00151 // PCL_INFO("SurfelSmoothing done iteration\n"); 00152 return total_residual; 00153 } 00154 00155 00156 template <typename PointT, typename PointNT> void 00157 pcl::SurfelSmoothing<PointT, PointNT>::smoothPoint (size_t &point_index, 00158 PointT &output_point, 00159 PointNT &output_normal) 00160 { 00161 Eigen::Vector4f average_normal = Eigen::Vector4f::Zero (); 00162 Eigen::Vector4f result_point = input_->points[point_index].getVector4fMap (); 00163 result_point(3) = 0.0f; 00164 00165 // @todo parameter 00166 float error_residual_threshold_ = 1e-3; 00167 float error_residual = error_residual_threshold_ + 1; 00168 float last_error_residual = error_residual + 100; 00169 00170 std::vector<int> nn_indices; 00171 std::vector<float> nn_distances; 00172 00173 int big_iterations = 0; 00174 int max_big_iterations = 500; 00175 00176 while (fabs (error_residual) < fabs (last_error_residual) -error_residual_threshold_ && 00177 big_iterations < max_big_iterations) 00178 { 00179 average_normal = Eigen::Vector4f::Zero (); 00180 big_iterations ++; 00181 PointT aux_point; aux_point.x = result_point(0); aux_point.y = result_point(1); aux_point.z = result_point(2); 00182 tree_->radiusSearch (aux_point, 5*scale_, nn_indices, nn_distances); 00183 00184 float theta_normalization_factor = 0.0; 00185 std::vector<float> theta (nn_indices.size ()); 00186 for (size_t nn_index_i = 0; nn_index_i < nn_indices.size (); ++nn_index_i) 00187 { 00188 float dist = nn_distances[nn_index_i]; 00189 float theta_i = exp ( (-1) * dist / scale_squared_); 00190 theta_normalization_factor += theta_i; 00191 00192 average_normal += theta_i * normals_->points[nn_indices[nn_index_i]].getNormalVector4fMap (); 00193 theta[nn_index_i] = theta_i; 00194 } 00195 average_normal /= theta_normalization_factor; 00196 average_normal(3) = 0.0f; 00197 average_normal.normalize (); 00198 00199 // find minimum along the normal 00200 float e_residual_along_normal = 2, last_e_residual_along_normal = 3; 00201 int small_iterations = 0; 00202 int max_small_iterations = 10; 00203 while ( fabs (e_residual_along_normal) < fabs (last_e_residual_along_normal) && 00204 small_iterations < max_small_iterations) 00205 { 00206 small_iterations ++; 00207 00208 e_residual_along_normal = 0.0f; 00209 for (size_t nn_index_i = 0; nn_index_i < nn_indices.size (); ++nn_index_i) 00210 { 00211 Eigen::Vector4f neighbor = input_->points[nn_indices[nn_index_i]].getVector4fMap (); 00212 neighbor(3) = 0.0f; 00213 float dot_product = average_normal.dot (neighbor - result_point); 00214 e_residual_along_normal += theta[nn_index_i] * dot_product; 00215 } 00216 e_residual_along_normal /= theta_normalization_factor; 00217 if (e_residual_along_normal < 1e-3) break; 00218 00219 result_point = result_point + e_residual_along_normal * average_normal; 00220 } 00221 00222 // if (small_iterations == max_small_iterations) 00223 // PCL_INFO ("passed the number of small iterations %d\n", small_iterations); 00224 00225 last_error_residual = error_residual; 00226 error_residual = e_residual_along_normal; 00227 00228 // PCL_INFO ("last %f current %f\n", last_error_residual, error_residual); 00229 } 00230 00231 output_point.x = result_point(0); 00232 output_point.y = result_point(1); 00233 output_point.z = result_point(2); 00234 output_normal = normals_->points[point_index]; 00235 00236 if (big_iterations == max_big_iterations) 00237 PCL_DEBUG ("[pcl::SurfelSmoothing::smoothPoint] Passed the number of BIG iterations: %d\n", big_iterations); 00238 } 00239 00240 00242 template <typename PointT, typename PointNT> void 00243 pcl::SurfelSmoothing<PointT, PointNT>::computeSmoothedCloud (PointCloudInPtr &output_positions, 00244 NormalCloudPtr &output_normals) 00245 { 00246 if (!initCompute ()) 00247 { 00248 PCL_ERROR ("[pcl::SurfelSmoothing::computeSmoothedCloud]: SurfelSmoothing not initialized properly, skipping computeSmoothedCloud ().\n"); 00249 return; 00250 } 00251 00252 tree_->setInputCloud (input_); 00253 00254 output_positions->header = input_->header; 00255 output_positions->height = input_->height; 00256 output_positions->width = input_->width; 00257 00258 output_normals->header = input_->header; 00259 output_normals->height = input_->height; 00260 output_normals->width = input_->width; 00261 00262 output_positions->points.resize (input_->points.size ()); 00263 output_normals->points.resize (input_->points.size ()); 00264 for (size_t i = 0; i < input_->points.size (); ++i) 00265 { 00266 smoothPoint (i, output_positions->points[i], output_normals->points[i]); 00267 } 00268 } 00269 00271 template <typename PointT, typename PointNT> void 00272 pcl::SurfelSmoothing<PointT, PointNT>::extractSalientFeaturesBetweenScales (PointCloudInPtr &cloud2, 00273 NormalCloudPtr &cloud2_normals, 00274 boost::shared_ptr<std::vector<int> > &output_features) 00275 { 00276 if (interm_cloud_->points.size () != cloud2->points.size () || 00277 cloud2->points.size () != cloud2_normals->points.size ()) 00278 { 00279 PCL_ERROR ("[pcl::SurfelSmoothing::extractSalientFeaturesBetweenScales]: Number of points in the clouds does not match.\n"); 00280 return; 00281 } 00282 00283 std::vector<float> diffs (cloud2->points.size ()); 00284 for (size_t i = 0; i < cloud2->points.size (); ++i) 00285 diffs[i] = cloud2_normals->points[i].getNormalVector4fMap ().dot (cloud2->points[i].getVector4fMap () - 00286 interm_cloud_->points[i].getVector4fMap ()); 00287 00288 std::vector<int> nn_indices; 00289 std::vector<float> nn_distances; 00290 00291 output_features->resize (cloud2->points.size ()); 00292 for (size_t point_i = 0; point_i < cloud2->points.size (); ++point_i) 00293 { 00294 // Get neighbors 00295 tree_->radiusSearch (point_i, scale_, nn_indices, nn_distances); 00296 00297 bool largest = true; 00298 bool smallest = true; 00299 for (std::vector<int>::iterator nn_index_it = nn_indices.begin (); nn_index_it != nn_indices.end (); ++nn_index_it) 00300 { 00301 if (diffs[point_i] < diffs[*nn_index_it]) 00302 largest = false; 00303 else 00304 smallest = false; 00305 } 00306 00307 if (largest == true || smallest == true) 00308 (*output_features)[point_i] = point_i; 00309 } 00310 } 00311 00312 00313 00314 #define PCL_INSTANTIATE_SurfelSmoothing(PointT,PointNT) template class PCL_EXPORTS pcl::SurfelSmoothing<PointT, PointNT>; 00315 00316 #endif /* PCL_SURFACE_IMPL_SURFEL_SMOOTHING_H_ */