puai_wj_2021/tests/lidar_calib_tools.cpp

/*
 * @Description: lidar calib tools
 * @Author: yct
 * @Date: 2021-09-02 11:02:00
 * @LastEditTime: 2021-10-12 14:01:29
 * @LastEditors: yct
 */

#include "../tool/point_tool.h"
#include <Eigen/Core>
#include <Eigen/Geometry>

/**
 * @description: used to find vlp16 intrinsic
 * @param  {*}
 * @return {*}
 * @param {Quaterniond} ground_quat  quaternion to align lidar coords with ground plane
 * @param {double} angle_z_axis  rotation angle along z axis to align lidar coords with PLC coords
 */
void find_intrinsic(Eigen::Vector3d ground_norm, Eigen::Vector3d y_axis)
{
    Eigen::Quaterniond ground_quat = Eigen::Quaterniond::FromTwoVectors(ground_norm, Eigen::Vector3d::UnitZ());
    Eigen::Quaterniond xy_plane_quat = Eigen::Quaterniond::FromTwoVectors(y_axis, Eigen::Vector3d::UnitY());
    // std::cout << "y euler: " << xy_plane_quat.toRotationMatrix().eulerAngles(2, 1, 0).transpose() << std::endl;
    Eigen::Matrix3d combined_rotation = ground_quat.toRotationMatrix() * xy_plane_quat.toRotationMatrix();
    Eigen::Vector3d euler_angles = combined_rotation.eulerAngles(2, 1, 0);
    for (size_t i = 0; i < euler_angles.rows(); i++)
    {
        if(fabs(euler_angles[i]) > M_PI_2)
        {
            if(euler_angles[i] > 0)
            {
                euler_angles[i] = euler_angles[i] - M_PI;
            }else{
                euler_angles[i] = M_PI + euler_angles[i];
            }
        }
    }
    if(euler_angles[0]<0)
    {
        euler_angles[0] = euler_angles[0] + M_PI;
    }
    std::cout << "lidar intrinsic y p r: " << euler_angles.transpose()*180.0/M_PI << std::endl;
}

/**
 * @description: fit ground norm from two poles, then find y axis direction from front to back
 * @return {*}
 * @param {Ptr} front_barrier
 * @param {Ptr} back_barrier
 */
