/* * @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 #include /** * @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::Ptr front_barrier, pcl::PointCloud::Ptr back_barrier, Eigen::Vector3d &front_direction, Eigen::Vector3d &back_direction, Eigen::Vector3d &y_direction) { pcl::PointCloud::Ptr direction_cloud = pcl::PointCloud::Ptr(new pcl::PointCloud); Eigen::Vector3d point_front, point_back; // 根据点云拟合直线 //创建一个模型参数对象，用于记录拟合结果 pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients); //inliers通过点云序号来记录模型内点 pcl::PointIndices::Ptr inliers(new pcl::PointIndices); //创建一个分割对象 pcl::SACSegmentation 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::Ptr t_front, t_back; t_front = pcl::PointCloud::Ptr(new pcl::PointCloud); t_back = pcl::PointCloud::Ptr(new pcl::PointCloud); 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; }