十五.激光和惯导LIO-SLAM框架学习之惯导与雷达外参标定（1）

专栏系列文章如下：

一：Tixiao Shan最新力作LVI-SAM(Lio-SAM+Vins-Mono)，基于视觉-激光-惯导里程计的SLAM框架，环境搭建和跑通过程_goldqiu的博客-CSDN博客

二.激光SLAM框架学习之A-LOAM框架---介绍及其演示_goldqiu的博客-CSDN博客

三.激光SLAM框架学习之A-LOAM框架---项目工程代码介绍---1.项目文件介绍（除主要源码部分）_goldqiu的博客-CSDN博客

四.激光SLAM框架学习之A-LOAM框架---项目工程代码介绍---2.scanRegistration.cpp--前端雷达处理和特征提取_goldqiu的博客-CSDN博客

五.激光SLAM框架学习之A-LOAM框架---项目工程代码介绍---3.laserOdometry.cpp--前端雷达里程计和位姿粗估计_goldqiu的博客-CSDN博客

六.激光SLAM框架学习之A-LOAM框架---项目工程代码介绍---4.laserMapping.cpp--后端建图和帧位姿精估计（优化）_goldqiu的博客-CSDN博客

七.激光SLAM框架学习之A-LOAM框架---速腾Robosense-16线雷达室内建图_goldqiu的博客-CSDN博客

八.激光SLAM框架学习之LeGO-LOAM框架---框架介绍和运行演示_goldqiu的博客-CSDN博客

九.激光SLAM框架学习之LeGO-LOAM框架---速腾Robosense-16线雷达室外建图和其他框架对比、录包和保存数据_goldqiu的博客-CSDN博客

十.激光SLAM框架学习之LeGO-LOAM框架---算法原理和改进、项目工程代码_goldqiu的博客-CSDN博客

十一.激光惯导LIO-SLAM框架学习之LIO-SAM框架---框架介绍和运行演示

十二.激光SLAM框架学习之livox-loam框架安装和跑数据集_goldqiu的博客-CSDN博客_livox 数据集

十三.激光SLAM框架学习之livox-Mid-70雷达使用和实时室外跑框架_goldqiu的博客-CSDN博客

十四.激光和惯导LIO-SLAM框架学习之惯导内参标定_goldqiu的博客-CSDN博客

即确认惯导与雷达的旋转变换矩阵

平移向量影响不是很大，直接从结构图纸中得到即可，即是惯导原点到雷达原点的向量。旋转矩阵对建图影响比较大，首先我们得确认理论的旋转矩阵，然后再进行标定。

确认理论的旋转矩阵的方法：

