原始点云
添加噪声后点云std=0.01
话不多说,直接上程序:
#include <pcl/PCLPointCloud2.h> #include <pcl/point_types.h> #include <pcl/io/pcd_io.h> #include <pcl/console/print.h> #include <pcl/console/parse.h> #include <pcl/console/time.h>
#include <pcl/filters/boost.h>using namespace pcl; using namespace pcl::io; using namespace pcl::console; double default_standard_deviation = 0.01; void printHelp (int, char **argv) { print_error ("Syntax is: %s input.pcd output.pcd <options>\n", argv[0]); print_info (" where options are:\n"); print_info (" -sd X = the standard deviation for the normal distribution (default: "); print_value ("%f", default_standard_deviation); print_info (")\n"); } bool loadCloud (const std::string &filename, pcl::PCLPointCloud2 &cloud) { TicToc tt; print_highlight ("Loading "); print_value ("%s ", filename.c_str ()); tt.tic (); if (loadPCDFile (filename, cloud) < 0) return (false); print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms: "); print_value ("%d", cloud.width * cloud.height); print_info (" points]\n"); print_info ("Available dimensions: "); print_value ("%s\n", pcl::getFieldsList (cloud).c_str ()); return (true); } void compute (const pcl::PCLPointCloud2::ConstPtr &input, pcl::PCLPointCloud2 &output, double standard_deviation) { TicToc tt; tt.tic (); print_highlight ("Adding Gaussian noise with mean 0.0 and standard deviation %f\n", standard_deviation); PointCloud<PointXYZ>::Ptr xyz_cloud (new pcl::PointCloud<PointXYZ> ()); fromPCLPointCloud2 (*input, *xyz_cloud); PointCloud<PointXYZ>::Ptr xyz_cloud_filtered (new PointCloud<PointXYZ> ()); xyz_cloud_filtered->points.resize (xyz_cloud->points.size ()); xyz_cloud_filtered->header = xyz_cloud->header; xyz_cloud_filtered->width = xyz_cloud->width; xyz_cloud_filtered->height = xyz_cloud->height; boost::mt19937 rng; rng.seed (static_cast<unsigned int> (time (0))); boost::normal_distribution<> nd (0, standard_deviation); boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > var_nor (rng, nd); for (size_t point_i = 0; point_i < xyz_cloud->points.size (); ++point_i) { xyz_cloud_filtered->points[point_i].x = xyz_cloud->points[point_i].x + static_cast<float> (var_nor ()); xyz_cloud_filtered->points[point_i].y = xyz_cloud->points[point_i].y + static_cast<float> (var_nor ()); xyz_cloud_filtered->points[point_i].z = xyz_cloud->points[point_i].z + static_cast<float> (var_nor ()); } pcl::PCLPointCloud2 input_xyz_filtered; toPCLPointCloud2 (*xyz_cloud_filtered, input_xyz_filtered); concatenateFields (*input, input_xyz_filtered, output); print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms: "); print_value ("%d", output.width * output.height); print_info (" points]\n"); } void saveCloud (const std::string &filename, const pcl::PCLPointCloud2 &output) { TicToc tt; tt.tic (); print_highlight ("Saving "); print_value ("%s ", filename.c_str ()); pcl::io::savePCDFile (filename, output); print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms: "); print_value ("%d", output.width * output.height); print_info (" points]\n"); } /* ---[ */ int main (int argc, char** argv) { print_info ("Add Gaussian noise to a point cloud. For more information, use: %s -h\n", argv[0]); if (argc < 3) { printHelp (argc, argv); return (-1); } // Parse the command line arguments for .pcd files std::vector<int> p_file_indices; p_file_indices = parse_file_extension_argument (argc, argv, ".pcd"); if (p_file_indices.size () != 2) { print_error ("Need one input PCD file and one output PCD file to continue.\n"); return (-1); } // Command line parsing double standard_deviation = default_standard_deviation; parse_argument (argc, argv, "-sd", standard_deviation); // Load the first file pcl::PCLPointCloud2::Ptr cloud (new pcl::PCLPointCloud2); if (!loadCloud (argv[p_file_indices[0]], *cloud)) return (-1); // Add the noise pcl::PCLPointCloud2 output; compute (cloud, output, standard_deviation); // Save into the second file saveCloud (argv[p_file_indices[1]], output); }
---------------------
作者:com1098247427
来源:CSDN
原文:https://blog.csdn.net/com1098247427/article/details/120660589
版权声明:本文为作者原创文章,转载请附上博文链接!
内容解析By:CSDN,CNBLOG博客文章一键转载插件