前段时间参与一个项目需要进行激光雷达和imu的外参标定，发现目前网上开源的标定包只有浙江大学开源的方案-lidar_IMU_calib、lidar-align等少数几种，其中lidar-align听说精度不高，所以最后选择浙大的开源方案lidar_IMU_calib，在github上查询后发现它目前只支持VLP-16线激光雷达，而我参与的项目用的是RS-32线激光雷达，但根据作者叙述应该很容易扩展，在经过一番挣扎终于成功了，下面是实现的流程。

一、修改方法

最开始感觉无从下手，还从官网上下载了论文翻译并看了一遍，毕竟以前没有接触过标定算法，后来才发现压根没必要，扩展激光雷达并不需要了解该标定算法的原理。

学长之前告诉我可以在issues里面找找其他人的做法，在翻完所有回答后发现了这位大佬的建议：

即修改从https://github.com/ethz-asl/lidar_align.git下载的代码中/include/utils下的dataset_reader.h文件和vlp_common.h文件，修改关于激光雷达类型的相关函数和变量、常量等。

二、修改vlp_common.h文件

打开文件后先看了一遍代码，即定义了一个VelodyneCorrection类，里面包含各种函数和结构体等，因为修改的地方比较多，所以直接一个个上图说明：

这一部分定义了激光雷达类型和构造函数，我参与的项目用的是RS-32 激光雷达，你可以把类型和初始化参数直接修改成你的激光雷达类型，名字随便取，但你要保证后面代码都用这个名字。

接下来是unpack_scan函数，作者进行了函数重载，有两个unpack_scan函数，第一个是将velodyne_msgs::VelodyneScan类型点云转化为TPointCloud（就是xyzit）类型点云，这里velodyne_msgs::VelodyneScan类型点云数据包格式在用户手册可以找到，例如下面是程序原来的vlp-16的数据包格式：

下面是我用的RS-32的数据包格式：

它们有一定的相似性但又有所不同，具体含义可以看用户手册，或者搜“vlp-16 激光雷达数据格式”有一些博客帮助理解。下面修改的代码基本上都要参考用户手册，16线激光雷达有fring这个参数，而32线激光雷达不需要，可以直接改为1。注意，要使用第一个unpack_scan函数，需要把输入点云的数据包类型改为你的激光雷达数据包类型，例如vlp-16是velodyne_msgs::VelodyneScan，我是把这个函数修改完后才发现这个问题的，但是因为时间紧迫，我没有精力再找rs-32这个类型叫啥，而且也不知道这种类型的包怎么录制，问学长说他也没有录过这个类型的包，所以最后我还是使用的第二个unpack_scan函数。