将imageProjection.cpp里这个回调函数中的注释打开。

  void imuHandler(const sensor_msgs::Imu::ConstPtr& imuMsg){sensor_msgs::Imu thisImu = imuConverter(*imuMsg);   // 对imu做一个坐标转换// 加一个线程锁，把imu数据保存进队列std::lock_guard<std::mutex> lock1(imuLock);imuQueue.push_back(thisImu);// debug IMU data// cout << std::setprecision(6);// cout << "IMU acc: " << endl;// cout << "x: " << thisImu.linear_acceleration.x << //       ", y: " << thisImu.linear_acceleration.y << //       ", z: " << thisImu.linear_acceleration.z << endl;// cout << "IMU gyro: " << endl;// cout << "x: " << thisImu.angular_velocity.x << //       ", y: " << thisImu.angular_velocity.y << //       ", z: " << thisImu.angular_velocity.z << endl;// double imuRoll, imuPitch, imuYaw;// tf::Quaternion orientation;// tf::quaternionMsgToTF(thisImu.orientation, orientation);// tf::Matrix3x3(orientation).getRPY(imuRoll, imuPitch, imuYaw);// cout << "IMU roll pitch yaw: " << endl;// cout << "roll: " << imuRoll << ", pitch: " << imuPitch << ", yaw: " << imuYaw << endl << endl;}

参考这个视频https://youtu.be/BOUK8LYQhHs

确认惯导的输出是否正常，如果Z轴的加速度是负数，则Z轴反过来了，查看

LIO-SAM中config文件夹中的配置yaml文件：

这两个矩阵需要做调整，根据与视频的现象进行的对比，这里发现单位阵是可以达到Z轴加速度是正数9.8的（重力加速度）。

这里就已经确定了Z轴，我们暂时无法确定X轴和Y轴的朝向，可以参考后面标定出来的旋转矩阵。

其中extrinsicRot表示imu->lidar的坐标变换, 用于旋转imu坐标系下的加速度和角速度到lidar坐标系下, extrinsicRPY则用于旋转imu坐标系下的欧拉角到lidar坐标下, 由于lio-sam作者使用的imu的欧拉角旋转方向与lidar坐标系不一致（即是按照什么旋转顺序旋转）, 因此使用了两个旋转不同, 但是大部分的设备两个旋转应该是设置为相同的，我们这里也是设置为相同即可。

下面进行标定：

第一种标定软件（不可行）：

下载标定工具

mkdir -p catkin_ws/src
cd catkin_ws/src
git clone https://github.com/chennuo0125-HIT/lidar_imu_calib.git
cd ..
catkin_make -DCATKIN_WHITELIST_PACKAGES="ndt_omp;lidar_imu_calib"

第二种标定软件（目前可行）：

https://github.com/stevengj/nlopt.git

github下载nlopt 并cmake编译

https://github.com/ethz-asl/lidar_align.git

github下载源码进行ROS编译

编译时出现Could not find NLOPTConfig.cmake

解决办法：找到这个文件并将其放入到工程目录下，并在CMakeLists.txt里加上这样一句话：

list(APPEND CMAKE_FIND_ROOT_PATH ${PROJECT_SOURCE_DIR})

由于这个标定软件没有IMU的数据接口，所以改写http://loader.cc

改写如下：

#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);}}/*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 linesif (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

录制话题数据，旋转三个轴，XY轴不要大幅度旋转

rosbag record -o 20211117.bag out /velodyne_points /imu_raw

修改标定软件包launch文件中的数据包路径，然后运行launch文件，等待漫长迭代优化时间。

最后输出数据：

这里发现标定矩阵类似于单位阵，说明单位阵是理论外参，而标定后的矩阵是考虑了小角度误差后的外参。

把旋转矩阵复制到LIO-SAM中config文件夹中的配置yaml文件中，更改这两个矩阵。

最后室外测试效果：

十五.激光和惯导LIO-SLAM框架学习之惯导与雷达外参标定（1）相关推荐

二十一.激光、视觉和惯导LVIO-SLAM框架学习之相机与雷达外参标定（1）
专栏系列文章如下: 一:Tixiao Shan最新力作LVI-SAM(Lio-SAM+Vins-Mono),基于视觉-激光-惯导里程计的SLAM框架,环境搭建和跑通过程_goldqiu的博客-CSDN ...
十四.激光和惯导LIO-SLAM框架学习之惯导内参标定
专栏系列文章如下: 一:Tixiao Shan最新力作LVI-SAM(Lio-SAM+Vins-Mono),基于视觉-激光-惯导里程计的SLAM框架,环境搭建和跑通过程_goldqiu的博客-CSDN ...
十二.激光SLAM框架学习之livox-loam框架安装和跑数据集
专栏系列文章如下: 一:Tixiao Shan最新力作LVI-SAM(Lio-SAM+Vins-Mono),基于视觉-激光-惯导里程计的SLAM框架,环境搭建和跑通过程_goldqiu的博客-CSDN ...
十.激光SLAM框架学习之LeGO-LOAM框架---算法原理和改进、项目工程代码
专栏系列文章如下: 一:Tixiao Shan最新力作LVI-SAM(Lio-SAM+Vins-Mono),基于视觉-激光-惯导里程计的SLAM框架,环境搭建和跑通过程_goldqiu的博客-CSDN ...
五.激光SLAM框架学习之A-LOAM框架---项目工程代码介绍---3.laserOdometry.cpp--前端雷达里程计和位姿粗估计
专栏系列文章如下: 一:Tixiao Shan最新力作LVI-SAM(Lio-SAM+Vins-Mono),基于视觉-激光-惯导里程计的SLAM框架,环境搭建和跑通过程_goldqiu的博客-CSDN ...
九.激光SLAM框架学习之LeGO-LOAM框架---速腾Robosense-16线雷达室外建图和其他框架对比、录包和保存数据
专栏系列文章如下: 一:Tixiao Shan最新力作LVI-SAM(Lio-SAM+Vins-Mono),基于视觉-激光-惯导里程计的SLAM框架,环境搭建和跑通过程_goldqiu的博客-CSDN ...
十三.激光SLAM框架学习之livox-Mid-70雷达使用和实时室外跑框架
专栏系列文章如下: 一:Tixiao Shan最新力作LVI-SAM(Lio-SAM+Vins-Mono),基于视觉-激光-惯导里程计的SLAM框架,环境搭建和跑通过程_goldqiu的博客-CSDN ...
八.激光SLAM框架学习之LeGO-LOAM框架---框架介绍和运行演示
专栏系列文章如下: 一:Tixiao Shan最新力作LVI-SAM(Lio-SAM+Vins-Mono),基于视觉-激光-惯导里程计的SLAM框架,环境搭建和跑通过程_goldqiu的博客-CSDN ...
七.激光SLAM框架学习之A-LOAM框架---速腾Robosense-16线雷达室内建图
专栏系列文章如下: 一:Tixiao Shan最新力作LVI-SAM(Lio-SAM+Vins-Mono),基于视觉-激光-惯导里程计的SLAM框架,环境搭建和跑通过程_goldqiu的博客-CSDN ...

十五.激光和惯导LIO-SLAM框架学习之惯导与雷达外参标定（1）

专栏系列文章如下：

十五.激光和惯导LIO-SLAM框架学习之惯导与雷达外参标定（1）相关推荐

最新文章

热门文章