puai_wj_2021/tests/lidar_calib_tools.cpp

/*
 * @Description: lidar calib tools
 * @Author: yct
 * @Date: 2021-09-02 11:02:00
 * @LastEditTime: 2021-12-14 15:31:13
 * @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");
}

// passthrough and fit plane 
#include "pcl/filters/passthrough.h"
void find_ground(pcl::PointCloud<pcl::PointXYZ>::Ptr ground_cloud, Eigen::Vector4d &params, double avg_z=0.03)
{
    if(ground_cloud->size()<=0)
    {
        std::cout<<"empty ground cloud"<<std::endl;
        return;
    }
    pcl::PointCloud<pcl::PointXYZ>::Ptr t_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
    // 直通滤波
    double min_x = -2.6, max_x = 2.3, min_y = -3.0, max_y = 3.0, max_z = 0.1;
    for (size_t i = 0; i < ground_cloud->size(); i++)
    {
        pcl::PointXYZ pt = ground_cloud->points[i];
        if (pt.x > min_x && pt.x < max_x && pt.y > min_y && pt.y < max_y && pt.z < max_z)
        {
            t_cloud->push_back(pt);
        }
    }
    // 平面拟合
    if(t_cloud->size()<1)
    {
        std::cout<<"ground cloud not enough"<<std::endl;
        return;
    }
    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_PLANE);
    seg.setMethodType(pcl::SAC_RANSAC);
    seg.setDistanceThreshold(avg_z);
    seg.setInputCloud(t_cloud);
    seg.segment(*inliers, *coefficients);

    if(coefficients->values.size()<4)
    {
        std::cout<<"fit plane coeff size not enough"<<std::endl;
        return;
    }
    params << coefficients->values[0], coefficients->values[1], coefficients->values[2], coefficients->values[3];
}

void matrix_trans()
{
    // 921 A left
    Eigen::Matrix3d rot_matrix0;
    rot_matrix0 << 0.3967, -0.9023, -0.1686,
        -0.8619, -0.4293, 0.2698,
        -0.3159, 0.03827, -0.948;
    Eigen::Matrix3d rot_matrix1;
    rot_matrix1 << 0.9993, 0.03618, 0.0,
        -0.03618, 0.9993, 0.0,
        0.0, 0.0, 1.0;

    Eigen::Vector3d euler = (rot_matrix1 * rot_matrix0).eulerAngles(2, 1, 0);
    std::cout << "mat to euler left (z y x): " << euler[0]*180.0/M_PI << ", " << euler[1]*180.0/M_PI << ", " << euler[2]*180.0/M_PI << std::endl;

    // 541 A right
    Eigen::Matrix3d rot_matrix2;
    rot_matrix2 << 0.0812, 0.997, 0.0185,
        0.8787, -0.0628, -0.4731,
        -0.4703, 0.0547, -0.8808;
    Eigen::Matrix3d rot_matrix3;
    rot_matrix3 << 0.97486, 0.2228, 0.0,
        -0.2228, 0.97486, 0.0,
        0.0, 0.0, 1.0;

    euler = (rot_matrix3 * rot_matrix2).eulerAngles(2, 1, 0);
    std::cout << "mat to euler right (z y x): " << euler[0]*180.0/M_PI << ", " << euler[1]*180.0/M_PI << ", " << euler[2]*180.0/M_PI << std::endl;

    // 921----- -2.31 161.58 -64 2.512
    // 541----- 176.45 28.05 71.85
    euler << -2.31, 161.58, -64;
    Eigen::Matrix3d euler2mat = Eigen::AngleAxisd(euler[2]*M_PI/180.0, Eigen::Vector3d::UnitZ()).toRotationMatrix() * 
                                        Eigen::AngleAxisd(euler[1]*M_PI/180.0, Eigen::Vector3d::UnitY()).toRotationMatrix() *
                                        Eigen::AngleAxisd(euler[0]*M_PI/180.0, Eigen::Vector3d::UnitX()).toRotationMatrix();
    std::cout << "921 euler mat: \n" << euler2mat << std::endl;
    // -0.415912  0.892481  0.174631
    // 0.852745  0.449462 -0.266101
    // -0.31598 0.0382411 -0.947995

    euler << 176.45, 28.05, 71.85;
    euler2mat = Eigen::AngleAxisd(euler[2]*M_PI/180.0, Eigen::Vector3d::UnitZ()).toRotationMatrix() * 
                                        Eigen::AngleAxisd(euler[1]*M_PI/180.0, Eigen::Vector3d::UnitY()).toRotationMatrix() *
                                        Eigen::AngleAxisd(euler[0]*M_PI/180.0, Eigen::Vector3d::UnitX()).toRotationMatrix();

    std::cout << "541 euler mat: \n" << euler2mat << std::endl;
    //   0.274916   0.957491 -0.0873633
    // 0.838626   -0.28324  -0.465276
    // -0.470242  0.0546463  -0.880844
}

int main()
{
    matrix_trans();
    return 0;

    // calib process:
    // 1. cc cut and fit ground, get normal and z
    // 2. get front and back pole txt
    // 3. run this calib func to get rpy and z
    std::cout << "------------------------ start calib lidar 0 ----------------------" << std::endl;
    // lidar 0 normal [09:22:54] 	- normal: (-0.000392,-0.011935,0.999929) z -0.07697
    // 90.6538 0.68354 0.0302616

    // lidar 1 normal 0.000297,-0.001893,0.999998 z -0.0686 
    // 91.3879 0.108841 -0.0143847

    // lidar 2 normal [10:43:07] 	- normal: (0.001131,-0.024040,0.999710) z -0.0461051
    // 86.3796  1.37069 -0.15165

    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/measure/puai_wj_2021/setting/calib/2front_pole.txt", t_front))
    {
        std::cout << "read front failed." << std::endl;
        return -1;
    }
    if(!read_pointcloud("/home/youchen/extra_space/chutian/measure/puai_wj_2021/setting/calib/2back_pole.txt", t_back))
    {
        std::cout << "read back failed." << std::endl;
        return -1;
    }

    Eigen::Vector3d front_direction, back_direction, y_direction;
    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.000392,-0.011935,0.999929), y_direction);
    find_intrinsic(Eigen::Vector3d(0.001131,-0.024040,0.999710), 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;
}