void find_rotation_from_points(pcl::PointCloud<pcl::PointXYZ>::Ptr front_barrier, pcl::PointCloud<pcl::PointXYZ>::Ptr back_barrier,
                                Eigen::Vector3d &front_direction, Eigen::Vector3d &back_direction, Eigen::Vector3d &y_direction)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr direction_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);

    Eigen::Vector3d point_front, point_back;
    // 根据点云拟合直线
    //创建一个模型参数对象，用于记录拟合结果
    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
	//inliers通过点云序号来记录模型内点
	pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
	//创建一个分割对象
	pcl::SACSegmentation<pcl::PointXYZ> seg;
	//Optional,设置结果直线展示的点是分割掉的点还是分割剩下的点
	seg.setOptimizeCoefficients(true);//flase 展示的是分割剩下的点
	//Mandatory-设置目标几何形状
	seg.setModelType(pcl::SACMODEL_LINE);
	//分割方法：随机采样法
	seg.setMethodType(pcl::SAC_RANSAC);
	//设置误差容忍范围，也就是阈值
	seg.setDistanceThreshold(0.01);
	//输入点云
	seg.setInputCloud(front_barrier);
	//分割点云
	seg.segment(*inliers, *coefficients);

    if(coefficients->values.size() < 6)
    {
        std::cout << "fatal, front line coeff size not enough." << std::endl;
        return;
    }
    else
    {
        point_front << coefficients->values[0], coefficients->values[1], coefficients->values[2];
        front_direction << coefficients->values[3], coefficients->values[4], coefficients->values[5];
        std::cout << "front direction: " << front_direction.transpose() << std::endl;
    }

    // 保存前杆方向点云，调试用
    for (size_t i = 0; i < 200; i++)
    {
        Eigen::Vector3d t_point = point_front + front_direction * 0.01 * i;
        direction_cloud->push_back(pcl::PointXYZ(t_point.x(), t_point.y(), t_point.z()));
    }

    seg.setInputCloud(back_barrier);
    seg.segment(*inliers, *coefficients);
    if(coefficients->values.size() < 6)
    {
        std::cout << "fatal, back line coeff size not enough." << std::endl;
        return;
    }
    else
    {
        point_back << coefficients->values[0], coefficients->values[1], coefficients->values[2];
        back_direction << coefficients->values[3], coefficients->values[4], coefficients->values[5];
        std::cout << "back direction: " << back_direction.transpose() << std::endl;
    }

    // 保存后杆方向点云，调试用
    for (size_t i = 0; i < 200; i++)
    {
        Eigen::Vector3d t_point = point_back + back_direction * 0.01 * i;
        direction_cloud->push_back(pcl::PointXYZ(t_point.x(), t_point.y(), t_point.z()));
    }

    std::cout << "point front to back : " << point_front.transpose() << " -----> " << point_back.transpose() << std::endl;

    // 注意y方向需要清除z分量
    y_direction = point_front - point_back;
    y_direction.z() = 0.0;
    std::cout << "y direction: " << y_direction.transpose() << std::endl;

    // 保存y方向点云，调试用
    for (size_t i = 0; i < 200; i++)
    {
        Eigen::Vector3d t_point = point_back + y_direction * 0.01 * i;
        direction_cloud->push_back(pcl::PointXYZ(t_point.x(), t_point.y(), t_point.z()));
    }
    save_cloud_txt(direction_cloud, "./direction_cloud.txt");
}

int main()
{
    std::cout << "------------------------ start calib lidar 4 ----------------------" << std::endl;
    pcl::PointCloud<pcl::PointXYZ>::Ptr t_front, t_back;
    t_front = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
    t_back = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
    if (!read_pointcloud("/home/youchen/extra_space/chutian/fence_wj/front_pole.txt", t_front))
    {
        std::cout << "read front failed." << std::endl;
        return -1;
    }
    if(!read_pointcloud("/home/youchen/extra_space/chutian/fence_wj/back_pole.txt", t_back))
    {
        std::cout << "read back failed." << std::endl;
        return -1;
    }

    Eigen::Vector3d front_direction, back_direction, y_direction;


    // front direction: 0.0191082  0.002411  0.999815
    // back direction: 0.0190939 0.0143106  0.999715
    // -0.000887596 00-0.0100556 00000.999949 0000.0650278
    find_rotation_from_points(t_front, t_back, front_direction, back_direction, y_direction);
    // 经过测试，地面法向量不适宜使用杆子获取，还是使用地面环点云拟合获得
    // find_intrinsic((front_direction + back_direction) / 2.0, y_direction);
    find_intrinsic(Eigen::Vector3d(-0.000887596, 0.0100556, 0.999949), y_direction);

    return 0;

    std::cout << "------------------------ start calib lidar 1 ----------------------" << std::endl;
    // cloud 1
    if (!read_pointcloud("/home/youchen/extra_space/chutian/fence_wj/front_pole2.txt", t_front))
    {
        std::cout << "read front failed." << std::endl;
        return -1;
    }
    if(!read_pointcloud("/home/youchen/extra_space/chutian/fence_wj/back_pole2.txt", t_back))
    {
        std::cout << "read back failed." << std::endl;
        return -1;
    }
    find_rotation_from_points(t_front, t_back, front_direction, back_direction, y_direction);

    // -0.00566995 0-0.0233422 0000.999712 000.0949598
    find_intrinsic(Eigen::Vector3d(-0.00566995,-0.0233422,0.999712), y_direction);

    return 0;
}