对点云进行变换(旋转+平移),并添加高斯噪声

主要步骤

1.读取点云数据
2.设置变换矩阵
3.添加高斯噪声

附上代码

#include <iostream>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/PCLPointCloud2.h>
#include <string>
#include <opencv2/opencv.hpp>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/io/io.h>
#include <pcl/common/transforms.h>
#include <boost/random.hpp>

#include <pcl/console/print.h>
#include <pcl/console/parse.h>
#include <pcl/console/time.h>
//#include "boost.h"

using namespace std;

int main(int argc, char** argv)
{
    if(argc != 3)
    {
        cerr<<"参数数量不对!"<<endl;
        exit(1);
    }

    string input_filename = argv[1];
    string output_filename = argv[2];
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

    std::string format = input_filename.substr(input_filename.length()-4, 4);
    //std::cout<<"pointcloud format:"<<format<<std::endl;
    if(format == ".ply")
    {
        if(pcl::io::loadPLYFile(input_filename, *cloud)==-1)
        {
            PCL_ERROR("error！ \n");
            exit(1);
        }
    }
    else if(format == ".pcd")
    {
        if(pcl::io::loadPCDFile(input_filename, *cloud)==-1)
        {
            PCL_ERROR("error！ \n");
            exit(1);
        }
    }
    else
        cout<<"点云格式不对!"<<endl;

    //设置变换矩阵
    Eigen::Matrix4f transform_1 = Eigen::Matrix4f::Identity();
    float theta = M_PI/4; // The angle of rotation in radians
    transform_1 (0,0) = std::cos (theta);
    transform_1 (0,1) = -sin(theta);
    transform_1 (1,0) = sin (theta);
    transform_1 (1,1) = std::cos (theta);

    // Define a translation
    transform_1 (0,3) = 5.5;
    transform_1 (1,3) = -6.5;
    transform_1 (2,3) = 0;

    // Print the transformation
    printf ("Method #1: using a Matrix4f\n");
    std::cout << transform_1 << std::endl;

    // Executing the transformation
    pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
    // You can either apply transform_1 or transform_2; they are the same
    pcl::transformPointCloud (*cloud, *transformed_cloud, transform_1);
    //*transformed_cloud += *cloud;

    //添加高斯噪声
    boost::mt19937 rng;
    rng.seed(static_cast<unsigned int>(time(0)));
    boost::normal_distribution<> nd(0, 0.5);
    boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > var_nor(rng, nd);//产生随机/数
    //添加噪声
    for (size_t point_i = 0; point_i < cloud->points.size(); point_i += 1)
    {
        transformed_cloud->points[point_i].x = transformed_cloud->points[point_i].x + static_cast<float> (var_nor());
        transformed_cloud->points[point_i].y = transformed_cloud->points[point_i].y + static_cast<float> (var_nor());
        transformed_cloud->points[point_i].z = transformed_cloud->points[point_i].z + static_cast<float> (var_nor());
    }

    pcl::io::savePCDFile(output_filename, *transformed_cloud);

    pcl::visualization::PCLVisualizer viewer ("Matrix transformation example");

    // Define R,G,B colors for the point cloud
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_cloud_color_handler (cloud, 255, 255, 255);
    // We add the point cloud to the viewer and pass the color handler
    viewer.addPointCloud (cloud, source_cloud_color_handler, "original_cloud");

    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> transformed_cloud_color_handler (transformed_cloud, 230, 20, 20); // Red
    viewer.addPointCloud (transformed_cloud, transformed_cloud_color_handler, "transformed_cloud");

    viewer.addCoordinateSystem (1.0, "cloud", 0);
    viewer.setBackgroundColor(0.05, 0.05, 0.05, 0); // Setting background to a dark grey
    viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "original_cloud");
    viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "transformed_cloud");
    //viewer.setPosition(800, 400); // Setting visualiser window position

    while (!viewer.wasStopped ()) { // Display the visualiser until 'q' key is pressed
        viewer.spinOnce ();
    }

    return 0;
}

CMakeLists.txt

cmake_minimum_required(VERSION 2.8)

project(fine_tuning)

find_package(PCL 1.7 REQUIRED)

list(REMOVE_ITEM PCL_LIBRARIES "vtkproj4")
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})

add_executable (main main.cpp)
target_link_libraries (main ${PCL_LIBRARIES})

效果展示