00001 #include "shape_reconstruction/SurfaceSmootherNode.h"
00002
00003 #include <pcl/conversions.h>
00004 #include <pcl/io/pcd_io.h>
00005 #include <pcl/filters/filter_indices.h>
00006 #include <sensor_msgs/Image.h>
00007
00008 #include <opencv2/imgproc/imgproc.hpp>
00009 #include <opencv2/video/tracking.hpp>
00010 #include <opencv2/highgui/highgui.hpp>
00011 #include <opencv2/video/tracking.hpp>
00012
00013 #include <pcl_conversions/pcl_conversions.h>
00014
00015 #include <ros/package.h>
00016
00017 #include <cmath>
00018
00019 #include <ros/console.h>
00020
00021 #include "shape_reconstruction/RangeImagePlanar.hpp"
00022
00023 #include <pcl/point_cloud.h>
00024 #include <pcl/point_types.h>
00025 #include <pcl/io/pcd_io.h>
00026 #include <pcl/visualization/pcl_visualizer.h>
00027 #include <pcl/filters/passthrough.h>
00028
00029 #include <boost/filesystem.hpp>
00030 #include <ctime>
00031
00032 #include <std_msgs/Bool.h>
00033
00034 #include <iostream>
00035 #include <pcl/common/common.h>
00036 #include <pcl/io/pcd_io.h>
00037 #include <pcl/io/ply_io.h>
00038 #include <pcl/search/kdtree.h>
00039 #include <pcl/features/normal_3d_omp.h>
00040 #include <pcl/point_types.h>
00041 #include <pcl/surface/mls.h>
00042 #include <pcl/surface/poisson.h>
00043 #include <pcl/surface/marching_cubes.h>
00044 #include <pcl/surface/marching_cubes_rbf.h>
00045 #include <pcl/surface/marching_cubes_hoppe.h>
00046 #include <pcl/surface/ear_clipping.h>
00047 #include <pcl/surface/vtk_smoothing/vtk_mesh_smoothing_laplacian.h>
00048 #include <pcl/surface/vtk_smoothing/vtk_mesh_smoothing_windowed_sinc.h>
00049 #include <pcl/surface/vtk_smoothing/vtk_mesh_subdivision.h>
00050 #include <pcl/filters/passthrough.h>
00051
00052 #include <pcl/io/vtk_lib_io.h>
00053 #include <pcl/visualization/pcl_visualizer.h>
00054
00055
00056 #include <vcg/complex/complex.h>
00057 #include <vcg/complex/all_types.h>
00058 #include <vcg/complex/algorithms/clean.h>
00059
00060 #include <vcg/complex/algorithms/update/bounding.h>
00061 #include <vcg/complex/algorithms/update/normal.h>
00062 #include <vcg/complex/algorithms/update/topology.h>
00063
00064 #include <pcl/surface/convex_hull.h>
00065 #include <pcl/surface/concave_hull.h>
00066
00067 #include <pcl/filters/normal_refinement.h>
00068
00069 #include <pcl/surface/simplification_remove_unused_vertices.h>
00070
00071 using namespace omip;
00072
00073 SurfaceSmootherNode::SurfaceSmootherNode()
00074 {
00075 std::string topic_name;
00076 this->_node_handle.getParam("/surface_smoother/topic_name", topic_name);
00077 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode","topic_name: " << topic_name);
00078 this->_pc_subscriber = this->_node_handle.subscribe(topic_name, 1,
00079 &SurfaceSmootherNode::measurementCallback, this);
00080 this->_current_pc.reset(new SRPointCloud());
00081 this->_current_pc_copy.reset(new SRPointCloud());
00082 }
00083
00084 SurfaceSmootherNode::~SurfaceSmootherNode()
00085 {
00086
00087 }
00088
00089 void SurfaceSmootherNode::measurementCallback(const sensor_msgs::PointCloud2ConstPtr &pc_msg)
00090 {
00091
00092 pcl::fromROSMsg(*pc_msg, *this->_current_pc);
00093
00094 pcl::fromROSMsg(*pc_msg, *this->_current_pc_copy);
00095
00096
00097 this->generateMesh();
00098 }
00099
00100 void SurfaceSmootherNode::ReadRosBag()
00101 {
00102
00103
00104 }
00105
00106 void SurfaceSmootherNode::generateMesh()
00107 {
00108 bool preprocessing_remove_nans;
00109 this->_node_handle.getParam("/surface_smoother/preprocessing_remove_nans", preprocessing_remove_nans);
00110 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode","preprocessing_remove_nans: " << preprocessing_remove_nans);
00111 if(preprocessing_remove_nans)
00112 {
00113 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode.generateMesh", "Begin NaNs removal");
00114
00115
00116 std::vector<int> not_nan_indices;
00117 pcl::removeNaNFromPointCloud<pcl::PointXYZRGB>(*this->_current_pc,*this->_current_pc, not_nan_indices);
00118
00119 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode.generateMesh", "End NaNs removal");
00120 }
00121
00122 bool preprocessing_rar;
00123 this->_node_handle.getParam("/surface_smoother/preprocessing_rar", preprocessing_rar);
00124 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode","preprocessing_rar: " << preprocessing_rar);
00125 if(preprocessing_rar)
00126 {
00127 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode.generateMesh", "Begin radius outlier removal");
00128
00129 int rar_num_neighbors;
00130 this->_node_handle.getParam("/surface_smoother/rar_num_neighbors", rar_num_neighbors);
00131 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode","rar_num_neighbors: " << rar_num_neighbors);
00132
00133 float rar_search_radius;
00134 this->_node_handle.getParam("/surface_smoother/rar_search_radius", rar_search_radius);
00135 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode","rar_search_radius: " << rar_search_radius);
00136
00137 pcl::RadiusOutlierRemoval<pcl::PointXYZRGB> rar;
00138
00139 rar.setMinNeighborsInRadius(rar_num_neighbors);\
00140 rar.setRadiusSearch(rar_search_radius);
00141 rar.setInputCloud(this->_current_pc);
00142
00143 rar.filter(*this->_current_pc);
00144
00145 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode.generateMesh", "End radius outlier removal");
00146 }
00147
00148 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr combined_rgbd_normals(new pcl::PointCloud<pcl::PointXYZRGBNormal>());
00149 pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>());
00150
00151 int pre_meshing_and_ne;
00152 this->_node_handle.getParam("/surface_smoother/pre_meshing", pre_meshing_and_ne);
00153 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode","pre_meshing: " << pre_meshing_and_ne);
00154 switch(pre_meshing_and_ne)
00155 {
00156 case 0:
00157 default:
00158 {
00159 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode.generateMesh", "Begin normals estimation");
00160
00161 pcl::NormalEstimationOMP<pcl::PointXYZRGB, pcl::Normal> ne;
00162 ne.setNumberOfThreads(8);
00163 ne.setInputCloud(this->_current_pc);
00164
00165
00166 int ne_num_neighbors;
00167 this->_node_handle.getParam("/surface_smoother/ne_num_neighbors", ne_num_neighbors);
00168 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode.generateMesh","ne_num_neighbors: " << ne_num_neighbors);
00169
00170 double ne_min_radius;
00171 this->_node_handle.getParam("/surface_smoother/ne_min_radius", ne_min_radius);
00172 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode.generateMesh","ne_min_radius: " << ne_min_radius);
00173
00174 if(ne_num_neighbors != -1 && ne_min_radius != -1)
00175 {
00176 ROS_WARN_STREAM_NAMED("SurfaceSmootherNode.generateMesh","Both ne_min_radius and ne_num_neighbors are not -1. Using ne_num_neighbors." << ne_min_radius);
00177 }
00178
00179 if(ne_num_neighbors != -1)
00180 {
00181 ne.setKSearch(ne_num_neighbors);
00182 }else{
00183 ne.setRadiusSearch(ne_num_neighbors);
00184 }
00185
00186 ne.useSensorOriginAsViewPoint();
00187 ne.compute(*cloud_normals);
00188
00189 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode.generateMesh", "End normals estimation");
00190
00191 concatenateFields(*this->_current_pc, *cloud_normals, *combined_rgbd_normals);
00192 break;
00193 }
00194 case 1:
00195 {
00196 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode.generateMesh", "Begin MLS and normals estimation");
00197
00198 float mls_search_radius;
00199 this->_node_handle.getParam("/surface_smoother/mls_search_radius", mls_search_radius);
00200 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode","mls_search_radius: " << mls_search_radius);
00201
00202
00203 pcl::search::KdTree<pcl::PointXYZRGB>::Ptr mls_tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
00204
00205
00206 pcl::MovingLeastSquares<pcl::PointXYZRGB, pcl::PointXYZRGBNormal> mls;
00207
00208 mls.setComputeNormals (true);
00209
00210
00211 mls.setInputCloud (this->_current_pc);
00212 mls.setPolynomialFit (true);
00213 mls.setSearchMethod (mls_tree);
00214 mls.setSearchRadius (mls_search_radius);
00215
00216
00217 mls.process (*combined_rgbd_normals);
00218
00219 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode.generateMesh", "End MLS and normals estimation");
00220 break;
00221 }
00222 }
00223
00224 bool align_normals;
00225 this->_node_handle.getParam("/surface_smoother/align_normals", align_normals);
00226 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode","align_normals: " << align_normals);
00227 if(align_normals)
00228 {
00229 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode.generateMesh", "Begin normals aligment");
00230
00231 bool use_centroid_for_normal_alignment;
00232 this->_node_handle.getParam("/surface_smoother/use_centroid_for_normal_alignment", use_centroid_for_normal_alignment);
00233 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode","use_centroid_for_normal_alignment: " << use_centroid_for_normal_alignment);
00234
00235 Eigen::Vector4f point_to_align;
00236
00237 if(use_centroid_for_normal_alignment)
00238 {
00239 pcl::compute3DCentroid(*this->_current_pc, point_to_align);
00240 }else{
00241 point_to_align[0] = 0;
00242 point_to_align[1] = 0;
00243 point_to_align[2] = 0;
00244 }
00245
00246 for(size_t i = 0; i < combined_rgbd_normals->size(); ++i)
00247 {
00248 pcl::flipNormalTowardsViewpoint<pcl::PointXYZRGBNormal>(combined_rgbd_normals->points[i], point_to_align[0], point_to_align[1], point_to_align[2],
00249 combined_rgbd_normals->points[i].normal_x,
00250 combined_rgbd_normals->points[i].normal_y,
00251 combined_rgbd_normals->points[i].normal_z);
00252 }
00253
00254
00255 for(size_t i = 0; i < combined_rgbd_normals->size(); ++i)
00256 {
00257
00258 pcl::flipNormalTowardsViewpoint<pcl::PointXYZRGBNormal>(combined_rgbd_normals->points[i], point_to_align[0], point_to_align[1], point_to_align[2],
00259 cloud_normals->points[i].normal_x,
00260 cloud_normals->points[i].normal_y,
00261 cloud_normals->points[i].normal_z);
00262 }
00263
00264 if(use_centroid_for_normal_alignment)
00265 {
00266 for(size_t i = 0; i < cloud_normals->size(); ++i){
00267 cloud_normals->points[i].normal_x *= -1;
00268 cloud_normals->points[i].normal_y *= -1;
00269 cloud_normals->points[i].normal_z *= -1;
00270 }
00271
00272 for(size_t i = 0; i < combined_rgbd_normals->size(); ++i){
00273 combined_rgbd_normals->points[i].normal_x *= -1;
00274 combined_rgbd_normals->points[i].normal_y *= -1;
00275 combined_rgbd_normals->points[i].normal_z *= -1;
00276 }
00277 }
00278
00279
00280
00281
00282
00283
00284 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode.generateMesh", "End normals aligment");
00285 }
00286
00287 bool show_pc_and_normals;
00288 this->_node_handle.getParam("/surface_smoother/show_pc_and_normals", show_pc_and_normals);
00289 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode","show_pc_and_normals: " << show_pc_and_normals);
00290 if(show_pc_and_normals)
00291 {
00292 boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer1 (new pcl::visualization::PCLVisualizer ("3D Viewer"));
00293 viewer1->setBackgroundColor (0, 0, 0);
00294 viewer1->addPointCloud(this->_current_pc, "cloudy");
00295 viewer1->addPointCloudNormals<pcl::PointXYZRGB, pcl::Normal>(this->_current_pc, cloud_normals,100, 0.05,"normals");
00296
00297 Eigen::Vector4f centroid2;
00298 pcl::compute3DCentroid(*this->_current_pc, centroid2);
00299 viewer1->addCoordinateSystem(0.2,centroid2[0],centroid2[1], centroid2[2]);
00300
00301 viewer1->addCoordinateSystem (0.10);
00302 viewer1->initCameraParameters ();
00303
00304 while (!viewer1->wasStopped ()){
00305 viewer1->spinOnce (100);
00306 boost::this_thread::sleep (boost::posix_time::microseconds (100000));
00307 }
00308 }
00309
00310 pcl::PolygonMesh mesh;
00311
00312 int meshing_method;
00313 this->_node_handle.getParam("/surface_smoother/meshing_method", meshing_method);
00314 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode.generateMesh","meshing_method: " << meshing_method);
00315 switch(meshing_method)
00316 {
00317 case 1:
00318 default:
00319 {
00320 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode.generateMesh", "Begin Poisson surface reconstruction");
00321
00322 int poisson_depth;
00323 this->_node_handle.getParam("/surface_smoother/poisson_depth", poisson_depth);
00324 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode","poisson_depth: " << poisson_depth);
00325
00326 bool poisson_confidence;
00327 this->_node_handle.getParam("/surface_smoother/poisson_confidence", poisson_confidence);
00328 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode","poisson_confidence: " << poisson_confidence);
00329
00330 int poisson_samples_per_node;
00331 this->_node_handle.getParam("/surface_smoother/poisson_samples_per_node", poisson_samples_per_node);
00332 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode","poisson_samples_per_node: " << poisson_samples_per_node);
00333
00334 pcl::Poisson<pcl::PointXYZRGBNormal> poisson;
00335 poisson.setDepth(poisson_depth);
00336 poisson.setOutputPolygons(false);
00337 poisson.setConfidence(poisson_confidence);
00338 poisson.setSamplesPerNode(poisson_samples_per_node);
00339
00340
00341 poisson.setInputCloud(combined_rgbd_normals);
00342 poisson.reconstruct(mesh);
00343
00344 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode.generateMesh", "End Poisson surface reconstruction");
00345 break;
00346 }
00347 case 2:
00348 {
00349 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode.generateMesh", "Begin Marching Cubes RBF surface reconstruction (this will very likely crush with a SegFault!)");
00350
00351 int mc_grid_resolution;
00352 this->_node_handle.getParam("/surface_smoother/mc_grid_resolution", mc_grid_resolution);
00353 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode","mc_grid_resolution: " << mc_grid_resolution);
00354
00355 double mc_percentage_ext_grid;
00356 this->_node_handle.getParam("/surface_smoother/mc_percentage_ext_grid", mc_percentage_ext_grid);
00357 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode","mc_percentage_ext_grid: " << mc_percentage_ext_grid);
00358
00359 double mc_rbf_offset_sfc_disp;
00360 this->_node_handle.getParam("/surface_smoother/mc_rbf_offset_sfc_disp", mc_rbf_offset_sfc_disp);
00361 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode","mc_rbf_offset_sfc_disp: " << mc_rbf_offset_sfc_disp);
00362
00363 double mc_iso_level;
00364 this->_node_handle.getParam("/surface_smoother/mc_iso_level", mc_iso_level);
00365 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode","mc_iso_level: " << mc_iso_level);
00366
00367 pcl::search::KdTree<pcl::PointXYZRGBNormal>::Ptr tree2 (new pcl::search::KdTree<pcl::PointXYZRGBNormal>);
00368 tree2->setInputCloud (combined_rgbd_normals);
00369 pcl::MarchingCubesRBF<pcl::PointXYZRGBNormal> mc;
00370 mc.setIsoLevel (mc_iso_level);
00371 mc.setGridResolution (mc_grid_resolution, mc_grid_resolution, mc_grid_resolution);
00372 mc.setPercentageExtendGrid (mc_percentage_ext_grid);
00373 mc.setOffSurfaceDisplacement (mc_rbf_offset_sfc_disp);
00374 mc.setInputCloud(combined_rgbd_normals);
00375 mc.setSearchMethod (tree2);
00376 mc.reconstruct (mesh);
00377
00378 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode.generateMesh", "End Marching Cubes RBF surface reconstruction");
00379 break;
00380 }
00381 case 3:
00382 {
00383 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode.generateMesh", "Begin Marching Cubes Hoppe surface reconstruction");
00384
00385 int mc_grid_resolution;
00386 this->_node_handle.getParam("/surface_smoother/mc_grid_resolution", mc_grid_resolution);
00387 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode","mc_grid_resolution: " << mc_grid_resolution);
00388
00389 double mc_percentage_ext_grid;
00390 this->_node_handle.getParam("/surface_smoother/mc_percentage_ext_grid", mc_percentage_ext_grid);
00391 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode","mc_percentage_ext_grid: " << mc_percentage_ext_grid);
00392
00393 double mc_iso_level;
00394 this->_node_handle.getParam("/surface_smoother/mc_iso_level", mc_iso_level);
00395 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode","mc_iso_level: " << mc_iso_level);
00396
00397 pcl::search::KdTree<pcl::PointXYZRGBNormal>::Ptr tree2 (new pcl::search::KdTree<pcl::PointXYZRGBNormal>);
00398 tree2->setInputCloud (combined_rgbd_normals);
00399 pcl::MarchingCubesHoppe<pcl::PointXYZRGBNormal> mc;
00400 mc.setIsoLevel (mc_iso_level);
00401 mc.setGridResolution (mc_grid_resolution, mc_grid_resolution, mc_grid_resolution);
00402 mc.setPercentageExtendGrid (mc_percentage_ext_grid);
00403 mc.setInputCloud(combined_rgbd_normals);
00404 mc.setSearchMethod (tree2);
00405 mc.reconstruct (mesh);
00406
00407 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode.generateMesh", "End Marching Cubes Hoppe surface reconstruction");
00408 break;
00409 }
00410 case 4:
00411 {
00412 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode.generateMesh", "Begin Convex hull surface reconstruction");
00413 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr convex_hull(new pcl::PointCloud<pcl::PointXYZRGBNormal>());
00414 pcl::ConvexHull<pcl::PointXYZRGBNormal> cHull;
00415 cHull.setComputeAreaVolume(false);
00416 cHull.setInputCloud(combined_rgbd_normals);
00417 cHull.reconstruct (*convex_hull, mesh.polygons);
00418 pcl::toPCLPointCloud2(*convex_hull, mesh.cloud);
00419
00420 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode.generateMesh", "End Convex hull surface reconstruction");
00421 break;
00422 }
00423 }
00424
00425 bool orient_normals_in_mesh;
00426 this->_node_handle.getParam("/surface_smoother/orient_normals_in_mesh", orient_normals_in_mesh);
00427 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode.generateMesh","orient_normals_in_mesh: " << orient_normals_in_mesh);
00428 if(orient_normals_in_mesh)
00429 {
00430 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode.generateMesh", "Begin orienting normals in mesh");
00431 orientNormalsInMesh(mesh);
00432 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode.generateMesh", "End orienting normals in mesh");
00433 }
00434
00435 pcl::PolygonMesh processed_mesh;
00436
00437 int post_meshing_method;
00438 this->_node_handle.getParam("/surface_smoother/post_meshing_method", post_meshing_method);
00439 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode.generateMesh","post_meshing_method: " << post_meshing_method);
00440 switch(post_meshing_method)
00441 {
00442 case 0:
00443 default:
00444 {
00445 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode.generateMesh", "No post-processing of the mesh");
00446 processed_mesh = mesh;
00447 break;
00448 }
00449 case 1:
00450 {
00451 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode.generateMesh", "Begin Earclipping");
00452
00453 pcl::EarClipping clipper;
00454 pcl::PolygonMesh::ConstPtr mesh_aux (new pcl::PolygonMesh(mesh));
00455
00456 clipper.setInputMesh (mesh_aux);
00457 clipper.process (processed_mesh);
00458
00459 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode.generateMesh", "End Earclipping");
00460 break;
00461 }
00462 case 2:
00463 {
00464 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode.generateMesh", "Begin Smoothing Laplacian VTK");
00465
00466 bool ms_boundary_smoothing;
00467 this->_node_handle.getParam("/surface_smoother/ms_boundary_smoothing", ms_boundary_smoothing);
00468 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode","ms_boundary_smoothing: " << ms_boundary_smoothing);
00469
00470 int ms_num_iter;
00471 this->_node_handle.getParam("/surface_smoother/ms_num_iter", ms_num_iter);
00472 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode","ms_num_iter: " << ms_num_iter);
00473
00474 bool ms_feat_edge_smoothing;
00475 this->_node_handle.getParam("/surface_smoother/ms_feat_edge_smoothing", ms_feat_edge_smoothing);
00476 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode","ms_feat_edge_smoothing: " << ms_feat_edge_smoothing);
00477
00478 float ms_feat_angle;
00479 this->_node_handle.getParam("/surface_smoother/ms_feat_angle", ms_feat_angle);
00480 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode","ms_feat_angle: " << ms_feat_angle);
00481
00482 float ms_lap_convergence;
00483 this->_node_handle.getParam("/surface_smoother/ms_lap_convergence", ms_lap_convergence);
00484 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode","ms_lap_convergence: " << ms_lap_convergence);
00485
00486 float ms_lap_relaxation_factor;
00487 this->_node_handle.getParam("/surface_smoother/ms_lap_relaxation_factor", ms_lap_relaxation_factor);
00488 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode","ms_lap_relaxation_factor: " << ms_lap_relaxation_factor);
00489
00490 float ms_edge_angle;
00491 this->_node_handle.getParam("/surface_smoother/ms_edge_angle", ms_edge_angle);
00492 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode","ms_edge_angle: " << ms_edge_angle);
00493
00494 pcl::MeshSmoothingLaplacianVTK ms;
00495 pcl::PolygonMesh::ConstPtr mesh_aux (new pcl::PolygonMesh(mesh));
00496
00497 ms.setBoundarySmoothing(ms_boundary_smoothing);
00498 ms.setNumIter(ms_num_iter);
00499 ms.setFeatureEdgeSmoothing(ms_feat_edge_smoothing);
00500 ms.setFeatureAngle(ms_feat_angle);
00501 ms.setConvergence(ms_lap_convergence);
00502 ms.setRelaxationFactor(ms_lap_relaxation_factor);
00503 ms.setEdgeAngle(ms_edge_angle);
00504
00505 ms.setInputMesh (mesh_aux);
00506 ms.process (processed_mesh);
00507
00508 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode.generateMesh", "End Smoothing Laplacian VTK");
00509 break;
00510 }
00511 case 3:
00512 {
00513 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode.generateMesh", "Begin Smoothing Windowed Sinc VTK");
00514
00515 int ms_num_iter;
00516 this->_node_handle.getParam("/surface_smoother/ms_num_iter", ms_num_iter);
00517 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode","ms_num_iter: " << ms_num_iter);
00518
00519 bool ms_boundary_smoothing;
00520 this->_node_handle.getParam("/surface_smoother/ms_boundary_smoothing", ms_boundary_smoothing);
00521 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode","ms_boundary_smoothing: " << ms_boundary_smoothing);
00522
00523 bool ms_feat_edge_smoothing;
00524 this->_node_handle.getParam("/surface_smoother/ms_feat_edge_smoothing", ms_feat_edge_smoothing);
00525 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode","ms_feat_edge_smoothing: " << ms_feat_edge_smoothing);
00526
00527 float ms_feat_angle;
00528 this->_node_handle.getParam("/surface_smoother/ms_feat_angle", ms_feat_angle);
00529 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode","ms_feat_angle: " << ms_feat_angle);
00530
00531 float ms_edge_angle;
00532 this->_node_handle.getParam("/surface_smoother/ms_edge_angle", ms_edge_angle);
00533 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode","ms_edge_angle: " << ms_edge_angle);
00534
00535 bool ms_win_norm_coord;
00536 this->_node_handle.getParam("/surface_smoother/ms_win_norm_coord", ms_win_norm_coord);
00537 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode","ms_win_norm_coord: " << ms_win_norm_coord);
00538
00539 float ms_win_pass_band;
00540 this->_node_handle.getParam("/surface_smoother/ms_win_pass_band", ms_win_pass_band);
00541 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode","ms_win_pass_band: " << ms_win_pass_band);
00542
00543 pcl::MeshSmoothingWindowedSincVTK ms;
00544 pcl::PolygonMesh::ConstPtr mesh_aux (new pcl::PolygonMesh(mesh));
00545
00546 ms.setBoundarySmoothing(ms_boundary_smoothing);
00547 ms.setNumIter(ms_num_iter);
00548 ms.setFeatureEdgeSmoothing(ms_feat_edge_smoothing);
00549 ms.setFeatureAngle(ms_feat_angle);
00550 ms.setEdgeAngle(ms_edge_angle);
00551 ms.setNormalizeCoordinates(ms_win_norm_coord);
00552 ms.setPassBand(ms_win_pass_band);
00553
00554 ms.setInputMesh (mesh_aux);
00555 ms.process (processed_mesh);
00556
00557 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode.generateMesh", "End Smoothing Windowed Sinc VTK");
00558 break;
00559 }
00560 }
00561
00562 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode.generateMesh", "Begin coloring the mesh");
00563 this->colorMesh(processed_mesh);
00564 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode.generateMesh", "End coloring the mesh");
00565
00566 bool save_mesh;
00567 this->_node_handle.getParam("/surface_smoother/save_mesh", save_mesh);
00568 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode.generateMesh","save_mesh: " << save_mesh);
00569 if(save_mesh)
00570 {
00571 std::string save_mesh_filename;
00572 this->_node_handle.getParam("/surface_smoother/save_mesh_filename", save_mesh_filename);
00573 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode.generateMesh","save_mesh_filename: " << save_mesh_filename);
00574
00575 std::string save_mesh_filename_ply_with_dots = save_mesh_filename + std::string(".withdots.ply");
00576 std::string save_mesh_filename_ply = save_mesh_filename + std::string(".ply");
00577 pcl::io::savePLYFile(save_mesh_filename_ply_with_dots, processed_mesh);
00578
00579
00580 std::string with_dots_line;
00581 std::ifstream with_dots;
00582 with_dots.open(save_mesh_filename_ply_with_dots);
00583 std::ofstream with_commas;
00584 with_commas.open(save_mesh_filename_ply);
00585 while (!with_dots.eof())
00586 {
00587 getline(with_dots, with_dots_line);
00588 std::replace(with_dots_line.begin(), with_dots_line.end(), '.', ',');
00589 with_commas << with_dots_line << std::endl;
00590 }
00591
00592 with_dots.close();
00593 with_commas.close();
00594
00595 std::string save_mesh_filename_stl = save_mesh_filename + std::string(".stl");
00596
00597 vtkSmartPointer<vtkPolyData> vtk_polygon_data_ptr = vtkSmartPointer<vtkPolyData>::New ();
00598 vtkSmartPointer<vtkSTLWriter> vtk_polygon_writer_ptr = vtkSmartPointer<vtkSTLWriter>::New ();
00599 vtk_polygon_writer_ptr->SetFileTypeToBinary();
00600
00601 pcl::io::mesh2vtk(processed_mesh, vtk_polygon_data_ptr);
00602
00603 vtk_polygon_writer_ptr->SetInput (vtk_polygon_data_ptr);
00604 vtk_polygon_writer_ptr->SetFileName (save_mesh_filename_stl.c_str ());
00605 vtk_polygon_writer_ptr->Write ();
00606 }
00607
00608 boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
00609 viewer->setBackgroundColor (0, 0, 0);
00610 viewer->addPolygonMesh(processed_mesh,"meshes");
00611
00612 bool show_pc_with_mesh;
00613 this->_node_handle.getParam("/surface_smoother/show_pc_with_mesh", show_pc_with_mesh);
00614 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode.generateMesh","show_pc_with_mesh: " << show_pc_with_mesh);
00615 if(show_pc_with_mesh)
00616 {
00617 viewer->addPointCloud(this->_current_pc, "cloudy");
00618 }
00619
00620 viewer->addCoordinateSystem (0.10);
00621 viewer->initCameraParameters ();
00622
00623 while (!viewer->wasStopped ())
00624 {
00625 viewer->spinOnce (100);
00626 boost::this_thread::sleep (boost::posix_time::microseconds (100000));
00627 }
00628
00629
00630 ROS_ERROR_STREAM_NAMED("SurfaceSmootherNode.generateMesh","Finished! Send me another pc, you bastard!");
00631 }
00632
00633 void SurfaceSmootherNode::colorMesh(pcl::PolygonMesh& mesh)
00634 {
00635 bool clean_mesh_while_coloring;
00636 this->_node_handle.getParam("/surface_smoother/clean_mesh_while_coloring", clean_mesh_while_coloring);
00637 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode.generateMesh","clean_mesh_while_coloring: " << clean_mesh_while_coloring);
00638
00639 bool interpolate_colors;
00640 this->_node_handle.getParam("/surface_smoother/interpolate_colors", interpolate_colors);
00641 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode","interpolate_colors: " << interpolate_colors);
00642
00643 int num_neighbors_color_interp;
00644 this->_node_handle.getParam("/surface_smoother/num_neighbors_color_interp", num_neighbors_color_interp);
00645 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode","num_neighbors_color_interp: " << num_neighbors_color_interp);
00646
00647 float radius_search_color_interp;
00648 this->_node_handle.getParam("/surface_smoother/radius_search_color_interp", radius_search_color_interp);
00649 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode","radius_search_color_interp: " << radius_search_color_interp);
00650
00651 bool print_outs_color_interp;
00652 this->_node_handle.getParam("/surface_smoother/print_outs_color_interp", print_outs_color_interp);
00653 ROS_INFO_STREAM_NAMED("SurfaceSmootherNode","print_outs_color_interp: " << print_outs_color_interp);
00654
00655 pcl::KdTreeFLANN<pcl::PointXYZRGB> kdtree;
00656 kdtree.setInputCloud (this->_current_pc);
00657
00658
00659 pcl::PointCloud<pcl::PointXYZ> cloud_mesh;
00660 pcl::fromPCLPointCloud2(mesh.cloud, cloud_mesh);
00661
00662
00663 pcl::PointCloud<pcl::PointXYZRGB> cloud_color_mesh;
00664 copyPointCloud(cloud_mesh, cloud_color_mesh);
00665
00666 if(interpolate_colors)
00667 {
00668
00669 std::vector<int> pointIdxNKNSearch(num_neighbors_color_interp);
00670 std::vector<float> pointNKNSquaredDistance(num_neighbors_color_interp);
00671
00672
00673 for(size_t i=0; i< cloud_color_mesh.points.size();++i)
00674 {
00675 float red = 0.0,green = 0.0, blue = 0.0, dist = 0.0;
00676 int red_int = 0, green_int =0, blue_int=0;
00677 if(kdtree.radiusSearch(cloud_color_mesh.points[i], radius_search_color_interp, pointIdxNKNSearch, pointNKNSquaredDistance, num_neighbors_color_interp) > 0 )
00678
00679 {
00680
00681 for (size_t j = 0; j < pointIdxNKNSearch.size (); ++j)
00682 {
00683 red += (int)this->_current_pc->points[ pointIdxNKNSearch[j] ].r * 1.0/pointNKNSquaredDistance[j];
00684 green += (int)this->_current_pc->points[ pointIdxNKNSearch[j] ].g * 1.0/pointNKNSquaredDistance[j];
00685 blue += (int)this->_current_pc->points[ pointIdxNKNSearch[j] ].b * 1.0/pointNKNSquaredDistance[j];
00686 dist += 1.0/pointNKNSquaredDistance[j];
00687
00688 if(print_outs_color_interp)
00689 {
00690 std::cout<<"partial red: "<< (int)this->_current_pc->points[ pointIdxNKNSearch[j] ].r << std::endl;
00691 std::cout<<"partial green: "<< (int)this->_current_pc->points[ pointIdxNKNSearch[j] ].g << std::endl;
00692 std::cout<<"partial blue: "<< (int)this->_current_pc->points[ pointIdxNKNSearch[j] ].b << std::endl;
00693 std::cout<<"dist: "<< pointNKNSquaredDistance[j] << std::endl;
00694 getchar();
00695 }
00696 }
00697 red_int = floor(red/dist+0.5);
00698 green_int = floor(green/dist+0.5);
00699 blue_int = floor(blue/dist+0.5);
00700 }
00701 else if(clean_mesh_while_coloring)
00702 {
00703
00704
00705
00706
00707 cloud_color_mesh.points[i].x = 0;
00708 cloud_color_mesh.points[i].y = 0;
00709 cloud_color_mesh.points[i].z = 0;
00710 std::vector<int> polygons_to_remove;
00711 for(int k=0; k<mesh.polygons.size(); k++)
00712 {
00713
00714 if(std::find(mesh.polygons[k].vertices.begin(), mesh.polygons[k].vertices.end(), i) != mesh.polygons[k].vertices.end())
00715 {
00716
00717
00718
00719
00720 polygons_to_remove.push_back(k);
00721 }
00722 }
00723
00724 for(int r=polygons_to_remove.size() -1; r>=0; r--)
00725 {
00726 mesh.polygons.erase(mesh.polygons.begin() + polygons_to_remove[r]);
00727 }
00728 }
00729
00730 if(print_outs_color_interp)
00731 {
00732 std::cout<<"red: "<< red << std::endl;
00733 std::cout<<"green: "<< green << std::endl;
00734 std::cout<<"blue: "<< blue << std::endl;
00735 std::cout<<"red int: "<< red_int << std::endl;
00736 std::cout<<"green int: "<< green_int << std::endl;
00737 std::cout<<"blue int: "<< blue_int << std::endl;
00738 std::cout<<"--------"<<std::endl;
00739 getchar();
00740 }
00741
00742
00743 cloud_color_mesh.points[i].r = red_int;
00744 cloud_color_mesh.points[i].g = green_int;
00745 cloud_color_mesh.points[i].b = blue_int;
00746 }
00747 }else{
00748
00749 int K = 1;
00750 std::vector<int> pointIdxNKNSearch(K);
00751 std::vector<float> pointNKNSquaredDistance(K);
00752
00753
00754 for(size_t i=0; i< cloud_color_mesh.points.size();++i)
00755 {
00756 int red_int = 0, green_int =0, blue_int=0;
00757 if(kdtree.radiusSearch(cloud_color_mesh.points[i], radius_search_color_interp, pointIdxNKNSearch, pointNKNSquaredDistance, 1) > 0 )
00758
00759 {
00760 red_int = (int)this->_current_pc->points[ pointIdxNKNSearch[0] ].r ;
00761 green_int = (int)this->_current_pc->points[ pointIdxNKNSearch[0] ].g ;
00762 blue_int = (int)this->_current_pc->points[ pointIdxNKNSearch[0] ].b ;
00763 }else if(clean_mesh_while_coloring)
00764 {
00765
00766
00767
00768 cloud_color_mesh.points[i].x = 0;
00769 cloud_color_mesh.points[i].y = 0;
00770 cloud_color_mesh.points[i].z = 0;
00771 std::vector<int> polygons_to_remove;
00772 for(int k=0; k<mesh.polygons.size(); k++)
00773 {
00774
00775 if(std::find(mesh.polygons[k].vertices.begin(), mesh.polygons[k].vertices.end(), i) != mesh.polygons[k].vertices.end())
00776 {
00777
00778
00779
00780 polygons_to_remove.push_back(k);
00781 }
00782 }
00783 for(int r=polygons_to_remove.size() -1; r>=0; r--)
00784 {
00785 mesh.polygons.erase(mesh.polygons.begin() + polygons_to_remove[r]);
00786 }
00787 }
00788
00789 if(print_outs_color_interp)
00790 {
00791 std::cout<<"red int: "<< red_int << std::endl;
00792 std::cout<<"green int: "<< green_int << std::endl;
00793 std::cout<<"blue int: "<< blue_int << std::endl;
00794 std::cout<<"--------"<<std::endl;
00795 getchar();
00796 }
00797
00798
00799 cloud_color_mesh.points[i].r = red_int;
00800 cloud_color_mesh.points[i].g = green_int;
00801 cloud_color_mesh.points[i].b = blue_int;
00802 }
00803 }
00804
00805
00806 pcl::toPCLPointCloud2(cloud_color_mesh, mesh.cloud);
00807
00808 if(clean_mesh_while_coloring)
00809 {
00810 pcl::PolygonMesh cleaned_mesh;
00811 pcl::surface::SimplificationRemoveUnusedVertices sruv;
00812 sruv.simplify(mesh, cleaned_mesh);
00813 mesh = cleaned_mesh;
00814 }
00815
00816
00817 }
00818
00819 using namespace vcg;
00820
00821 class MyVertex; class MyEdge; class MyFace;
00822 struct MyUsedTypes : public vcg::UsedTypes<vcg::Use<MyVertex> ::AsVertexType,
00823 vcg::Use<MyEdge> ::AsEdgeType,
00824 vcg::Use<MyFace> ::AsFaceType>{};
00825 class MyVertex : public vcg::Vertex< MyUsedTypes, vcg::vertex::Coord3f, vcg::vertex::Normal3f, vcg::vertex::BitFlags >{};
00826 class MyFace : public vcg::Face< MyUsedTypes, vcg::face::FFAdj, vcg::face::VertexRef, vcg::face::Normal3f, vcg::face::BitFlags > {};
00827 class MyEdge : public vcg::Edge< MyUsedTypes> {};
00828 class MyMesh : public vcg::tri::TriMesh< std::vector<MyVertex>, std::vector<MyFace> , std::vector<MyEdge> > {};
00829
00830
00831 void SurfaceSmootherNode::orientNormalsInMesh(pcl::PolygonMesh& mesh)
00832 {
00833
00834 MyMesh m;
00835
00836
00837 m.Clear();
00838
00839
00840 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1 (new pcl::PointCloud<pcl::PointXYZ> ());
00841 pcl::fromPCLPointCloud2(mesh.cloud,*cloud1);
00842
00843
00844 int vertCount = cloud1->width*cloud1->height;
00845 vcg::tri::Allocator<MyMesh>::AddVertices(m, vertCount);
00846 for(unsigned int i=0;i<vertCount;++i)
00847 m.vert[i].P()=vcg::Point3f(cloud1->points[i].x,cloud1->points[i].y,cloud1->points[i].z);
00848
00849
00850 int triCount = mesh.polygons.size();
00851 if(triCount==1)
00852 {
00853 if(mesh.polygons[0].vertices[0]==0 && mesh.polygons[0].vertices[1]==0 && mesh.polygons[0].vertices[2]==0)
00854 triCount=0;
00855 }
00856 vcg::tri::Allocator<MyMesh>::AddFaces(m, triCount);
00857 for(unsigned int i=0;i<triCount;++i)
00858 {
00859 m.face[i].V(0)=&m.vert[mesh.polygons[i].vertices[0]];
00860 m.face[i].V(1)=&m.vert[mesh.polygons[i].vertices[1]];
00861 m.face[i].V(2)=&m.vert[mesh.polygons[i].vertices[2]];
00862 }
00863
00864 vcg::tri::UpdateBounding<MyMesh>::Box(m);
00865 vcg::tri::UpdateNormal<MyMesh>::PerFace(m);
00866 vcg::tri::UpdateNormal<MyMesh>::PerVertexNormalizedPerFace(m);
00867 printf("Input mesh vn:%i fn:%i\n",m.VN(),m.FN());
00868
00869
00870 vcg::face::FFAdj<MyMesh>();
00871 vcg::tri::UpdateTopology<MyMesh>::FaceFace(m);
00872 bool oriented, orientable;
00873 if ( vcg::tri::Clean<MyMesh>::CountNonManifoldEdgeFF(m)>0 ) {
00874 std::cout << "Mesh has some not 2-manifold faces, Orientability requires manifoldness" << std::endl;
00875 return;
00876 }
00877
00878 vcg::tri::Clean<MyMesh>::OrientCoherentlyMesh(m, oriented, orientable);
00879
00880
00881
00882
00883 vcg::tri::UpdateNormal<MyMesh>::PerVertexNormalizedPerFace(m);
00884 vcg::tri::UpdateNormal<MyMesh>::PerVertexFromCurrentFaceNormal(m);
00885
00886
00887 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
00888 cloud->is_dense = false;
00889 cloud->width = vertCount;
00890 cloud->height = 1;
00891 cloud->points.resize (vertCount);
00892
00893 for(int i=0; i<vertCount; i++)
00894 {
00895 cloud->points[i].x = m.vert[i].P()[0];
00896 cloud->points[i].y = m.vert[i].P()[1];
00897 cloud->points[i].z = m.vert[i].P()[2];
00898 }
00899 pcl::toPCLPointCloud2(*cloud,mesh.cloud);
00900 std::vector<pcl::Vertices> polygons;
00901
00902 for(int i=0; i<triCount; i++)
00903 {
00904 pcl::Vertices vertices;
00905 vertices.vertices.push_back(m.face[i].V(0)-&*m.vert.begin());
00906 vertices.vertices.push_back(m.face[i].V(1)-&*m.vert.begin());
00907 vertices.vertices.push_back(m.face[i].V(2)-&*m.vert.begin());
00908 polygons.push_back(vertices);
00909 }
00910 mesh.polygons = polygons;
00911 }
00912
00913
00914 int main(int argc, char** argv)
00915 {
00916 ros::init(argc, argv, "SurfaceSmootherNode");
00917 SurfaceSmootherNode sr_node;
00918
00919 ros::Rate r(10);
00920 while (ros::ok())
00921 {
00922 ros::spinOnce();
00923 r.sleep();
00924 }
00925
00926 std::cout << " Shutting down SurfaceSmootherNode " << std::endl;
00927
00928 return (0);
00929 }