这里放一下第一个函数修改后的代码（传入的数据包格式不知道叫啥就没改），给想要使用这个函数的同学参考一下（当然不一定对，我也没验证过）。

    void unpack_scan(const velodyne_msgs::VelodyneScan::ConstPtr &lidarMsg,TPointCloud &outPointCloud) const{outPointCloud.clear();outPointCloud.header = pcl_conversions::toPCL(lidarMsg->header);//设置点云格式if (m_modelType == ModelType::RS_32){outPointCloud.height = 32;outPointCloud.width = 12 * (int)lidarMsg->packets.size();outPointCloud.is_dense = false; //点云中的数据不都是有限的，包含inf/NaN 值outPointCloud.resize(outPointCloud.height * outPointCloud.width);}int block_counter = 0;double scan_timestamp = lidarMsg->header.stamp.toSec();for (size_t i = 0; i < lidarMsg->packets.size(); ++i) // lidarMsg->packets.size()：packet总数{float azimuth; //方位float azimuth_diff;float last_azimuth_diff = 0;float azimuth_corrected_f;int azimuth_corrected;float x, y, z;const raw_packet_t *raw = (const raw_packet_t *)&lidarMsg->packets[i].data[0]; //将第i个packets的data数据（1204个字节）赋值给自定义数据类型raw_packet_t的对象for (int block = 0; block < BLOCKS_PER_PACKET; block++, block_counter++) //遍历12个block块{// Calculate difference between current and next block's azimuth angle.//计算当前block和下一block的角度差azimuth = (float)(raw->blocks[block].rotation);if (block < (BLOCKS_PER_PACKET - 1)){azimuth_diff = (float)((36000 + raw->blocks[block + 1].rotation - raw->blocks[block].rotation) % 36000);last_azimuth_diff = azimuth_diff;}else{azimuth_diff = last_azimuth_diff;}for (int firing = 0, k = 0; firing < FIRINGS_PER_BLOCK; firing++) //一个block有两个firing(直接改为1个){for (int dsr = 0; dsr < SCANS_PER_FIRING; dsr++, k += RAW_SCAN_SIZE) //每个firing有16个点（改为32）{/** Position Calculation ，2字节距离值*/union two_bytes tmp;//RS-32的距离数据前一个字节是高位，后一个字节是低位，与VLP-16相反 ?tmp.bytes[0] = raw->blocks[block].data[k + 1];tmp.bytes[1] = raw->blocks[block].data[k];/** correct for the laser rotation as a function of timing during the firings **/azimuth_corrected_f = azimuth + (azimuth_diff * (dsr * DSR_TOFFSET) / BLOCK_TDURATION); //该点的角度值//四舍五入并转化为整型2azimuth_corrected = ((int)round(azimuth_corrected_f)) % 36000;/*condition added to avoid calculating points which are notin the interesting defined area (min_angle < area < max_angle)（添加条件以避免计算不在感兴趣的定义区域内的点）*/if ((azimuth_corrected >= m_config.min_angle && azimuth_corrected <= m_config.max_angle && m_config.min_angle < m_config.max_angle) || (m_config.min_angle > m_config.max_angle && (azimuth_corrected <= m_config.max_angle || azimuth_corrected >= m_config.min_angle))){// convert polar coordinates to Euclidean XYZ（将极坐标转换为欧几里得 XYZ）float distance = tmp.uint * DISTANCE_RESOLUTION; //距离值乘以分辨率 float cos_vert_angle = cos_vert_angle_[dsr];float sin_vert_angle = sin_vert_angle_[dsr];float cos_rot_angle = cos_rot_table_[azimuth_corrected];float sin_rot_angle = sin_rot_table_[azimuth_corrected];//将极坐标下的角度和距离信息转化为笛卡尔坐标系下的 x,y,z 坐标x = distance * cos_vert_angle * sin_rot_angle;y = distance * cos_vert_angle * cos_rot_angle;z = distance * sin_vert_angle;/** Use standard ROS coordinate system (right-hand rule) ——使用标准 ROS 坐标系（右手法则）*/float x_coord = y;float y_coord = -x;float z_coord = z;// 反射率float intensity = raw->blocks[block].data[k + 2];double point_timestamp = scan_timestamp + getExactTime(scan_mapping_32[dsr],block_counter ); //激光点的时间戳//定义云点TPoint point;point.timestamp = point_timestamp;//距离满足要求，则将求得的值赋值给云点，否则赋值NANif (pointInRange(distance)){point.x = x_coord;point.y = y_coord;point.z = z_coord;point.intensity = intensity;}else{point.x = NAN;point.y = NAN;point.z = NAN;point.intensity = 0;}//如果为RS_32的激光雷达，赋值给点云if (m_modelType == ModelType::RS_32)outPointCloud.at(block_counter , scan_mapping_32[dsr]) = point;}}}}}}

第二个unpack_scan函数（也就是我实际使用的函数）就比较简单了，它是将PointCloud2格式点云转换为XYZIT格式点云，这一部分当初有点困扰我的是：使用pcl::fromROSMsg()函数将PointCloud2格式点云转换为XYZI格式点云后，点云变为无序的了，所以运行程序一直报错，后来查阅资料修改如下：

    //点云格式转换函数（将PointCloud2格式点云转换为XYZIT格式点云）void unpack_scan(const sensor_msgs::PointCloud2::ConstPtr &lidarMsg,TPointCloud &outPointCloud) const{VPointCloud temp_pc; //XYZI格式点云pcl::fromROSMsg(*lidarMsg, temp_pc); //将PointCloud2格式转换为XYZI格式outPointCloud.clear();outPointCloud.header = pcl_conversions::toPCL(lidarMsg->header);// 时间戳的转换outPointCloud.height = 32;outPointCloud.width = temp_pc.size() / 32;outPointCloud.is_dense = true;outPointCloud.resize(outPointCloud.height * outPointCloud.width);double timebase = lidarMsg->header.stamp.toSec(); //初始时间for (int h = 0; h < outPointCloud.height; h++){for (int w = 0; w < outPointCloud.width; w++){TPoint point; //XYZIT格式云点pcl::PointXYZI& rs_point =  temp_pc[w + h * temp_pc.size() / 32];point.x = rs_point.x;point.y = rs_point.y;point.z = rs_point.z;point.intensity = rs_point.intensity;point.timestamp = timebase + getExactTime(h, w); //计算云点时间戳outPointCloud.at(w, h) = point;}}}

之后是各种函数。例如获取点的相对时间：

    inline double getExactTime(int dsr, int firing) const{return mRS32TimeBlock[firing][dsr];}

参数初始化（参考激光雷达的用户手册）：

  private:void setParameters(ModelType modelType){m_modelType = modelType;m_config.max_range = 200;m_config.min_range = 0.4;m_config.min_angle = 0;m_config.max_angle = 36000;// Set up cached values for sin and cos of all the possible headingsfor (uint16_t rot_index = 0; rot_index < ROTATION_MAX_UNITS; ++rot_index){float rotation = angles::from_degrees(ROTATION_RESOLUTION * rot_index);cos_rot_table_[rot_index] = cosf(rotation);sin_rot_table_[rot_index] = sinf(rotation);}if (modelType == RS_32){FIRINGS_PER_BLOCK = 1;SCANS_PER_FIRING = 32;BLOCK_TDURATION = 55.52f; // [µs]DSR_TOFFSET = 1.44f;       // [µs]FIRING_TOFFSET = 55.52f;   // [µs]PACKET_TIME = (BLOCKS_PER_PACKET * BLOCK_TDURATION);//垂直角度float vert_correction[32] = {-0.179437300397537023,-0.112119951148115732,0.040718531449027709,0.058171823968971004,0.081454516190575361,0.12217304763960307,0.180344871608574075,0.261799387799149436,0.005811946409141117,0,-0.005811946409141117,-0.011641346110802178,0.029094638630745474,0.023265238929084413,0.017453292519943295,0.011641346110802178,-0.436332312998582394,-0.255481295906929963,-0.138055543832751469,-0.0943699526553334,-0.064001223670632065,-0.069813170079773183,-0.0756251164889143,-0.081454516190575361,-0.040718531449027709,-0.046547931150688769,-0.052359877559829887,-0.058171823968971004,-0.017453292519943295,-0.023265238929084413,-0.029094638630745474,-0.034906585039886591};//垂直角度对应的cos和sin值for (int i = 0; i < 32; i++){cos_vert_angle_[i] = std::cos(vert_correction[i]);sin_vert_angle_[i] = std::sin(vert_correction[i]);}scan_mapping_32[0] = 29;scan_mapping_32[1] = 27;scan_mapping_32[2] = 5;scan_mapping_32[3] = 4;scan_mapping_32[4] = 3;scan_mapping_32[5] = 2;scan_mapping_32[6] = 1;scan_mapping_32[7] = 0;scan_mapping_32[8] = 10;scan_mapping_32[9] = 11;scan_mapping_32[10] = 12;scan_mapping_32[11] = 13;scan_mapping_32[12] = 6;scan_mapping_32[13] = 7;scan_mapping_32[14] = 8;scan_mapping_32[15] = 9;scan_mapping_32[16] = 31;scan_mapping_32[17] = 30;scan_mapping_32[18] =28 ;scan_mapping_32[19] = 26;scan_mapping_32[20] = 22;scan_mapping_32[21] = 23;scan_mapping_32[22] = 24;scan_mapping_32[23] = 25;scan_mapping_32[24] = 18;scan_mapping_32[25] = 19;scan_mapping_32[26] = 20;scan_mapping_32[27] = 21;scan_mapping_32[28] = 14;scan_mapping_32[29] = 15;scan_mapping_32[30] = 16;scan_mapping_32[31] = 17;for (unsigned int w = 0; w < 1800; w++){for (unsigned int h = 0; h < 32; h++){mRS32TimeBlock[w][h] = h * 1.44 * 1e-6 + w * 55.52 * 1e-6; /// VLP_16 16*1824，(RS-32 32*1800)}}}}

各种常量、结构体设置（同样参考用户手册）：

  private:static const int RAW_SCAN_SIZE = 3;static const int SCANS_PER_BLOCK = 32;static const int BLOCK_DATA_SIZE = (SCANS_PER_BLOCK * RAW_SCAN_SIZE); //一个block中32个点数据（距离+强度）constexpr static const float ROTATION_RESOLUTION = 0.01f;             //角度分辨率static const uint16_t ROTATION_MAX_UNITS = 36000u;                    //角度最大值36000，除以100，即360constexpr static const float DISTANCE_RESOLUTION = 0.005f;            //距离分辨率/** @todo make this work for both big and little-endian machines  ? */static const uint16_t UPPER_BANK = 0xeeff;static const uint16_t LOWER_BANK = 0xddff;static const int BLOCKS_PER_PACKET = 12; //一个packet含有12个blockstatic const int PACKET_STATUS_SIZE = 2;int FIRINGS_PER_BLOCK;int SCANS_PER_FIRING;float BLOCK_TDURATION;float DSR_TOFFSET;float FIRING_TOFFSET;float PACKET_TIME;float sin_rot_table_[ROTATION_MAX_UNITS];float cos_rot_table_[ROTATION_MAX_UNITS];float cos_vert_angle_[32];float sin_vert_angle_[32];int scan_mapping_16[16];int scan_mapping_32[32];typedef struct raw_block{uint16_t header;               ///< UPPER_BANK or LOWER_BANK（起始标志符）uint16_t rotation;             ///< 0-35999, divide by 100 to get degrees（角度值）uint8_t data[BLOCK_DATA_SIZE]; //32个点的数据} raw_block_t;                   //block数据格式union two_bytes{uint16_t uint;uint8_t bytes[2];};union four_bytes{uint32_t uint32;float_t float32;};typedef struct raw_packet{raw_block_t blocks[BLOCKS_PER_PACKET];uint32_t revolution;uint8_t status[PACKET_STATUS_SIZE];} raw_packet_t; //packet数据格式/** configuration parameters */typedef struct{double max_range; ///< maximum range to publishdouble min_range;int min_angle; ///< minimum angle to publishint max_angle; ///< maximum angle to publish} Config;Config m_config;ModelType m_modelType;double mRS32TimeBlock[1800][32];

三、修改dataset_reader.h文件

这个文件要改的很少，主要把和激光雷达类型相关代码修改成自己定义的类型就可以了，修改了下面这些部分。

这里如果要用第一个unpack_scan函数，需要修改下面代码（还是之前说的那个数据包类型名称的相关修改）:

总结

因为用rosbag录制的包的数据类型是PointCloud2格式，所以我实际上只使用了第二个unpack_scan函数。主要代码修改完后，之后在licalib_gui.launch和calib.sh文件中修改一下imu和激光雷达话题名，录制的rosbag包时间、路径等。

之后运行代码，结果一直初始化失败。找到lidar_IMU_calib/src/core/inertial_initializer.cpp，即初始化程序发现，它与cov(2)值有关，程序设置必须值大于0.25才能初始化成功，而我标定的rosbag包它最开始的值只有零点零几，离0.25差很多，我猜测是因为录制的数据包不太好，因为这个标定程序需要录制的rosbag包有很多条件，例如：需要在平面多的房间里录制、xyz轴方向都需要移动等。因为我们项目rosbag包录制太麻烦了，所以我没有更换rosbag包，而是直接将阈值设置为0.20，然后一直初始化直到它大于0.20（花了一天时间，特别慢）初始化成功，之后按照官网给的步骤逐步运行，最后终于标定成功，结果如下：

大约看了一下，姿态角度大致和实际相同，把参数发给学长后，学长说应该是正确的。大致步骤就是上面这些，在学长的建议下写下这篇博客，第一次写csdn有什么不足欢迎指出，希望能对各位小伙伴有所帮助。

激光雷达和IMU联合标定包 lidar_IMU_calib 基于RS-32的扩展相关推荐

使用二维激光雷达和cartographer_ros实现实时SLAM
在前面已经完成了cartographer_ros的安装和demo的运行了.接下来,就要放到机器人上,实时进行SLAM了. 前一篇内容的链接如下: Cartographer_ros的下载.配置及编译与问 ...
双目相机与IMU联合标定
前言为了后面的视觉激光融合SLAM以及跑通VINS-Fusion,需要标定双目相机和IMU得内参以及它们得外参(变换矩阵). 准备工作双目相机:ZED-m IMU:realsense-t265(仅 ...
ZED2跑ORB-SLAM3+双目相机、IMU联合标定+显卡驱动与cuda/cudnn安装
一.引言同样是项目需求,需要利用视觉惯性导航做一些开发,所以第一步先做些算法的测试–仿真与实物测验,通过仿真的测试结果,最终是决定使用ORB-SLAM3来完成任务,当然了,Vins-fusion作为 ...
相机与IMU联合标定
相机与IMU联合标定 1 imu_utils 标定IMU的内参 1.1 安装环境 1.2 录制IMU数据集 2 kalibr 标定工具 2.1 安装 2.2 校准相机的内外参 2.3 校准相机与IMU ...
双目相机 -- IMU联合标定
声明:一些图片是不该有水印的,CSDN把图片链接的格式改了,暂时还不知道怎么去掉,请见谅!!! 目录 **声明**:一些图片是不该有水印的,CSDN把图片链接的格式改了,暂时还不知道怎么去掉,请见谅! ...
激光雷达和V2X技术
激光雷达和V2X技术
标定（二）----------双目相机与IMU联合标定（工具箱使用方法——Kalibr）
16个相机参数: Overview ethz-asl/kalibr is a toolbox that solves the following calibration problems: Multi ...
rs_D455相机内外参标定+imu联合标定
IMU标定 <launch><node pkg="imu_utils" type="imu_an" name="imu_an&quo ...
使用杉川3i-T1单线激光雷达和Cartographer库SLAM问题及解决
用Cartographer做二维的激光SLAM,用杉川给的ROS例子发布LaserScan数据,发现在Rviz中显示的数据,本来应该是平直的墙变成弧形的,建图也是混乱的,如下图: 研究杉川的ROS例子 ...

激光雷达和IMU联合标定包 lidar_IMU_calib 基于RS-32的扩展

一、修改方法

二、修改vlp_common.h文件

三、修改dataset_reader.h文件

激光雷达和IMU联合标定包 lidar_IMU_calib 基于RS-32的扩展相关推荐

最新文章

热门文章