雷达点云位姿矫正2 计算雷达与地面倾角 在转换雷达坐标时加上或减去相应的角度来抵消偏移角。

#include "pclAlter.h"
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/conditional_removal.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_line.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/visualization/cloud_viewer.h>
#include <vector>
#include <cmath>
/// <summary>
/// 读取并转换点云文件
/// </summary>
/// <param name="inputfilename"></param>
/// <param name="outputfilename"></param>
/// <returns></returns>
int main() 
{

    typedef struct TXT_Point_XYZ {

        double x;
        double y;
        double z;

    }TOPOINT_XYZ;

    int num_txt;
    FILE* fp_txt;
    TXT_Point_XYZ txt_points;
    std::vector<TXT_Point_XYZ> my_TxtPoints;
    fp_txt = fopen("C:/Users/ShangAN/Desktop/Da/PointCloud20240408164357.txt", "r");

    if (fp_txt)
    {
        while (fscanf(fp_txt, "%lf %lf %lf", &txt_points.x, &txt_points.y, &txt_points.z) != EOF)
        {//将点存入容器尾部
            my_TxtPoints.push_back(txt_points);
        }
    }
    else
        cout << "读取txt文件失败" << endl;
    num_txt = my_TxtPoints.size();
    //写入点云数据
    pcl::PointCloud<pcl::PointXYZ> ::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    cloud->width = num_txt;
    cloud->height = 1;
    cloud->is_dense = false;
    cloud->points.resize(cloud->width * cloud->height);
    for (int i = 0; i < cloud->points.size(); ++i)
    {
        cloud->points[i].x = my_TxtPoints[i].x;
        cloud->points[i].y = my_TxtPoints[i].y;
        cloud->points[i].z = my_TxtPoints[i].z;
    }
   // pcl::io::savePCDFileASCII(outputfilename, *cloud);
  //  cout << "从txt读取" << cloud->points.size() << "点写入pcd" << endl;
    

/// <summary>
/// 条件滤波得到地面点云
/// </summary>
/// <param name="Cloud"></param>
/// <returns></returns>

    pcl::ConditionalRemoval<pcl::PointXYZ> ConRemal;
    pcl::ConditionAnd<pcl::PointXYZ>::Ptr condition_and(new pcl::ConditionAnd<pcl::PointXYZ>());
    pcl::PointCloud<pcl::PointXYZ>::Ptr Cloud_Filtered(new pcl::PointCloud<pcl::PointXYZ>);

    condition_and->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr(new pcl::FieldComparison<pcl::PointXYZ>("z", pcl::ComparisonOps::GE, 5600.0)));
    condition_and->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr(new pcl::FieldComparison<pcl::PointXYZ>("z", pcl::ComparisonOps::LT, 6000.0)));
    condition_and->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr(new pcl::FieldComparison<pcl::PointXYZ>("y", pcl::ComparisonOps::GE, 2500)));
    condition_and->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr(new pcl::FieldComparison<pcl::PointXYZ>("y", pcl::ComparisonOps::LT, 2510)));
    condition_and->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr(new pcl::FieldComparison<pcl::PointXYZ>("x", pcl::ComparisonOps::GE, 0)));
    condition_and->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr(new pcl::FieldComparison<pcl::PointXYZ>("x", pcl::ComparisonOps::LT, 6000)));

    ConRemal.setInputCloud(cloud);
    ConRemal.setKeepOrganized(false);
    ConRemal.setCondition(condition_and);
    ConRemal.filter(*Cloud_Filtered);

/// <summary>
/// 拟合直线求斜率以及求出偏移角度
/// </summary>
/// <param name="Cloud"></param>
/// <returns></returns>

    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
    pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
    pcl::SACSegmentation<pcl::PointXYZ> seg;
    seg.setOptimizeCoefficients(true);
    seg.setModelType(pcl::SACMODEL_LINE);
    seg.setMethodType(pcl::SAC_RANSAC);
    seg.setDistanceThreshold(5);
    seg.setMaxIterations(500);
    seg.setInputCloud(Cloud_Filtered);
    seg.segment(*inliers, *coefficients);

    // 计算直线斜率（倾斜角的正切值，直线在 XZ 平面内）
    double slope = std::abs(coefficients->values[5] / coefficients->values[3]);

    cout << "拟合直线的模型系数为：" << endl;
    cout << "a:" << coefficients->values[0] << endl;
    cout << "b:" << coefficients->values[1] << endl;
    cout << "c:" << coefficients->values[2] << endl;
    cout << "d:" << coefficients->values[3] << endl;
    cout << "e:" << coefficients->values[4] << endl;
    cout << "f:" << coefficients->values[5] << endl;

    //直线提取
    pcl::PointCloud<pcl::PointXYZ>::Ptr line(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::ExtractIndices<pcl::PointXYZ> extract;  //创建点云提取对象
    extract.setInputCloud(Cloud_Filtered);    //设置输入点云
    extract.setIndices(inliers);     //设置分割后的内点为需要提取的点集
    extract.setNegative(false);      //false提取内点, true提取外点
    extract.filter(*line);        //提取输出存储到c_plane2

    
    // 使用atan函数计算弧度
    double radians = atan(slope);
    // 将弧度转换为度数，注意需要包含cmath库中的M_PI常量
    double degrees = radians * (180.0 / M_PI);
     
    cout << slope << "弧度：" << radians<<" degrees:"<<degrees << endl;
}