标签：main scan 标定 2D 拼接 pcl include 激光雷达
0.简介
为了保证激光雷达的360°环境覆盖，我们常常需要用到多传感器的拼接，如果我们单纯的取读取激光雷达的信息会出现如下图的情况，两个激光雷达会发生重叠，这就需要我们去对激光雷达进行标定。
<arg name="device_ip1" default="192.168.1.200" />
<arg name="device_ip2" default="192.168.1.200" />
<arg name="msop_port1" default="2368" />
<arg name="difop_port1" default="2369" />
<arg name="msop_port2" default="2370" />
<arg name="difop_port2" default="2371" />
<arg name="return_mode" default="1" />
<arg name="time_synchronization" default="true" />
1. ICP和NDT
NDT：相当于是栅格化的ICP
优点：
-
栅格化可以去除噪点的影响
-
NDT算法便于用GPU加速
缺点；
-
对于结构化点云，NDT对一块栅格高斯分布的假设不成立，效果不好，反而点线ICP(2D)或点面ICP效果更好一些。
-
对于非结构大规模点云，NDT速度快一些，初值鲁棒性取决于栅格大小，越大，精度越差，但对初值鲁棒性好一些，反之，对初值更依赖，类似于ICP对应的min_distance最小匹配距离作用。
-
如果想让点云匹配对初值不那么敏感，可以考虑CSM+ICP的方法，CSM确定一个初始范围，再通过ICP精确化。
两种方法的详细数学推导部分可以参照这篇文章，一般我们在PCL库中即可直接使用
2. 多激光雷达标定
这里推荐一个非常好的博主AdamShan，其文章对于多激光标定部分写了非常详细的内容。同时也有在Github上存在有多激光雷达标定以及激光雷达与摄像头的标定程序，这里就不贴代码了（写这一段是为了方便后续自我回顾以及查找）。而在室内机器人中同样会面临使用多传感器标定的问题（多机器人编队中地图与地图之间的外参匹配也会有这样问题），这里给出基于2D雷达的匹配方案。
multi_lidar_calibration.h
#include <iostream>
#include <vector>
#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/Transform.h>
#include <geometry_msgs/TransformStamped.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <tf2/utils.h>
#include <tf2_ros/transform_listener.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/io/ply_io.h>
#include <pcl/io/vtk_lib_io.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/visualization/pcl_visualizer.h>
class MultiLidarCalibration
{
public:
MultiLidarCalibration(ros::NodeHandle &n);
~MultiLidarCalibration();
// 函数处理
void Run();
private:
// 订阅的激光话题
std::string source_lidar_topic_str_;
std::string target_lidar_topic_str_;
// 激光雷达坐标系
std::string source_lidar_frame_str_;
std::string target_lidar_frame_str_;
// icp匹配得分
float icp_score_;
// 在base_link坐标系下main_laser_link的坐标
float main_to_base_transform_x_;
float main_to_base_transform_y_;
float main_to_base_transform_row_;
float main_to_base_transform_yaw_;
// 第一次运行时进行矩阵赋值
bool is_first_run_;
// 两个激光间的transfrom，通过tf2获得
Eigen::Matrix4f transform_martix_;
// 主激光到base_link的TF
Eigen::Matrix4f front_to_base_link_;
ros::NodeHandle nh_;
// 纠正激光输出，类型pointcloud2
ros::Publisher final_point_cloud_pub_;
// 进行激光时间同步和数据缓存
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::LaserScan, sensor_msgs::LaserScan> SyncPolicyT;
message_filters::Subscriber<sensor_msgs::LaserScan> *scan_front_subscriber_, *scan_back_subscriber_;
message_filters::Synchronizer<SyncPolicyT> *scan_synchronizer_;
// pcl格式的激光数据
pcl::PointCloud<pcl::PointXYZ>::Ptr main_scan_pointcloud_;
// 标定雷达数据
pcl::PointCloud<pcl::PointXYZ>::Ptr sub_scan_pointcloud_;
// 通过tf2转换后的待标定的激光数据
pcl::PointCloud<pcl::PointXYZ>::Ptr sub_scan_pointcloud_init_transformed_;
/**
* @brief icp
* @param final_registration_scan_ 通过icp处理把待标定数据，转换到目标坐标系下的激光点云数据
*/
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp_;
pcl::PointCloud<pcl::PointXYZ>::Ptr final_registration_scan_;
// 两个激光坐标系间初始坐标变换
void GetFrontLasertoBackLaserTf();
// 订阅main雷达和sub雷达两个激光数据
void ScanCallBack(const sensor_msgs::LaserScan::ConstPtr &in_main_scan_msg, const sensor_msgs::LaserScan::ConstPtr &in_sub_scan_msg);
// 极坐标转换到笛卡尔坐标，sensor_msgs::LaserScan to pcl::PointCloud<pcl::PointXYZ> (方法很多不限于这一种)
pcl::PointCloud<pcl::PointXYZ> ConvertScantoPointCloud(const sensor_msgs::LaserScan::ConstPtr &scan_msg);
// 标定后的激光点云进行发布，发布的数据格式是sensor_msgs::PointCloud2
void PublishCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr &in_cloud_to_publish_ptr);
// 对main雷达和sub雷达的激光点云进行icp匹配
bool ScanRegistration();
// 发布标定结果
void PrintResult();
// 可视化
void View();
};
…详情请参照古月居
标签：main,scan,标定,2D,拼接,pcl,include,激光雷达 来源： https://blog.csdn.net/lovely_yoshino/article/details/120726065
本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享； 2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关； 3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关； 4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除； 5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。