激光雷达点云地平面校准 地面分割

激光雷达点云地平面校准 地面分割 https://blog.csdn.net/u014679795/article/details/82189901

#include 

using namespace std;
#include  //PCL的PCD格式文件的输入输出头文件

#include  //PCL对各种格式的点的支持头文件

#include //点云查看窗口头文件
#include 
#include 
#include
#include 
#include
#include 
#include
#include 
using namespace pcl;

bool estimateGroundPlane（pcl::PointCloud::Ptr &in_cloud pcl::PointCloud::Ptr &out_cloud
                         const float in_distance_thre）;

typedef pcl::PointXYZRGBA PointT;
typedef pcl::PointCloud PointCloudT;

pcl::PointCloud::Ptr clicked_points_3d（new pcl::PointCloud）;
int num = 0;
pcl::PointCloud::Ptr cloud（new pcl::PointCloud）; // 创建点云（指针）
pcl::PointCloud::Ptr cloud_final（new pcl::PointCloud）; // 创建点云（指针）
boost::shared_ptr viewer（new pcl::visualization::PCLVisualizer（“viewer“））;

// Mutex: //
boost::mutex cloud_mutex;

struct callback_args{
    // structure used to pass arguments to the callback function
    PointCloudT::Ptr clicked_points_3d;
    pcl::visualization::PCLVisualizer::Ptr viewerPtr;
};

Eigen::Matrix4f CreateRotateMatrix（Eigen::Vector3f beforeEigen::Vector3f after）
{
    before.normalize（）;
    after.normalize（）;

    float angle = acos（before.dot（after））;
    Eigen::Vector3f p_rotate =before.cross（after）;
    p_rotate.normalize（）;

    Eigen::Matrix4f rotationMatrix = Eigen::Matrix4f::Identity（）;
    rotationMatrix（0 0） = cos（angle） + p_rotate[0] * p_rotate[0] * （1 - cos（angle））;
    rotationMatrix（0 1） = p_rotate[0] * p_rotate[1] * （1 - cos（angle） - p_rotate[2] * sin（angle））;//这里跟公式比多了一个括号，但是看实验结果它是对的。
    rotationMatrix（0 2） = p_rotate[1] * sin（angle） + p_rotate[0] * p_rotate[2] * （1 - cos（angle））;


    rotationMatrix（1 0） = p_rotate[2] * sin（angle） + p_rotate[0] * p_rotate[1] * （1 - cos（angle））;
    rotationMatrix（1 1） = cos（angle） + p_rotate[1] * p_rotate[1] * （1 - cos（angle））;
    rotationMatrix（1 2） = -p_rotate[0] * sin（angle） + p_rotate[1] * p_rotate[2] * （1 - cos（angle））;


    rotationMatrix（2 0） = -p_rotate[1] * sin（angle） +p_rotate[0] * p_rotate[2] * （1 - cos（angle））;
    rotationMatrix（2 1） = p_rotate[0] * sin（angle） + p_rotate[1] * p_rotate[2] * （1 - cos（angle））;
    rotationMatrix（2 2） = cos（angle） + p_rotate[2] * p_rotate[2] * （1 - cos（angle））;

    return rotationMatrix;
}

void AreaPick_callback（const pcl::visualization::AreaPickingEvent& event void* args）
{
    std::vector< int > indices;
    if （event.getPointsIndices（indices）==-1）
        return;

    for （int i = 0; i < indices.size（）; ++i）
    {
        clicked_points_3d->points.push_back（cloud->points.at（indices[i]））;
    }
    pcl::PointCloud