lidar-imu calibration---lidar_align+运行liosam算法测试标定效果
本文lidar-imu标定方法为瑞士苏黎世理工大学-自动驾驶实验室开源的一种校准 3D 激光雷达和 6 自由度位姿传感器外参的方法。该方法需要大量非平面的运动，因此不适合校准安装在汽车上的传感器标定代码地址：https://github.com/ethz-sl/lidar_align测试环境：nvidia agx xavier +ubuntu18.04+ROS Melodiclidar_align
·
本文lidar-imu标定方法为瑞士苏黎世理工大学-自动驾驶实验室开源的一种校准 3D 激光雷达和 6 自由度位姿传感器外参的方法。
该方法需要大量非平面的运动，因此不适合校准安装在汽车上的传感器标定
代码地址：https://github.com/ethz-sl/lidar_align
测试环境：nvidia agx xavier +ubuntu18.04+ROS Melodic
lidar_align工程代码实现了以下功能：
1、读取lidar和位姿传感器的数据
2、通过时间戳匹配lidar每帧里面的每个点和位姿传感器的坐标变换
2、通过上面的变换矩阵将利用位姿信息将lidar的每帧拼接成点云
3、每个点和它最近邻点的距离总和求出，在优化中就是不断的迭代找到坐标变换使这个距离最小
算法的整体思想：
1、为每帧lidar的每个点通过时间戳去匹配一个位姿数据，并通过插值的方式得到更准确的位姿值，每个点的时间偏移也是优化因素之一
2、利用NLopt的库的非线性优化方法
3、目标函数：让拼接后的点云的每个点的最近邻点最小
4、优化向量：x、y 、z、roll、pitch、yaw、time_offset
总体来说就是不断的迭代，找到一个合适的优化向量（也就是lidar到里程计的坐标变换）使得拼在一起的点云每个点的最近邻点距离最小。
可以离线计算，录一个rosbag，然后跑一下就可以求的外参
它只会读取一个bag ，所以 lidar和位姿都要在里面
最终的结果：
在运行的时候，功能包会输出当前的估计变化
优化结束的时候 坐标变换的参数会打印在终端上
也会在路径下面保存一个txt文件和ply文件。
可以查看ply文件，就是拼接后的点云和场景是否一致
编译：
mkdir -p lidar_align_ws/src
cd lidar_align_ws/src
git clone https://github.com/ethz-sl/lidar_align
cd ..
catkin_make
编译过程中，报错如下：
问题一：编译时出现Could not find NLOPTConfig.cmake
解决方法：
下载 安装非线性优化库nlopt
sudo apt-get install libnlopt-dev
若编译还是会报错：
Could not find a package configuration file provided by “NLOPT”
解决办法：
将 lidar_align 文件夹下的 NLOPTConfig.cmake 复制到 ROS工作空间lidar_align_ws/src路径下面
问题二：源码中没有接收IMU数据的接口，需要自己改写代码，加入imu数据的接口，只要改写loader.cpp即可，改写好的代码如下，可直接替换源码中loader.cpp的代码
#include <geometry_msgs/TransformStamped.h>
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <sensor_msgs/Imu.h>
#include "lidar_align/loader.h"
#include "lidar_align/transform.h"
namespace lidar_align {
Loader::Loader(const Config& config) : config_(config) {}
Loader::Config Loader::getConfig(ros::NodeHandle* nh) {
Loader::Config config;
nh->param("use_n_scans", config.use_n_scans, config.use_n_scans);
return config;
}
void Loader::parsePointcloudMsg(const sensor_msgs::PointCloud2 msg,
LoaderPointcloud* pointcloud) {
bool has_timing = false;
bool has_intensity = false;
for (const sensor_msgs::PointField& field : msg.fields) {
if (field.name == "time_offset_us") {
has_timing = true;
} else if (field.name == "intensity") {
has_intensity = true;
}
}
if (has_timing) {
pcl::fromROSMsg(msg, *pointcloud);
return;
} else if (has_intensity) {
Pointcloud raw_pointcloud;
pcl::fromROSMsg(msg, raw_pointcloud);
for (const Point& raw_point : raw_pointcloud) {
PointAllFields point;
point.x = raw_point.x;
point.y = raw_point.y;
point.z = raw_point.z;
point.intensity = raw_point.intensity;
if (!std::isfinite(point.x) || !std::isfinite(point.y) ||
!std::isfinite(point.z) || !std::isfinite(point.intensity)) {
continue;
}
pointcloud->push_back(point);
}
pointcloud->header = raw_pointcloud.header;
} else {
pcl::PointCloud<pcl::PointXYZ> raw_pointcloud;
pcl::fromROSMsg(msg, raw_pointcloud);
for (const pcl::PointXYZ& raw_point : raw_pointcloud) {
PointAllFields point;
point.x = raw_point.x;
point.y = raw_point.y;
point.z = raw_point.z;
if (!std::isfinite(point.x) || !std::isfinite(point.y) ||
!std::isfinite(point.z)) {
continue;
}
pointcloud->push_back(point);
}
pointcloud->header = raw_pointcloud.header;
}
}
bool Loader::loadPointcloudFromROSBag(const std::string& bag_path,
const Scan::Config& scan_config,
Lidar* lidar) {
rosbag::Bag bag;
try {
bag.open(bag_path, rosbag::bagmode::Read);
} catch (rosbag::BagException e) {
ROS_ERROR_STREAM("LOADING BAG FAILED: " << e.what());
return false;
}
std::vector<std::string> types;
types.push_back(std::string("sensor_msgs/PointCloud2"));
rosbag::View view(bag, rosbag::TypeQuery(types));
size_t scan_num = 0;
for (const rosbag::MessageInstance& m : view) {
std::cout << " Loading scan: \e[1m" << scan_num++ << "\e[0m from ros bag"
<< '\r' << std::flush;
LoaderPointcloud pointcloud;
parsePointcloudMsg(*(m.instantiate<sensor_msgs::PointCloud2>()),
&pointcloud);
lidar->addPointcloud(pointcloud, scan_config);
if (lidar->getNumberOfScans() >= config_.use_n_scans) {
break;
}
}
if (lidar->getTotalPoints() == 0) {
ROS_ERROR_STREAM(
"No points were loaded, verify that the bag contains populated "
"messages of type sensor_msgs/PointCloud2");
return false;
}
return true;
}
bool Loader::loadTformFromROSBag(const std::string& bag_path, Odom* odom) {
rosbag::Bag bag;
try {
bag.open(bag_path, rosbag::bagmode::Read);
} catch (rosbag::BagException e) {
ROS_ERROR_STREAM("LOADING BAG FAILED: " << e.what());
return false;
}
std::vector<std::string> types;
types.push_back(std::string("sensor_msgs/Imu"));
rosbag::View view(bag, rosbag::TypeQuery(types));
size_t imu_num = 0;
double shiftX=0,shiftY=0,shiftZ=0,velX=0,velY=0,velZ=0;
ros::Time time;
double timeDiff,lastShiftX,lastShiftY,lastShiftZ;
for (const rosbag::MessageInstance& m : view){
std::cout <<"Loading imu: \e[1m"<< imu_num++<<"\e[0m from ros bag"<<'\r'<< std::flush;
sensor_msgs::Imu imu=*(m.instantiate<sensor_msgs::Imu>());
Timestamp stamp = imu.header.stamp.sec * 1000000ll +imu.header.stamp.nsec / 1000ll;
if(imu_num==1){
time=imu.header.stamp;
Transform T(Transform::Translation(0,0,0),Transform::Rotation(1,0,0,0));
odom->addTransformData(stamp, T);
}
else{
timeDiff=(imu.header.stamp-time).toSec();
time=imu.header.stamp;
velX=velX+imu.linear_acceleration.x*timeDiff;
velY=velX+imu.linear_acceleration.y*timeDiff;
velZ=velZ+(imu.linear_acceleration.z-9.801)*timeDiff;
lastShiftX=shiftX;
lastShiftY=shiftY;
lastShiftZ=shiftZ;
shiftX=lastShiftX+velX*timeDiff+imu.linear_acceleration.x*timeDiff*timeDiff/2;
shiftY=lastShiftY+velY*timeDiff+imu.linear_acceleration.y*timeDiff*timeDiff/2;
shiftZ=lastShiftZ+velZ*timeDiff+(imu.linear_acceleration.z-9.801)*timeDiff*timeDiff/2;
Transform T(Transform::Translation(shiftX,shiftY,shiftZ),
Transform::Rotation(imu.orientation.w,
imu.orientation.x,
imu.orientation.y,
imu.orientation.z));
odom->addTransformData(stamp, T);
}
}
// std::vector<std::string> types;
// types.push_back(std::string("geometry_msgs/TransformStamped"));
// rosbag::View view(bag, rosbag::TypeQuery(types));
// size_t tform_num = 0;
// for (const rosbag::MessageInstance& m : view) {
// std::cout << " Loading transform: \e[1m" << tform_num++
// << "\e[0m from ros bag" << '\r' << std::flush;
// geometry_msgs::TransformStamped transform_msg =
// *(m.instantiate<geometry_msgs::TransformStamped>());
// Timestamp stamp = transform_msg.header.stamp.sec * 1000000ll +
// transform_msg.header.stamp.nsec / 1000ll;
// Transform T(Transform::Translation(transform_msg.transform.translation.x,
// transform_msg.transform.translation.y,
// transform_msg.transform.translation.z),
// Transform::Rotation(transform_msg.transform.rotation.w,
// transform_msg.transform.rotation.x,
// transform_msg.transform.rotation.y,
// transform_msg.transform.rotation.z));
// odom->addTransformData(stamp, T);
// }
if (odom->empty()) {
ROS_ERROR_STREAM("No odom messages found!");
return false;
}
return true;
}
bool Loader::loadTformFromMaplabCSV(const std::string& csv_path, Odom* odom) {
std::ifstream file(csv_path, std::ifstream::in);
size_t tform_num = 0;
while (file.peek() != EOF) {
std::cout << " Loading transform: \e[1m" << tform_num++
<< "\e[0m from csv file" << '\r' << std::flush;
Timestamp stamp;
Transform T;
if (getNextCSVTransform(file, &stamp, &T)) {
odom->addTransformData(stamp, T);
}
}
return true;
}
// lots of potential failure cases not checked
bool Loader::getNextCSVTransform(std::istream& str, Timestamp* stamp,
Transform* T) {
std::string line;
std::getline(str, line);
// ignore comment lines
if (line[0] == '#') {
return false;
}
std::stringstream line_stream(line);
std::string cell;
std::vector<std::string> data;
while (std::getline(line_stream, cell, ',')) {
data.push_back(cell);
}
if (data.size() < 9) {
return false;
}
constexpr size_t TIME = 0;
constexpr size_t X = 2;
constexpr size_t Y = 3;
constexpr size_t Z = 4;
constexpr size_t RW = 5;
constexpr size_t RX = 6;
constexpr size_t RY = 7;
constexpr size_t RZ = 8;
*stamp = std::stoll(data[TIME]) / 1000ll;
*T = Transform(Transform::Translation(std::stod(data[X]), std::stod(data[Y]),
std::stod(data[Z])),
Transform::Rotation(std::stod(data[RW]), std::stod(data[RX]),
std::stod(data[RY]), std::stod(data[RZ])));
return true;
}
} // namespace lidar_align
注意：改写后记得包含头文件#include <sensor_msgs/Imu.h>
否则会报如下错误：
error: ‘Imu’ is not a member of ‘sensor_msgs’
sensor_msgs::Imu imu=*(m.instantiate<sensor_msgs::Imu>());
^~~
/home/ubuntu/Sensors_Calibration/lidar_align_ws/src/lidar_align/src/loader.cpp:134:23: error: ‘imu’ was not declared in this scope
Timestamp stamp = imu.header.stamp.sec * 1000000ll +imu.header.stamp.nsec / 1000ll;
^~~
/home/ubuntu/Sensors_Calibration/lidar_align_ws/src/lidar_align/src/loader.cpp:134:23: note: suggested alternative: ‘time’
Timestamp stamp = imu.header.stamp.sec * 1000000ll +imu.header.stamp.nsec / 1000ll;
^~~
time
lidar_align/CMakeFiles/lidar_align.dir/build.make:110: recipe for target 'lidar_align/CMakeFiles/lidar_align.dir/src/loader.cpp.o' failed
make[2]: *** [lidar_align/CMakeFiles/lidar_align.dir/src/loader.cpp.o] Error 1
CMakeFiles/Makefile2:510: recipe for target 'lidar_align/CMakeFiles/lidar_align.dir/all' failed
make[1]: *** [lidar_align/CMakeFiles/lidar_align.dir/all] Error 2
Makefile:140: recipe for target 'all' failed
make: *** [all] Error 2
Invoking "make -j8 -l8" failed
运行LIOSAM
安装并编译好LIOSAM。
github链接： https://github.com/TixiaoShan/LIO-SAM
问题一：Error: GTSAM was built against a different version of Eigen。
这个问题是GTSAM自带的eigen库和之前安装的eigen库的版本不同导致的。
解决办法：
修改gtsam下的CMakeLists.txt文件中的内容，在if(GTSAM_USE_SYSTEM_EIGEN)前面加上set(GTSAM_USE_SYSTEM_EIGEN ON)，使得gtsam使用自己安装的eigen而不是它自带的eigen。然后重新编译gtsam，之后再编译LIO-SAM，问题解决。
编译好就可以运行数据集了，运行官网数据集的时候不需要改写param.yaml。
运行自己采集的数据，需要修改你录制的话题、IMU的内参、IMU到雷达的外参以及
PCD存放路径：
# 保存地图设置为true
savePCD: true # https://github.com/TixiaoShan/LIO-SAM/issues/3
# 设置地图保存路径
savePCDDirectory: "自己设置的路径" # in your home folder, starts and ends with "/". Warning: the code deletes "LOAM" folder then recreates it. See "mapOptimization" for implementation
然后，为了避免保存还没完成，ros就已经关闭了节点，需要设置_TIMEOUT_SIGINT的值。按如下命令打开文件：
sudo gedit /opt/ros/melodic/lib/python2.7/dist-packages/roslaunch/nodeprocess.py
在文件中找到_TIMEOUT_SIGINT，并设置为100秒。
这样，在命令窗口中按下Ctrl+C来结束run.launch的运行时，便会自动将pcd文件保存到指定地址。
lio_sam:
# Topics
# pointCloudTopic: "points_raw" # Point cloud data
pointCloudTopic: "velodyne_points"
imuTopic: "imu/data" # IMU data
# imuTopic: "imu_correct"
odomTopic: "odometry/imu" # IMU pre-preintegration odometry, same frequency as IMU
gpsTopic: "odometry/gpsz" # GPS odometry topic from navsat, see module_navsat.launch file
# Frames
lidarFrame: "base_link"
baselinkFrame: "base_link"
odometryFrame: "odom"
mapFrame: "map"
# GPS Settings
# useImuHeadingInitialization: true # if using GPS data, set to "true"
useImuHeadingInitialization: false
useGpsElevation: false # if GPS elevation is bad, set to "false"
gpsCovThreshold: 2.0 # m^2, threshold for using GPS data
poseCovThreshold: 25.0 # m^2, threshold for using GPS data
# Export settings
# savePCD: false # https://github.com/TixiaoShan/LIO-SAM/issues/3
savePCD: true
savePCDDirectory: "/dataset/LIO-SAM/xsens_velodyne" # in your home folder, starts and ends with "/". Warning: the code deletes "LOAM" folder then recreates it. See "mapOptimization" for implementation
# Sensor Settings
sensor: velodyne # lidar sensor type, either 'velodyne' or 'ouster'
N_SCAN: 16 # number of lidar channel (i.e., 16, 32, 64, 128)
Horizon_SCAN: 1800 # lidar horizontal resolution (Velodyne:1800, Ouster:512,1024,2048)
downsampleRate: 1 # default: 1. Downsample your data if too many points. i.e., 16 = 64 / 4, 16 = 16 / 1
lidarMinRange: 1.0 # default: 1.0, minimum lidar range to be used
lidarMaxRange: 1000.0 # default: 1000.0, maximum lidar range to be used
#IMU Settings
imuAccNoise: 9.7816033475532395e-03
imuGyrNoise: 1.9958347084630267e-03
imuAccBiasN: 8.4485975927320214e-05
imuGyrBiasN: 2.5139562639019187e-05
imuGravity: 9.80511
imuRPYWeight: 0.01
# imuAccNoise: 1.9238237446574064e-02
# imuGyrNoise: 1.5385754496033436e-03
# imuAccBiasN: 4.9615115224550062e-04
# imuGyrBiasN: 5.0721205121154150e-06
# imuGravity: 9.80511
# imuRPYWeight: 0.01
# Extrinsics (lidar -> IMU)
extrinsicTrans: [-0.00201536, 0.00144471, -0.00145396]
# extrinsicTrans: [0.0, 0.0, 0.0]
# extrinsicRot: [-1, 0, 0,
# 0, 1, 0,
# 0, 0, -1]
# extrinsicRPY: [0, 1, 0,
# -1, 0, 0,
# 0, 0, 1]
# extrinsicRot: [1, 0, 0,
# 0, 1, 0,
# 0, 0, 1]
# extrinsicRPY: [1, 0, 0,
# 0, 1, 0,
# 0, 0, 1]
extrinsicRot: [-0.295055, -0.562411, 0.772423,
-0.438059, -0.638821, -0.632466,
0.849145, -0.524979, -0.0578822]
extrinsicRPY: [-0.295055, -0.562411, 0.772423,
-0.438059, -0.638821, -0.632466,
0.849145, -0.524979, -0.0578822]
# LOAM feature threshold
edgeThreshold: 1.0
surfThreshold: 0.1
edgeFeatureMinValidNum: 10
surfFeatureMinValidNum: 100
# voxel filter paprams
odometrySurfLeafSize: 0.4 # default: 0.4 - outdoor, 0.2 - indoor
mappingCornerLeafSize: 0.2 # default: 0.2 - outdoor, 0.1 - indoor
mappingSurfLeafSize: 0.4 # default: 0.4 - outdoor, 0.2 - indoor
# robot motion constraint (in case you are using a 2D robot)
z_tollerance: 1000 # meters
rotation_tollerance: 1000 # radians
# CPU Params
numberOfCores: 4 # number of cores for mapping optimization
mappingProcessInterval: 0.15 # seconds, regulate mapping frequency
# Surrounding map
surroundingkeyframeAddingDistThreshold: 1.0 # meters, regulate keyframe adding threshold
surroundingkeyframeAddingAngleThreshold: 0.2 # radians, regulate keyframe adding threshold
surroundingKeyframeDensity: 2.0 # meters, downsample surrounding keyframe poses
surroundingKeyframeSearchRadius: 50.0 # meters, within n meters scan-to-map optimization (when loop closure disabled)
# Loop closure
loopClosureEnableFlag: true
loopClosureFrequency: 1.0 # Hz, regulate loop closure constraint add frequency
surroundingKeyframeSize: 50 # submap size (when loop closure enabled)
historyKeyframeSearchRadius: 15.0 # meters, key frame that is within n meters from current pose will be considerd for loop closure
historyKeyframeSearchTimeDiff: 30.0 # seconds, key frame that is n seconds older will be considered for loop closure
historyKeyframeSearchNum: 25 # number of hostory key frames will be fused into a submap for loop closure
historyKeyframeFitnessScore: 0.3 # icp threshold, the smaller the better alignment
# Visualization
globalMapVisualizationSearchRadius: 1000.0 # meters, global map visualization radius
globalMapVisualizationPoseDensity: 10.0 # meters, global map visualization keyframe density
globalMapVisualizationLeafSize: 1.0 # meters, global map visualization cloud density
# Navsat (convert GPS coordinates to Cartesian)
navsat:
frequency: 50
wait_for_datum: false
delay: 0.0
magnetic_declination_radians: 0
yaw_offset: 0
zero_altitude: true
broadcast_utm_transform: false
broadcast_utm_transform_as_parent_frame: false
publish_filtered_gps: false
# EKF for Navsat
ekf_gps:
publish_tf: false
map_frame: map
odom_frame: odom
base_link_frame: base_link
world_frame: odom
# frequency: 50
frequency: 200
two_d_mode: false
sensor_timeout: 0.01
# -------------------------------------
# External IMU:
# -------------------------------------
# imu0: imu_correct
imu0: imu/data
# make sure the input is aligned with ROS REP105. "imu_correct" is manually transformed by myself. EKF can also transform the data using tf between your imu and base_link
imu0_config: [false, false, false,
true, true, true,
false, false, false,
false, false, true,
true, true, true]
imu0_differential: false
imu0_queue_size: 50
imu0_remove_gravitational_acceleration: true
适配、运行自己数据集
由于lio-sam源码中使用了参数"ring"和"time"，这两个参数分别表示一个激光点所属的线束序号和当前激光点在一帧激光点云的扫描时间(例如一个16线激光雷达，ring代表了激光点在竖直方向上所属的线束序号，而time表示当前激光点相对于当前激光帧第一个激光点的扫描时间)。
在lio-sam系统启动后，会在imageProjection.cpp的cachePointCloud函数中检查输入点云数据是否包含这两项参数，如果没有，则程序会报错。
如果你使用的激光雷达，采集的数据中包含参数"ring"和"time"，那么是不需要进行源码改动的，只需要改动config/params.yaml中的话题名及extrinsicRot和extrinsicRPY矩阵即可(extrinsicRot和extrinsicRPY矩阵如果设置不正确则建出的地图或出现重叠)。
自己使用其它品牌雷达采集的点云数据中往往没有“ring”和”time“参数，因此需要自己在源码中进行修改，适配自己的数据。
- 对于"ring"参数，可以参考lego-loam的做法，利用公式求出当前激光点所属线束序号
- 对于“time“参数，其实这个参数的作用是记录每个点的扫描时间，用于消除点云运动畸变，因此如果暂时不考虑点云运动畸变，则可以将与time相关代码和函数注释，不影响程序运行; 如果必须考虑点云运动畸变，则可以参考lego-loam中的方法，利用公式求出每个激光点的扫描时间
运行自己数据集效果
source devel/setup.bash
roslaunch lio_sam run.launch
source devel/setup.bash
rosbag play your-bag
未使用点云去畸变，并且按照自己设备中传感器安装关系设置了extrinsicRot和extrinsicRPY矩阵
华为开发者空间，是为全球开发者打造的专属开发空间，汇聚了华为优质开发资源及工具，致力于让每一位开发者拥有一台云主机，基于华为根生态开发、创新。
更多推荐
8
0- 0
已为社区贡献1条内容
所有评论(0)