Closed liang0724s closed 2 years ago
成功适配某品牌的Lidar(32线)和IMU/INS(6轴),IMU坐标系为前右下,Lidar坐标系为x向左、y向后、z向上,激光雷达点云通过外参矩阵统一到IMU坐标系,演示平台为车载。 适配注意事项: 1.Lidar和IMU时间对齐 2.注意Lidar和IMU时间戳单位是否一致 3.如果是INS,注意加速度计输出单位g or m/s2 4.基于Lidar坐标系,注意点云方位角计算方式
大哥您好,有个问题想请教一下。我猜测你使用的激光雷达是禾赛32线,它的y朝向是向后的。我现在也是采用这款激光雷达,加一个博世的BMI085(6 轴 MEMS)IMU,它的朝向是xyz前左下,我设置的lidar->imu的外参数在LIO_SAM上顺利跑完。我用一个室内测场景来测试,在fastlio2中,开始一小段直线路段是正常的,但是在墙角的地方有一个不太大旋转,系统就飞了,终端提示position shape_scale contains NaN: Vector3(-nan, 0.000413303, 0.000549774)。我用rqt_bag查看了测试数据时间戳正常并且没有丢帧。此外,我在另外一个室内测试场景中,也是走到墙角的地方飞掉了。此外,时间戳部分,在我的代码中已经统一成s,终端输出的每一帧点云最后一个点的时间接近0.1s,应该也是正确的。请教一下,这可能是什么原因导致的呢?
common:
lid_topic: "/hesai/pandar"
imu_topic: "/alphasense/imu"
time_sync_en: false # ONLY turn on when external time synchronization is really not possible
preprocess:
lidar_type: 4 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 4 for hesai
scan_line: 32
blind: 0.3
mapping:
acc_cov: 0.0011501915187049582
gyr_cov: 5.084312924828687e-05
b_acc_cov: 0.06080652138668933
b_gyr_cov: -0.0015351229643790084
fov_degree: 180
det_range: 150.0
extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic
extrinsic_T: [ -0.001, -0.00855, 0.055 ]
extrinsic_R: [ 0, -1, 0,
-1, 0, 0,
0, 0, -1 ]
double time_begin = pl_orig.points[0].timestamp;
for (uint i = 0; i < plsize; i++) {
double range = pl_orig.points[i].x * pl_orig.points[i].x + pl_orig.points[i].y * pl_orig.points[i].y
+ pl_orig.points[i].z * pl_orig.points[i].z;
if (range < (blind * blind)) continue;
Eigen::Vector3d pt_vec;
PointType added_pt;
added_pt.x = pl_orig.points[i].x;
added_pt.y = pl_orig.points[i].y;
added_pt.z = pl_orig.points[i].z;
added_pt.intensity = pl_orig.points[i].intensity;
added_pt.normal_x = 0;
added_pt.normal_y = 0;
added_pt.normal_z = 0;
double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.3;
if (yaw_angle >= 180.0)
yaw_angle -= 360.0;
if (yaw_angle <= -180.0)
yaw_angle += 360.0;
added_pt.curvature = pl_orig.points[i].timestamp - time_begin;
if (pl_orig.points[i].ring < N_SCANS) {
pl_buff[pl_orig.points[i].ring].push_back(added_pt);
}
}
大哥您好,有个问题想请教一下。我猜测你使用的激光雷达是禾赛32线,它的y朝向是向后的。我现在也是采用这款激光雷达,加一个博世的BMI085(6 轴 MEMS)IMU,它的朝向是xyz前左下,我设置的lidar->imu的外参数在LIO_SAM上顺利跑完。我用一个室内测场景来测试,在fastlio2中,开始一小段直线路段是正常的,但是在墙角的地方有一个不太大旋转,系统就飞了,终端提示position shape_scale contains NaN: Vector3(-nan, 0.000413303, 0.000549774)。我用rqt_bag查看了测试数据时间戳正常并且没有丢帧。此外,我在另外一个室内测试场景中,也是走到墙角的地方飞掉了。此外,时间戳部分,在我的代码中已经统一成s,终端输出的每一帧点云最后一个点的时间接近0.1s,应该也是正确的。请教一下,这可能是什么原因导致的呢?
![]()
common: lid_topic: "/hesai/pandar" imu_topic: "/alphasense/imu" time_sync_en: false # ONLY turn on when external time synchronization is really not possible preprocess: lidar_type: 4 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 4 for hesai scan_line: 32 blind: 0.3 mapping: acc_cov: 0.0011501915187049582 gyr_cov: 5.084312924828687e-05 b_acc_cov: 0.06080652138668933 b_gyr_cov: -0.0015351229643790084 fov_degree: 180 det_range: 150.0 extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic extrinsic_T: [ -0.001, -0.00855, 0.055 ] extrinsic_R: [ 0, -1, 0, -1, 0, 0, 0, 0, -1 ]
double time_begin = pl_orig.points[0].timestamp; for (uint i = 0; i < plsize; i++) { double range = pl_orig.points[i].x * pl_orig.points[i].x + pl_orig.points[i].y * pl_orig.points[i].y + pl_orig.points[i].z * pl_orig.points[i].z; if (range < (blind * blind)) continue; Eigen::Vector3d pt_vec; PointType added_pt; added_pt.x = pl_orig.points[i].x; added_pt.y = pl_orig.points[i].y; added_pt.z = pl_orig.points[i].z; added_pt.intensity = pl_orig.points[i].intensity; added_pt.normal_x = 0; added_pt.normal_y = 0; added_pt.normal_z = 0; double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.3; if (yaw_angle >= 180.0) yaw_angle -= 360.0; if (yaw_angle <= -180.0) yaw_angle += 360.0; added_pt.curvature = pl_orig.points[i].timestamp - time_begin; if (pl_orig.points[i].ring < N_SCANS) { pl_buff[pl_orig.points[i].ring].push_back(added_pt); } }
你怎么判断时间戳是对的,详细说一下,你引用的这段代码是不对的。还有,你这个代码不是最新版。
是这样的,我新增了pandar_handler来处理禾赛点云,其中禾赛点云结构的定义如下:
namespace pandar_ros{
struct Point {
PCL_ADD_POINT4D
float intensity;
double timestamp;
uint16_t ring;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}
POINT_CLOUD_REGISTER_POINT_STRUCT(pandar_ros::Point,
(float, x, x)
(float, y, y)
(float, z, z)
(float, intensity, intensity)
(double, timestamp, timestamp)
(uint16_t, ring, ring)
)
其时间通道名字为timestamp,时间戳的定义是utc时间,所以在求取点云中每个点的时间戳的时候需要减掉起始点的时间,保证每个点的时间是相对于点云起始点的相对时间,范围在0-0.1s内,我把这些点的时间打印出来,发现时间数值是符合要求的。
void Preprocess::pandar_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) {
pl_surf.clear();
pl_corn.clear();
pl_full.clear();
pcl::PointCloud<pandar_ros::Point> pl_orig;
pcl::fromROSMsg(*msg, pl_orig);
int plsize = pl_orig.size();
pl_corn.reserve(plsize);
pl_surf.reserve(plsize);
if (feature_enabled) {
for (int i = 0; i < N_SCANS; i++) {
pl_buff[i].clear();
pl_buff[i].reserve(plsize);
}
double timeScanCur = msg->header.stamp.toSec();
double time_begin = pl_orig.points[0].timestamp;
for (uint i = 0; i < plsize; i++) {
double range = pl_orig.points[i].x * pl_orig.points[i].x + pl_orig.points[i].y * pl_orig.points[i].y
+ pl_orig.points[i].z * pl_orig.points[i].z;
if (range < (blind * blind)) continue;
Eigen::Vector3d pt_vec;
PointType added_pt;
added_pt.x = pl_orig.points[i].x;
added_pt.y = pl_orig.points[i].y;
added_pt.z = pl_orig.points[i].z;
added_pt.intensity = pl_orig.points[i].intensity;
added_pt.normal_x = 0;
added_pt.normal_y = 0;
added_pt.normal_z = 0;
double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.3;
if (yaw_angle >= 180.0)
yaw_angle -= 360.0;
if (yaw_angle <= -180.0)
yaw_angle += 360.0;
added_pt.curvature = pl_orig.points[i].timestamp - time_begin;
if (i % 2000) {
std::cout << std::fixed << std::setprecision(12) << "time: " << added_pt.curvature << std::endl;
std::cout << std::fixed << std::setprecision(12) << "point time: " << pl_orig.points[i].timestamp << std::endl;
}
if (pl_orig.points[i].ring < N_SCANS) {
pl_buff[pl_orig.points[i].ring].push_back(added_pt);
}
}
for (int j = 0; j < N_SCANS; j++) {
PointCloudXYZI &pl = pl_buff[j];
int linesize = pl.size();
vector<orgtype> &types = typess[j];
types.clear();
types.resize(linesize);
linesize--;
for (uint i = 0; i < linesize; i++) {
types[i].range = sqrt(pl[i].x * pl[i].x + pl[i].y * pl[i].y);
vx = pl[i].x - pl[i + 1].x;
vy = pl[i].y - pl[i + 1].y;
vz = pl[i].z - pl[i + 1].z;
types[i].dista = vx * vx + vy * vy + vz * vz;
}
types[linesize].range = sqrt(pl[linesize].x * pl[linesize].x + pl[linesize].y * pl[linesize].y);
give_feature(pl, types);
}
} else {
double time_stamp = msg->header.stamp.toSec();
double time_begin = pl_orig.points[0].timestamp;
// cout << "===================================" << endl;
// printf("Pt size = %d, N_SCANS = %d\r\n", plsize, N_SCANS);
for (int i = 0; i < pl_orig.points.size(); i++) {
if (i % point_filter_num != 0) continue;
double range = pl_orig.points[i].x * pl_orig.points[i].x + pl_orig.points[i].y * pl_orig.points[i].y
+ pl_orig.points[i].z * pl_orig.points[i].z;
if (range < (blind * blind)) continue;
Eigen::Vector3d pt_vec;
PointType added_pt;
// change the axis
added_pt.x = pl_orig.points[i].x;
added_pt.y = pl_orig.points[i].y;
added_pt.z = pl_orig.points[i].z;
added_pt.intensity = pl_orig.points[i].intensity;
added_pt.normal_x = 0;
added_pt.normal_y = 0;
added_pt.normal_z = 0;
added_pt.curvature = pl_orig.points[i].timestamp - time_begin;
if (i % 2000) {
std::cout << std::fixed << std::setprecision(12) << "curvature: " << added_pt.curvature << std::endl;
std::cout << std::fixed << std::setprecision(12) << "point time: " << pl_orig.points[i].timestamp << std::endl;
}
pl_surf.points.push_back(added_pt);
}
ROS_WARN("GET HESAI POINT CLOUD %d: ", pl_surf.points.size());
}
// pub_func(pl_surf, pub_full, msg->header.stamp);
// pub_func(pl_surf, pub_corn, msg->header.stamp);
}
因为这里点的时间戳我采用的是秒为单位,所以在laserMapping.cpp中sync_packages的时候判定点云结束时间部分代码我也进行了相应调整,统一时间单位为s.
/*** push a lidar scan ***/
if (!lidar_pushed) {
meas.lidar = lidar_buffer.front();
meas.lidar_beg_time = time_buffer.front();
if (meas.lidar->points.size() <= 1) // time too little
{
lidar_end_time = meas.lidar_beg_time + lidar_mean_scantime;
ROS_WARN("Too few input point cloud!\n");
} else if (meas.lidar->points.back().curvature < 0.5 * lidar_mean_scantime) {
// } else if (meas.lidar->points.back().curvature / double(1000) < 0.5 * lidar_mean_scantime) {
// std::cout << "lidar end time: " << lidar_end_time << std::endl;
} else {
scan_num++;
lidar_end_time = meas.lidar_beg_time + meas.lidar->points.back().curvature / double(1000);
// lidar_mean_scantime += (meas.lidar->points.back().curvature / double(1000) - lidar_mean_scantime) / scan_num;
lidar_mean_scantime += (meas.lidar->points.back().curvature - lidar_mean_scantime) / scan_num;
std::cout << "lidar end time: " << lidar_end_time << std::endl;
}
lidar_end_time = meas.lidar_beg_time + lidar_mean_scantime;
meas.lidar_end_time = lidar_end_time;
lidar_pushed = true;
}
终端的输出结果如下,点云中点的时间基本保持在0-0.1s之间,这与我10hz的激光雷达是相匹配的。我这个代码是两个月前下载的,我也观察到您在20天前更新了代码,增加了时间单位设置的内容。其他的核心算法部分有更改吗?
我的坐标系定义如下:
added_pt.curvature的单位需要是毫秒,也就是需要在0-100 .0之间。
您好,我刚才更新了最新的代码。我应该找到了原因。可能是我的IMU数据不太稳定,加上我本身用它的标定的加速度计和陀螺仪的bia也有一定的问题,我换成了默认的bias(比我标定的结果小2个数量级),就正常跑完。此外,在室内这种场景中,要想达到fastlio的最佳效果,有哪些参数需要格外注意的吗?
您好,我刚才更新了最新的代码。我应该找到了原因。可能是我的IMU数据不太稳定,加上我本身用它的标定的加速度计和陀螺仪的bia也有一定的问题,我换成了默认的bias(比我标定的结果小2个数量级),就正常跑完。此外,在室内这种场景中,要想达到fastlio的最佳效果,有哪些参数需要格外注意的吗?
你是学术实验还是?
您好,我刚才更新了最新的代码。我应该找到了原因。可能是我的IMU数据不太稳定,加上我本身用它的标定的加速度计和陀螺仪的bia也有一定的问题,我换成了默认的bias(比我标定的结果小2个数量级),就正常跑完。此外,在室内这种场景中,要想达到fastlio的最佳效果,有哪些参数需要格外注意的吗?
你是学术实验还是?
对,我是用于学术实验,室内场景居多。
您好,我刚才更新了最新的代码。我应该找到了原因。可能是我的IMU数据不太稳定,加上我本身用它的标定的加速度计和陀螺仪的bia也有一定的问题,我换成了默认的bias(比我标定的结果小2个数量级),就正常跑完。此外,在室内这种场景中,要想达到fastlio的最佳效果,有哪些参数需要格外注意的吗?
你是学术实验还是?
对,我是用于学术实验,室内场景居多。
我用的数据室外场景居多,包括退化场景,目前从适配结果看,容易发生180度转弯(慢速)匹配失败,退化场景比如长隧道也容易失败
您好,我刚才更新了最新的代码。我应该找到了原因。可能是我的IMU数据不太稳定,加上我本身用它的标定的加速度计和陀螺仪的bia也有一定的问题,我换成了默认的bias(比我标定的结果小2个数量级),就正常跑完。此外,在室内这种场景中,要想达到fastlio的最佳效果,有哪些参数需要格外注意的吗?
你是学术实验还是?
对,我是用于学术实验,室内场景居多。
我用的数据室外场景居多,包括退化场景,目前从适配结果看,容易发生180度转弯(慢速)匹配失败,退化场景比如长隧道也容易失败
你这个现象听上去就像是时间戳单位不对。
确定Lidar输出的时间戳UTM时间,单位是秒,所以对应的配置项timestamp_unit =0, 最终换算成ms,处理lidar的句柄函数已根据velodyne句柄做了适配修改。 下面是我的配置项 common: lid_topic: "/points_raw" imu_topic: "/imu" time_sync_en: false # ONLY turn on when external time synchronization is really not possible
preprocess: lidar_type: 4 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 4 for hesai LiDAR scan_line: 32 scan_rate: 20 # only need to be set for velodyne, unit: Hz, timestamp_unit: 0 # the unit of time/t field in the PointCloud2 rostopic: 0-second-s, 1-milisecond-ms, 2-microsecond-μs, 3-nanosecond-ns. blind: 2
mapping: acc_cov: 0.1 gyr_cov: 0.1 b_acc_cov: 0.0001 b_gyr_cov: 0.0001 fov_degree: 180 det_range: 100.0 extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic, extrinsic_T: [ 0.002, 0.807, 1.680 ] extrinsic_R: [ 1.000, 0.012, 0.023, -0.013, -1.000, -0.024, 0.022, -0.024, 0.999]
publish: path_en: true scan_publish_en: true # false: close all the point cloud output dense_publish_en: true # false: low down the points number in a global-frame point clouds scan. scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame
pcd_save: pcd_save_en: true interval: -1 # how many LiDAR frames saved in each pcd file;
下图是180度掉头匹配失败情况,旋涡左右两边车道应该是重合的
ikfom结果输出:
IMU结果输出:
从图片看,在转弯时刻陀螺仪数据特征很明显,可以看出经过了三个转弯,前面两个是90度转弯。后面是180度掉头
确定Lidar输出的时间戳UTM时间,单位是秒,所以对应的配置项timestamp_unit =0, 最终换算成ms,处理lidar的句柄函数已根据velodyne句柄做了适配修改。 下面是我的配置项 common: lid_topic: "/points_raw" imu_topic: "/imu" time_sync_en: false # ONLY turn on when external time synchronization is really not possible
preprocess: lidar_type: 4 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 4 for hesai LiDAR scan_line: 32 scan_rate: 20 # only need to be set for velodyne, unit: Hz, timestamp_unit: 0 # the unit of time/t field in the PointCloud2 rostopic: 0-second-s, 1-milisecond-ms, 2-microsecond-μs, 3-nanosecond-ns. blind: 2
mapping: acc_cov: 0.1 gyr_cov: 0.1 b_acc_cov: 0.0001 b_gyr_cov: 0.0001 fov_degree: 180 det_range: 100.0 extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic, extrinsic_T: [ 0.002, 0.807, 1.680 ] extrinsic_R: [ 1.000, 0.012, 0.023, -0.013, -1.000, -0.024, 0.022, -0.024, 0.999]
publish: path_en: true scan_publish_en: true # false: close all the point cloud output dense_publish_en: true # false: low down the points number in a global-frame point clouds scan. scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame
pcd_save: pcd_save_en: true interval: -1 # how many LiDAR frames saved in each pcd file; # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.
下图是180度掉头匹配失败情况,旋涡左右两边车道应该是重合的
![]()
ikfom结果输出:
IMU结果输出:
从图片看,在转弯时刻陀螺仪数据特征很明显,可以看出经过了三个转弯,前面两个是90度转弯。后面是180度掉头
180度掉头后的Attitude曲线应该如下图红线所示:
怀疑问题是不是出现在ICP匹配上?
确定Lidar输出的时间戳UTM时间,单位是秒,所以对应的配置项timestamp_unit =0, 最终换算成ms,处理lidar的句柄函数已根据velodyne句柄做了适配修改。 下面是我的配置项 common: lid_topic: "/points_raw" imu_topic: "/imu" time_sync_en: false # ONLY turn on when external time synchronization is really not possible
preprocess: lidar_type: 4 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 4 for hesai LiDAR scan_line: 32 scan_rate: 20 # only need to be set for velodyne, unit: Hz, timestamp_unit: 0 # the unit of time/t field in the PointCloud2 rostopic: 0-second-s, 1-milisecond-ms, 2-microsecond-μs, 3-nanosecond-ns. blind: 2
mapping: acc_cov: 0.1 gyr_cov: 0.1 b_acc_cov: 0.0001 b_gyr_cov: 0.0001 fov_degree: 180 det_range: 100.0 extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic, extrinsic_T: [ 0.002, 0.807, 1.680 ] extrinsic_R: [ 1.000, 0.012, 0.023, -0.013, -1.000, -0.024, 0.022, -0.024, 0.999]
publish: path_en: true scan_publish_en: true # false: close all the point cloud output dense_publish_en: true # false: low down the points number in a global-frame point clouds scan. scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame
pcd_save: pcd_save_en: true interval: -1 # how many LiDAR frames saved in each pcd file; # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.
下图是180度掉头匹配失败情况,旋涡左右两边车道应该是重合的
![]()
ikfom结果输出:
IMU结果输出:
从图片看,在转弯时刻陀螺仪数据特征很明显,可以看出经过了三个转弯,前面两个是90度转弯。后面是180度掉头
你的旋转外参是怎么搞到的?你这个旋转外参的旋转矩阵的行列式是-1,这是不对的。
确定Lidar输出的时间戳UTM时间,单位是秒,所以对应的配置项timestamp_unit =0, 最终换算成ms,处理lidar的句柄函数已根据velodyne句柄做了适配修改。 下面是我的配置项 common: lid_topic: "/points_raw" imu_topic: "/imu" time_sync_en: false # ONLY turn on when external time synchronization is really not possible
preprocess: lidar_type: 4 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 4 for hesai LiDAR scan_line: 32 scan_rate: 20 # only need to be set for velodyne, unit: Hz, timestamp_unit: 0 # the unit of time/t field in the PointCloud2 rostopic: 0-second-s, 1-milisecond-ms, 2-microsecond-μs, 3-nanosecond-ns. blind: 2 mapping: acc_cov: 0.1 gyr_cov: 0.1 b_acc_cov: 0.0001 b_gyr_cov: 0.0001 fov_degree: 180 det_range: 100.0 extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic, extrinsic_T: [ 0.002, 0.807, 1.680 ] extrinsic_R: [ 1.000, 0.012, 0.023, -0.013, -1.000, -0.024, 0.022, -0.024, 0.999] publish: path_en: true scan_publish_en: true # false: close all the point cloud output dense_publish_en: true # false: low down the points number in a global-frame point clouds scan. scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame pcd_save: pcd_save_en: true interval: -1 # how many LiDAR frames saved in each pcd file; # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. 下图是180度掉头匹配失败情况,旋涡左右两边车道应该是重合的
![]()
ikfom结果输出:
IMU结果输出:
从图片看,在转弯时刻陀螺仪数据特征很明显,可以看出经过了三个转弯,前面两个是90度转弯。后面是180度掉头
你的旋转外参是怎么搞到的?你这个旋转外参的旋转矩阵的行列式是-1,这是不对的。
仔细检查了下,粗心大意,旋转矩阵的(0,0)项少了”-“号。
这个旋转矩阵和平移向量其实是激光到车体坐标系的旋转,我最终把他俩转到IMU坐标系下【 左乘IMU(前-右-下)到车体坐标系(右-前-上)的转换矩阵的逆(x,y,z,r,p,y)=(0.0, 0.0, 0.0, 0.0, PI_M, -PI_M / 2))】,再传给IMU,同时每帧点云也是先转换到IMU坐标系后再进行畸变校准和匹配,最终到得到的pose也是在IMU坐标系。
更正后又跑了一下,还是得到上面的错误轨迹
注:我的这段数据有动态交通流,180度转弯发生在内地小区门口(结构特征还挺明显),当时转弯时有一辆货车经过。掉头后也有车辆经过,但从数据看,也能匹配上。
确定Lidar输出的时间戳UTM时间,单位是秒,所以对应的配置项timestamp_unit =0, 最终换算成ms,处理lidar的句柄函数已根据velodyne句柄做了适配修改。 下面是我的配置项 common: lid_topic: "/points_raw" imu_topic: "/imu" time_sync_en: false # ONLY turn on when external time synchronization is really not possible
preprocess: lidar_type: 4 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 4 for hesai LiDAR scan_line: 32 scan_rate: 20 # only need to be set for velodyne, unit: Hz, timestamp_unit: 0 # the unit of time/t field in the PointCloud2 rostopic: 0-second-s, 1-milisecond-ms, 2-microsecond-μs, 3-nanosecond-ns. blind: 2 mapping: acc_cov: 0.1 gyr_cov: 0.1 b_acc_cov: 0.0001 b_gyr_cov: 0.0001 fov_degree: 180 det_range: 100.0 extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic, extrinsic_T: [ 0.002, 0.807, 1.680 ] extrinsic_R: [ 1.000, 0.012, 0.023, -0.013, -1.000, -0.024, 0.022, -0.024, 0.999] publish: path_en: true scan_publish_en: true # false: close all the point cloud output dense_publish_en: true # false: low down the points number in a global-frame point clouds scan. scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame pcd_save: pcd_save_en: true interval: -1 # how many LiDAR frames saved in each pcd file; # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. 下图是180度掉头匹配失败情况,旋涡左右两边车道应该是重合的
![]()
ikfom结果输出:
IMU结果输出:
从图片看,在转弯时刻陀螺仪数据特征很明显,可以看出经过了三个转弯,前面两个是90度转弯。后面是180度掉头
你的旋转外参是怎么搞到的?你这个旋转外参的旋转矩阵的行列式是-1,这是不对的。
仔细检查了下,粗心大意,旋转矩阵的(0,0)项少了”-“号。 这个旋转矩阵和平移向量其实是激光到车体坐标系的旋转,我最终把他俩转到IMU坐标系下【 左乘IMU(前-右-下)到车体坐标系(右-前-上)的转换矩阵的逆(x,y,z,r,p,y)=(0.0, 0.0, 0.0, 0.0, PI_M, -PI_M / 2))】,再传给IMU,同时每帧点云也是先转换到IMU坐标系后再进行畸变校准和匹配,最终到得到的pose也是在IMU坐标系。 更正后又跑了一下,还是得到上面的错误轨迹
注:我的这段数据有动态交通流,180度转弯发生在内地小区门口(结构特征还挺明显),当时转弯时有一辆货车经过。掉头后也有车辆经过,但从数据看,也能匹配上。
你这个旋转的说法我没看懂,既然你的imu和lidar已经align到了xyz朝向一致,那你的外参应该使用单位阵而不是对角线元素-1 -1 1。 而且,我说的时间戳单位不是指lidar rostopic header的时间戳单位,而是指lidar一帧里面每个点的相对时间戳的单位。
- launch file 里面的 point_filter_num 改成1试试
- 你可以检查下转弯的时候路边建筑的点云细节,如果墙面有模糊,那大概率是imu外参或者数据转换的问题(比如xy轴反了这种可能)。
@XW-HKU 太感谢啦,根据您的思路仔细梳理一下坐标系的转换过程,确实是数据转换出现了问题,首先您在后向传播过程中其实已经利用外参对点云进行了坐标转换,但是我又在处理点云ros节点的时候又对点云进行了坐标转换。
注意到一个问题,对于后向传播阶段对点云进行校准的公式,您注释说公式不准确,是因为只用到旋转,就像 Duplicate of #113 所以提到的那样。
确定Lidar输出的时间戳UTM时间,单位是秒,所以对应的配置项timestamp_unit =0, 最终换算成ms,处理lidar的句柄函数已根据velodyne句柄做了适配修改。 下面是我的配置项 common: lid_topic: "/points_raw" imu_topic: "/imu" time_sync_en: false # ONLY turn on when external time synchronization is really not possible
preprocess: lidar_type: 4 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 4 for hesai LiDAR scan_line: 32 scan_rate: 20 # only need to be set for velodyne, unit: Hz, timestamp_unit: 0 # the unit of time/t field in the PointCloud2 rostopic: 0-second-s, 1-milisecond-ms, 2-microsecond-μs, 3-nanosecond-ns. blind: 2 mapping: acc_cov: 0.1 gyr_cov: 0.1 b_acc_cov: 0.0001 b_gyr_cov: 0.0001 fov_degree: 180 det_range: 100.0 extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic, extrinsic_T: [ 0.002, 0.807, 1.680 ] extrinsic_R: [ 1.000, 0.012, 0.023, -0.013, -1.000, -0.024, 0.022, -0.024, 0.999] publish: path_en: true scan_publish_en: true # false: close all the point cloud output dense_publish_en: true # false: low down the points number in a global-frame point clouds scan. scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame pcd_save: pcd_save_en: true interval: -1 # how many LiDAR frames saved in each pcd file; # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. 下图是180度掉头匹配失败情况,旋涡左右两边车道应该是重合的
![]()
ikfom结果输出:
IMU结果输出:
从图片看,在转弯时刻陀螺仪数据特征很明显,可以看出经过了三个转弯,前面两个是90度转弯。后面是180度掉头
你的旋转外参是怎么搞到的?你这个旋转外参的旋转矩阵的行列式是-1,这是不对的。
仔细检查了下,粗心大意,旋转矩阵的(0,0)项少了”-“号。 这个旋转矩阵和平移向量其实是激光到车体坐标系的旋转,我最终把他俩转到IMU坐标系下【 左乘IMU(前-右-下)到车体坐标系(右-前-上)的转换矩阵的逆(x,y,z,r,p,y)=(0.0, 0.0, 0.0, 0.0, PI_M, -PI_M / 2))】,再传给IMU,同时每帧点云也是先转换到IMU坐标系后再进行畸变校准和匹配,最终到得到的pose也是在IMU坐标系。 更正后又跑了一下,还是得到上面的错误轨迹
注:我的这段数据有动态交通流,180度转弯发生在内地小区门口(结构特征还挺明显),当时转弯时有一辆货车经过。掉头后也有车辆经过,但从数据看,也能匹配上。
你这个旋转的说法我没看懂,既然你的imu和lidar已经align到了xyz朝向一致,那你的外参应该使用单位阵而不是对角线元素-1 -1 1。 而且,我说的时间戳单位不是指lidar rostopic header的时间戳单位,而是指lidar一帧里面每个点的相对时间戳的单位。
我的imu和lidar的坐标系需要需要我设定的旋转矩阵才能align到了xyz朝向一致,您的这行代码也说明了这点: V3D P_compensate = imu_state.offset_R_L_I.conjugate() (imu_state.rot.conjugate() (R_i (imu_state.offset_R_L_I P_i + imu_state.offset_T_L_I) + T_ei) - imu_state.offset_T_L_I);// not accurate!
lidar一帧里面每个点的相对时间戳的单位=s, 然后基于time_unit_scale换算成ms进行后面的计算,您的代码里也是这么写的,这部分的逻辑我没有改动。
确定Lidar输出的时间戳UTM时间,单位是秒,所以对应的配置项timestamp_unit =0, 最终换算成ms,处理lidar的句柄函数已根据velodyne句柄做了适配修改。 下面是我的配置项 common: lid_topic: "/points_raw" imu_topic: "/imu" time_sync_en: false # ONLY turn on when external time synchronization is really not possible
preprocess: lidar_type: 4 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 4 for hesai LiDAR scan_line: 32 scan_rate: 20 # only need to be set for velodyne, unit: Hz, timestamp_unit: 0 # the unit of time/t field in the PointCloud2 rostopic: 0-second-s, 1-milisecond-ms, 2-microsecond-μs, 3-nanosecond-ns. blind: 2 mapping: acc_cov: 0.1 gyr_cov: 0.1 b_acc_cov: 0.0001 b_gyr_cov: 0.0001 fov_degree: 180 det_range: 100.0 extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic, extrinsic_T: [ 0.002, 0.807, 1.680 ] extrinsic_R: [ 1.000, 0.012, 0.023, -0.013, -1.000, -0.024, 0.022, -0.024, 0.999] publish: path_en: true scan_publish_en: true # false: close all the point cloud output dense_publish_en: true # false: low down the points number in a global-frame point clouds scan. scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame pcd_save: pcd_save_en: true interval: -1 # how many LiDAR frames saved in each pcd file; # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. 下图是180度掉头匹配失败情况,旋涡左右两边车道应该是重合的
![]()
ikfom结果输出:
IMU结果输出:
从图片看,在转弯时刻陀螺仪数据特征很明显,可以看出经过了三个转弯,前面两个是90度转弯。后面是180度掉头
你的旋转外参是怎么搞到的?你这个旋转外参的旋转矩阵的行列式是-1,这是不对的。
仔细检查了下,粗心大意,旋转矩阵的(0,0)项少了”-“号。 这个旋转矩阵和平移向量其实是激光到车体坐标系的旋转,我最终把他俩转到IMU坐标系下【 左乘IMU(前-右-下)到车体坐标系(右-前-上)的转换矩阵的逆(x,y,z,r,p,y)=(0.0, 0.0, 0.0, 0.0, PI_M, -PI_M / 2))】,再传给IMU,同时每帧点云也是先转换到IMU坐标系后再进行畸变校准和匹配,最终到得到的pose也是在IMU坐标系。 更正后又跑了一下,还是得到上面的错误轨迹
注:我的这段数据有动态交通流,180度转弯发生在内地小区门口(结构特征还挺明显),当时转弯时有一辆货车经过。掉头后也有车辆经过,但从数据看,也能匹配上。
你这个旋转的说法我没看懂,既然你的imu和lidar已经align到了xyz朝向一致,那你的外参应该使用单位阵而不是对角线元素-1 -1 1。 而且,我说的时间戳单位不是指lidar rostopic header的时间戳单位,而是指lidar一帧里面每个点的相对时间戳的单位。
我的imu和lidar的坐标系需要需要我设定的旋转矩阵才能align到了xyz朝向一致,您的这行代码也说明了这点: V3D P_compensate = imu_state.offset_R_L_I.conjugate() (imu_state.rot.conjugate() (R_i (imu_state.offset_R_L_I P_i + imu_state.offset_T_L_I) + T_ei) - imu_state.offset_T_L_I);// not accurate!
lidar一帧里面每个点的相对时间戳的单位=s, 然后基于time_unit_scale换算成ms进行后面的计算,您的代码里也是这么写的,这部分的逻辑我没有改动。
后向传播的公司是正确的,注释应该是忘删了。不过问题解决了就行。
大哥您好,有个问题想请教一下。我猜测你使用的激光雷达是禾赛32线,它的y朝向是向后的。我现在也是采用这款激光雷达,加一个博世的BMI085(6 轴 MEMS)IMU,它的朝向是xyz前左下,我设置的lidar->imu的外参数在LIO_SAM上顺利跑完。我用一个室内测场景来测试,在fastlio2中,开始一小段直线路段是正常的,但是在墙角的地方有一个不太大旋转,系统就飞了,终端提示position shape_scale contains NaN: Vector3(-nan, 0.000413303, 0.000549774)。我用rqt_bag查看了测试数据时间戳正常并且没有丢帧。此外,我在另外一个室内测试场景中,也是走到墙角的地方飞掉了。此外,时间戳部分,在我的代码中已经统一成s,终端输出的每一帧点云最后一个点的时间接近0.1s,应该也是正确的。请教一下,这可能是什么原因导致的呢?
![]()
common: lid_topic: "/hesai/pandar" imu_topic: "/alphasense/imu" time_sync_en: false # ONLY turn on when external time synchronization is really not possible preprocess: lidar_type: 4 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 4 for hesai scan_line: 32 blind: 0.3 mapping: acc_cov: 0.0011501915187049582 gyr_cov: 5.084312924828687e-05 b_acc_cov: 0.06080652138668933 b_gyr_cov: -0.0015351229643790084 fov_degree: 180 det_range: 150.0 extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic extrinsic_T: [ -0.001, -0.00855, 0.055 ] extrinsic_R: [ 0, -1, 0, -1, 0, 0, 0, 0, -1 ]
double time_begin = pl_orig.points[0].timestamp; for (uint i = 0; i < plsize; i++) { double range = pl_orig.points[i].x * pl_orig.points[i].x + pl_orig.points[i].y * pl_orig.points[i].y + pl_orig.points[i].z * pl_orig.points[i].z; if (range < (blind * blind)) continue; Eigen::Vector3d pt_vec; PointType added_pt; added_pt.x = pl_orig.points[i].x; added_pt.y = pl_orig.points[i].y; added_pt.z = pl_orig.points[i].z; added_pt.intensity = pl_orig.points[i].intensity; added_pt.normal_x = 0; added_pt.normal_y = 0; added_pt.normal_z = 0; double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.3; if (yaw_angle >= 180.0) yaw_angle -= 360.0; if (yaw_angle <= -180.0) yaw_angle += 360.0; added_pt.curvature = pl_orig.points[i].timestamp - time_begin; if (pl_orig.points[i].ring < N_SCANS) { pl_buff[pl_orig.points[i].ring].push_back(added_pt); } }
大佬,您好,我使用的是禾赛Panda人QT64,禾赛64线的激光雷达,没办法适配,我按照您说的一些内容进行配置,在终端打印的是 No point, skip this scan! 我想咨询你关于适配的问题,诚盼您的回复,感谢
大哥您好,有个问题想请教一下。我猜测你使用的激光雷达是禾赛32线,它的y朝向是向后的。我现在也是采用这款激光雷达,加一个博世的BMI085(6 轴 MEMS)IMU,它的朝向是xyz前左下,我设置的lidar->imu的外参数在LIO_SAM上顺利跑完。我用一个室内测场景来测试,在fastlio2中,开始一小段直线路段是正常的,但是在墙角的地方有一个不太大旋转,系统就飞了,终端提示position shape_scale contains NaN: Vector3(-nan, 0.000413303, 0.000549774)。我用rqt_bag查看了测试数据时间戳正常并且没有丢帧。此外,我在另外一个室内测试场景中,也是走到墙角的地方飞掉了。此外,时间戳部分,在我的代码中已经统一成s,终端输出的每一帧点云最后一个点的时间接近0.1s,应该也是正确的。请教一下,这可能是什么原因导致的呢?
![]()
common: lid_topic: "/hesai/pandar" imu_topic: "/alphasense/imu" time_sync_en: false # ONLY turn on when external time synchronization is really not possible preprocess: lidar_type: 4 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 4 for hesai scan_line: 32 blind: 0.3 mapping: acc_cov: 0.0011501915187049582 gyr_cov: 5.084312924828687e-05 b_acc_cov: 0.06080652138668933 b_gyr_cov: -0.0015351229643790084 fov_degree: 180 det_range: 150.0 extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic extrinsic_T: [ -0.001, -0.00855, 0.055 ] extrinsic_R: [ 0, -1, 0, -1, 0, 0, 0, 0, -1 ]
double time_begin = pl_orig.points[0].timestamp; for (uint i = 0; i < plsize; i++) { double range = pl_orig.points[i].x * pl_orig.points[i].x + pl_orig.points[i].y * pl_orig.points[i].y + pl_orig.points[i].z * pl_orig.points[i].z; if (range < (blind * blind)) continue; Eigen::Vector3d pt_vec; PointType added_pt; added_pt.x = pl_orig.points[i].x; added_pt.y = pl_orig.points[i].y; added_pt.z = pl_orig.points[i].z; added_pt.intensity = pl_orig.points[i].intensity; added_pt.normal_x = 0; added_pt.normal_y = 0; added_pt.normal_z = 0; double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.3; if (yaw_angle >= 180.0) yaw_angle -= 360.0; if (yaw_angle <= -180.0) yaw_angle += 360.0; added_pt.curvature = pl_orig.points[i].timestamp - time_begin; if (pl_orig.points[i].ring < N_SCANS) { pl_buff[pl_orig.points[i].ring].push_back(added_pt); } }
大哥,如果可以的话,我想您能不能将您进行修改用于适配的文件发给我学习一下,感激不尽!
前提:已标定好Lidar(32线)和IMU(6轴)外参,,rosbag topic类型已转成livox_ros_driver/CustomMsg和 sensor_msgs/Imu, 但是坐标系和livox avia不同。请问在坐标转换的时候,那些地方需要注意或者要修改的?