前段时间参与一个项目需要进行激光雷达和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文件,修改关于激光雷达类型的相关函数和变量、常量等。
打开文件后先看了一遍代码,即定义了一个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); //该点的角度值
//四舍五入并转化为整型2
azimuth_corrected = ((int)round(azimuth_corrected_f)) % 36000;
/*condition added to avoid calculating points which are not
in 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;
//距离满足要求,则将求得的值赋值给云点,否则赋值NAN
if (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 headings
for (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,即360
constexpr 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个block
static 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 publish
double min_range;
int min_angle; ///< minimum angle to publish
int max_angle; ///< maximum angle to publish
} Config;
Config m_config;
ModelType m_modelType;
double mRS32TimeBlock[1800][32];
这个文件要改的很少,主要把和激光雷达类型相关代码修改成自己定义的类型就可以了,修改了下面这些部分。
这里如果要用第一个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有什么不足欢迎指出,希望能对各位小伙伴有所帮助。