从零开始激光slam定位【1】- 激光里程计

本文主要是介绍从零开始激光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-->imustd::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_idstd::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作为初始posefront_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);               // 更新当前帧，返回当前帧的poselaser_odom_pub_ptr->Publish(laser_matrix);                  // lidar发布里程计信息（当前帧的pose）/****************************************************/// 以下为我新增的tf变换，map--->lidar// 发布tf变换map--->lidar// current_pose_写入transformtransform.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 rpytransform.setRotation(q);//trans from qbr_.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点云指针的时候没有初始化，应该在构造函数中进行初始化。

参考

从零开始做自动驾驶定位(四): 前端里程计之初试

这篇关于从零开始激光slam定位【1】- 激光里程计的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！

---