0. 简介
最近在尝试着用SensorsCalibration这个项目来做一些工作，我们知道imu与lidar的外参比较难标定，而OpenCalib提出了手动和自动标定两种方式。我们本文将会学习如何完成Lidar_imu自动标定，并学习如何拿到自己的数据来完成离线标定。
1. 相关代码
这个lidar2imu的相关代码已经有博主注释过了，这里我们给出链接
- Lidar_imu自动标定
- Lidar_imu自动标定源码阅读（一）——registration部分
- Lidar_imu自动标定源码阅读（二）——calibration部分
- Lidar_imu自动标定源码阅读（三）——gen_BALM_feature部分
- Lidar_imu自动标定源码阅读（四）——optimize部分
- Lidar_imu自动标定源码阅读（五）——BALM部分
- Lidar_imu自动标定源码阅读（六）——run部分
有了个相册作者基本将lidar2imu的相关代码都注释完了，我们可以看到该算法的主要操作就是利用特征点来完成优化，目前测试下来结果的准确度还是比较依赖初始位置的，基本上10°以内的修正还是可以做到的，如果太大就不行了。一般来说越结构化的场景肯定优化效果更好：
gen_BALM_feature.hpp：主要实现的功能是对于点云计算曲率，并根据曲率来提取特征点，并分为角点和面点。
optimize.hpp：主要是利用ceres完成非线性优化，但是在这个cpp中只有代价函数的构造和残差的定义，以及使用自动求导，没有针对一个具体的问题展开求解
BALM.hpp：利用八叉树的思想，构建体素地图，然后利用ceres非线性优化进行求解，优化外参变化矩阵。
2. 数据采集
由于该算法是使用txt文件和pcd生成的，所以我们需要自己写一套数据生成脚本，来完成ros bag 数据包的转换，下面是详细的代码：
#include "online_calib.h"
namespace localization {
namespace onlinecalib {
OnlineCalib::OnlineCalib()
: Node("OnlineCalib"), sync_policy_(SyncPolicy(100)) {
std::string car_num;
this->declare_parameter<std::string>("ROBOT_ID", "xxx");
this->get_parameter("ROBOT_ID", ns_);
QINFO << "ROBOT_ID: " << ns_ << std::endl;
lid_topic_front_left_ = ns_ + "/full_point";
gnss_odom_topic_ = ns_ + "/gnss/odom";
is_calib_init_ = false;
// sub clouds
lid_front_left_sub_.subscribe(this, lid_topic_front_left_);
gnss_odom_sub_.subscribe(this, gnss_odom_topic_);
// do time sync
sync_policy_.connectInput(lid_front_left_sub_, gnss_odom_sub_);
sync_policy_.registerCallback(std::bind(&OnlineCalib::TimeSyncCallback, this,
std::placeholders::_1,
std::placeholders::_2));
QINFO << "finish initialize";
timer_ =
this->create_wall_timer(std::chrono::milliseconds(20),
std::bind(&OnlineCalib::timer_callback, this));
}
void OnlineCalib::TimeSyncCallback(
const sensor_msgs::msg::PointCloud2::ConstSharedPtr front_left_msg,
const nav_msgs::msg::Odometry::ConstSharedPtr gnss_odom_msg) {
pcl::PointCloud<LidarPointXYZIRT>::Ptr cloud(
new pcl::PointCloud<LidarPointXYZIRT>);
cloud->clear();
// cloud->resize(front_left_msg->width*front_left_msg->height);
// pcl::fromROSMsg(*front_left_msg, *cloud);
for (uint32_t i = 0; i < front_left_msg->width * front_left_msg->height;
++i) {
LidarPointXYZIRT p;
float x = 0.0, y = 0.0, z = 0.0;
uint8_t r = 0, g = 0, b = 0, a = 0;
memcpy(&p.x,
&front_left_msg->data[i * front_left_msg->point_step +
front_left_msg->fields[0].offset],
sizeof(float));
memcpy(&p.y,
&front_left_msg->data[i * front_left_msg->point_step +
front_left_msg->fields[1].offset],
sizeof(float));
memcpy(&p.z,
&front_left_msg->data[i * front_left_msg->point_step +
front_left_msg->fields[2].offset],
sizeof(float));
memcpy(&p.intensity,
&front_left_msg->data[i * front_left_msg->point_step +
front_left_msg->fields[3].offset],
sizeof(float));
memcpy(&p.ring,
&front_left_msg->data[i * front_left_msg->point_step +
front_left_msg->fields[4].offset],
sizeof(uint16_t));
memcpy(&p.timestamp,
&front_left_msg->data[i * front_left_msg->point_step +
front_left_msg->fields[5].offset],
sizeof(double));
cloud->points.push_back(p);
}
if (lidar_queue_.size() < 100 && gnss_odom_queue_.size() < 100) {
lidar_queue_.push(cloud);
gnss_odom_queue_.push(gnss_odom_msg);
} else {
lidar_queue_.pop();
gnss_odom_queue_.pop();
lidar_queue_.push(cloud);
gnss_odom_queue_.push(gnss_odom_msg);
}
return;
}
void OnlineCalib::timer_callback() {
if (!lidar_queue_.empty() && !gnss_odom_queue_.empty()) {
auto cloud_msg = lidar_queue_.front();
auto gnss_odom_msg = gnss_odom_queue_.front();
gnss_odom_queue_.pop();
lidar_queue_.pop();
Eigen::Vector3d position =
Eigen::Vector3d(gnss_odom_msg->pose.pose.position.x,
gnss_odom_msg->pose.pose.position.y,
gnss_odom_msg->pose.pose.position.z);
Eigen::Quaterniond rotation =
Eigen::Quaterniond(gnss_odom_msg->pose.pose.orientation.w,
gnss_odom_msg->pose.pose.orientation.x,
gnss_odom_msg->pose.pose.orientation.y,
gnss_odom_msg->pose.pose.orientation.z);
Sophus::SE3d current_pose(rotation, position);
if (is_calib_init_ == false) {
is_calib_init_ = true;
T_lidar_init = current_pose;
}
Sophus::SE3d T_lidar = T_lidar_init.inverse() * current_pose;
std::string path = "/debug/pcd/";
std::string path_pose = path + "pose.txt";
std::string stamp_str =
std::to_string(rclcpp::Time(gnss_odom_msg->header.stamp).seconds());
if (!boost::filesystem::exists(path)) {
boost::filesystem::create_directory(path);
}
std::ofstream fout_pose(path_pose, std::ios::app);
Eigen::Matrix3d pose_matrix3d =
T_lidar.so3().unit_quaternion().toRotationMatrix();
fout_pose << stamp_str << " " << pose_matrix3d(0, 0) << " "
<< pose_matrix3d(0, 1) << " " << pose_matrix3d(0, 2) << " "
<< T_lidar.translation().x() << " " << pose_matrix3d(1, 0) << " "
<< pose_matrix3d(1, 1) << " " << pose_matrix3d(1, 2) << " "
<< T_lidar.translation().y() << " " << pose_matrix3d(2, 0) << " "
<< pose_matrix3d(2, 1) << " " << pose_matrix3d(2, 2) << " "
<< T_lidar.translation().z() << " 0.0 0.0 0.0 1.0" << std::endl;
std::string file_name = stamp_str + ".pcd";
std::string file_path = path + "top_center_lidar/";
if (!boost::filesystem::exists(file_path)) {
boost::filesystem::create_directory(file_path);
}
// pcl::transformPointCloud(*cloud_msg, *cloud_msg, T_lidar.matrix());
pcl::io::savePCDFileBinary(file_path + file_name, *cloud_msg);
}
}
} // namespace onlinecalib
} // namespace localization
int main(int argc, char **argv) {
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<localization::onlinecalib::OnlineCalib>());
rclcpp::shutdown();
return 0;
}通过这样的形式我们可以读取时间戳作为pcd保存
并将对应的结果存入对应的txt中
经过测试，该方法标定精度还是比较依赖激光建图环境的，因为是体素的方法，这里还是建议用更高细粒度的提速完成更高精度的建图
评论（0）
您还未登录，请登录后发表或查看评论