从零开始激光slam定位【1】- 激光里程计
写在前面：本文是参考知乎任乾大佬的帖子完成，增加了自己的注释以及一些其他功能，如果有理解不到位的地方，请大家指正~
里程计流程
1、如果当前帧是第一帧，那么将其看做关键帧，进行关键帧更新
2、关键帧更新都做了什么？更新local_map队列，更新local_map，更新ndt的target_cloud。
3、如果不是第一帧，那么将其与local_map进行ndt配准，配准的所需要的guess_pose是根据匀速运动模型进行计算的。
4、配准得到当前帧的pose，利用当前帧和上一阵的pose，结合匀速运动模型得到下一次配准的guess_pose；如果当前帧的pose相对于上一关键帧的pose距离较大，则更新关键帧。
front_end_node.cpp的注释
/*
* @Descripttion: 对任乾大佬的tag4.0增加map--->lidar的tf变换，并可视化最原始的变换后的点云
* @vision:
* @Author: suyunzzz
* @Date: 2020-08-20 23:40:44
* @LastEditors: Please set LastEditors
* @LastEditTime: 2020-08-26 23:20:21
*/
/*
* @Description:
* @Author: Ren Qian
* @Date: 2020-02-05 02:56:27
*/
#include <ros/ros.h>
#include <pcl/common/transforms.h>
#include "glog/logging.h"
#include "lidar_localization/global_defination/global_defination.h"
#include "lidar_localization/subscriber/cloud_subscriber.hpp"
#include "lidar_localization/subscriber/imu_subscriber.hpp"
#include "lidar_localization/subscriber/gnss_subscriber.hpp"
#include "lidar_localization/tf_listener/tf_listener.hpp"
#include "lidar_localization/publisher/cloud_publisher.hpp"
#include "lidar_localization/publisher/odometry_publisher.hpp"
#include "lidar_localization/front_end/front_end.hpp" // 前端相关的头文件
#include <tf/transform_broadcaster.h>
#include <tf/transform_datatypes.h>
using namespace lidar_localization;
int main(int argc, char *argv[]) {
google::InitGoogleLogging(argv[0]);
FLAGS_log_dir = WORK_SPACE_PATH + "/Log";
FLAGS_alsologtostderr = 1;
// 初始化节点
ros::init(argc, argv, "front_end_node");
ros::NodeHandle nh;
std::shared_ptr<CloudSubscriber> cloud_sub_ptr = std::make_shared<CloudSubscriber>(nh, "/kitti/velo/pointcloud", 100000);
std::shared_ptr<IMUSubscriber> imu_sub_ptr = std::make_shared<IMUSubscriber>(nh, "/kitti/oxts/imu", 1000000);
std::shared_ptr<GNSSSubscriber> gnss_sub_ptr = std::make_shared<GNSSSubscriber>(nh, "/kitti/oxts/gps/fix", 1000000);
// TF监听。lidar-->imu
std::shared_ptr<TFListener> lidar_to_imu_ptr = std::make_shared<TFListener>(nh, "velo_link", "imu_link");
// 发布者，发布到frame_id="/map"
std::shared_ptr<CloudPublisher> cloud_pub_ptr = std::make_shared<CloudPublisher>(nh, "current_scan", 100, "/map");
std::shared_ptr<CloudPublisher> raw_cloud_pub_ptr = std::make_shared<CloudPublisher>(nh,"raw_current_scan",100,"/map");
std::shared_ptr<CloudPublisher> local_map_pub_ptr = std::make_shared<CloudPublisher>(nh, "local_map", 100, "/map");
std::shared_ptr<CloudPublisher> global_map_pub_ptr = std::make_shared<CloudPublisher>(nh, "global_map", 100, "/map");
// 发布odom信息，topic_name,base_frame_id,child_frame_id
std::shared_ptr<OdometryPublisher> laser_odom_pub_ptr = std::make_shared<OdometryPublisher>(nh, "laser_odom", "map", "lidar", 100);
std::shared_ptr<OdometryPublisher> gnss_pub_ptr = std::make_shared<OdometryPublisher>(nh, "gnss", "map", "lidar", 100);
// 创建一个前端对象
std::shared_ptr<FrontEnd> front_end_ptr = std::make_shared<FrontEnd>();
// 数据队列
std::deque<CloudData> cloud_data_buff;
std::deque<IMUData> imu_data_buff;
std::deque<GNSSData> gnss_data_buff;
Eigen::Matrix4f lidar_to_imu = Eigen::Matrix4f::Identity();
bool transform_received = false;
bool gnss_origin_position_inited = false;
bool front_end_pose_inited = false; // true 前端pose未初始化
// 局部地图，全局地图，当前帧点云
CloudData::CLOUD_PTR local_map_ptr(new CloudData::CLOUD());
CloudData::CLOUD_PTR global_map_ptr(new CloudData::CLOUD());
CloudData::CLOUD_PTR current_scan_ptr(new CloudData::CLOUD());
CloudData::CLOUD_PTR current_scan_ptr_raw(new CloudData::CLOUD()); // 当前帧变换后的结果，
/****************************************************/
// 以下为我新增的tf变换，map--->lidar
// 发布tf变换 map--->lidar
//static tf::TransformBroadcaster br;
tf::TransformBroadcaster br_;
tf::Transform transform;
tf::Quaternion q;
struct pose{double x,y,z;
double roll,pitch,yaw;};
struct pose current_pose_;
/***************************************************/
double run_time = 0.0;
double init_time = 0.0;
bool time_inited = false;
bool has_global_map_published = false;
ros::Rate rate(100);
while (ros::ok()) {
ros::spinOnce();
// 数据读入
cloud_sub_ptr->ParseData(cloud_data_buff);
imu_sub_ptr->ParseData(imu_data_buff);
gnss_sub_ptr->ParseData(gnss_data_buff);
// 如果没有得到变换
if (!transform_received) {
if (lidar_to_imu_ptr->LookupData(lidar_to_imu)) {
transform_received = true;
}
} else {
while (cloud_data_buff.size() > 0 && imu_data_buff.size() > 0 && gnss_data_buff.size() > 0) {
CloudData cloud_data = cloud_data_buff.front();
IMUData imu_data = imu_data_buff.front();
GNSSData gnss_data = gnss_data_buff.front();
if (!time_inited) {
time_inited = true;
init_time = cloud_data.time;
} else {
run_time = cloud_data.time - init_time;
}
double d_time = cloud_data.time - imu_data.time;
if (d_time < -0.05) { // cloud慢
cloud_data_buff.pop_front();
} else if (d_time > 0.05) { // cloud快
imu_data_buff.pop_front();
gnss_data_buff.pop_front();
} else { // 开始
cloud_data_buff.pop_front(); // 将使用的数据在队列中删除
imu_data_buff.pop_front();
gnss_data_buff.pop_front();
Eigen::Matrix4f odometry_matrix = Eigen::Matrix4f::Identity();
if (!gnss_origin_position_inited) {
gnss_data.InitOriginPosition();
gnss_origin_position_inited = true;
}
gnss_data.UpdateXYZ();
odometry_matrix(0,3) = gnss_data.local_E;
odometry_matrix(1,3) = gnss_data.local_N;
odometry_matrix(2,3) = gnss_data.local_U;
odometry_matrix.block<3,3>(0,0) = imu_data.GetOrientationMatrix();
odometry_matrix *= lidar_to_imu;
gnss_pub_ptr->Publish(odometry_matrix); // gnss发布里程计信息
if (!front_end_pose_inited) { // 如果未初始化前端，将gnss的pose作为初始pose
front_end_pose_inited = true;
front_end_ptr->SetInitPose(odometry_matrix);
}
front_end_ptr->SetPredictPose(odometry_matrix); // gnss的里程计作为预测（没有用到）
Eigen::Matrix4f laser_matrix = front_end_ptr->Update(cloud_data); // 更新当前帧，返回当前帧的pose
laser_odom_pub_ptr->Publish(laser_matrix); // lidar发布里程计信息（当前帧的pose）
/****************************************************/
// 以下为我新增的tf变换，map--->lidar
// 发布tf变换map--->lidar
// current_pose_写入transform
transform.setOrigin(tf::Vector3(laser_matrix(0,3), laser_matrix(1,3), laser_matrix(2,3))); // transform 设置平移
tf::Matrix3x3 mat_b;
mat_b.setValue(static_cast<double>(laser_matrix(0, 0)), static_cast<double>(laser_matrix(0, 1)),
static_cast<double>(laser_matrix(0, 2)), static_cast<double>(laser_matrix(1, 0)),
static_cast<double>(laser_matrix(1, 1)), static_cast<double>(laser_matrix(1, 2)),
static_cast<double>(laser_matrix(2, 0)), static_cast<double>(laser_matrix(2, 1)),
static_cast<double>(laser_matrix(2, 2)));
mat_b.getRPY(current_pose_.roll, current_pose_.pitch, current_pose_.yaw, 1);//mat2rpy 旋转
q.setRPY(current_pose_.roll, current_pose_.pitch, current_pose_.yaw); //q from rpy
transform.setRotation(q);//trans from q
br_.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "map", "lidar")); // map--->scan的transform
/****************************************************/
// 当前帧的点云
front_end_ptr->GetCurrentScan(current_scan_ptr); // 变换到global下的当前帧点云
cloud_pub_ptr->Publish(current_scan_ptr); // 发布当前帧点云
front_end_ptr->GetCurrentScanRaw(current_scan_ptr_raw); // 发布变换后的原始点云,未下采样
raw_cloud_pub_ptr->Publish(current_scan_ptr_raw);
if (front_end_ptr->GetNewLocalMap(local_map_ptr)) // 发布局部地图,只有遇到关键帧才能更新局部地图
local_map_pub_ptr->Publish(local_map_ptr);
}
if (run_time > 460.0 && !has_global_map_published) { // 全局地图，只能发布一次，并且有时间限制
if (front_end_ptr->GetNewGlobalMap(global_map_ptr)) { // 每100个关键帧，才更新一次全局地图
global_map_pub_ptr->Publish(global_map_ptr);
has_global_map_published = true;
}
}
}
}
rate.sleep();
}
return 0;
}
可视化
黄色的代表gnss的里程计，红色的代表激光里程计，大的Axis的坐标系是lidar，小的坐标系是map。
debug
typename boost::detail::sp_dereference<T>::type boost::shared_ptr<T>::operator*() const [with T = pcl::PointCloud<pcl::PointXYZ>; typename boost::detail::sp_dereference<T>::type = pcl::PointCloud<pcl::PointXYZ>&]: Assertion `px != 0' failed
解决：
这个问题的原因是创建pcl点云指针的时候没有初始化，应该在构造函数中进行初始化。