# 在此首先感谢任佬的无私分享，有兴趣的同学可以去任佬的知乎进行学习

从零开始做自动驾驶定位

注：遇到不清不楚的地方一定要立马解决，总想着以后解决，以后不懂的越来越多就会焦虑想放弃，希望这句话能时刻提醒自己

 

对应任佬代码tag4.0，后续会对代码不断的改进，任佬说一套好的代码要从最简单的开始，所以我准备一步一步进行优化，也是对不怎么熟练的C++学习。

CMakeLists部分

---

首先学习CMakeLists,这部分包含了整体框架的结构，是学习工程很好的一个切入点

cmake_minimum_required(VERSION 2.8.3)
project(lidar_localization)

#Release:发布版本进行优化  Debug：调试版本不进行优化
SET(CMAKE_BUILD_TYPE "Release")
SET(CMAKE_CXX_FLAGS_RELEASE "$ENV{CXXFLAGS} -O3 -Wall")
#使用C++11
#为当前路径和下层路径的目标增加编译器命令行选项
add_compile_options(-std=c++11)
add_definitions(-std=c++11)

#发现依赖包
find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  pcl_ros
  geometry_msgs
  tf
  eigen_conversions
)
#将所有的依赖进行打包，修改后也随着变化
set(ALL_TARGET_LIBRARIES "")
#将文件放入cmake文件下，方便管理，在cmake文件中在去找对应的包
include(cmake/glog.cmake)
include(cmake/PCL.cmake)
include(cmake/eigen.cmake)
include(cmake/geographic.cmake)

include_directories(include ${catkin_INCLUDE_DIRS})
include(cmake/global_defination.cmake)
catkin_package()

#简化代码
#整合所有的cpp文件 到 ALL_SRCS
file(GLOB_RECURSE ALL_SRCS "*.cpp")
#整合所有的src/*_node.cpp文件 到 NODE_SRCS
file(GLOB_RECURSE NODE_SRCS "src/*_node.cpp")
#整合所有的third_party/*.cpp文件 到 THIRD_PARTY_SRCS
file(GLOB_RECURSE THIRD_PARTY_SRCS "third_party/*.cpp")
#在 ALL_SRCS 剔除 NODE_SRCS THIRD_PARTY_SRCS 所包含的内容
list(REMOVE_ITEM ALL_SRCS ${NODE_SRCS})
list(REMOVE_ITEM ALL_SRCS ${THIRD_PARTY_SRCS})

#最终需要编译的就是两个运行节点
#加入上面打包好的依赖，很方便使用
add_executable(test_frame_node src/test_frame_node.cpp ${ALL_SRCS})
target_link_libraries(test_frame_node ${catkin_LIBRARIES} ${ALL_TARGET_LIBRARIES})

add_executable(front_end_node src/front_end_node.cpp ${ALL_SRCS})
target_link_libraries(front_end_node ${catkin_LIBRARIES} ${ALL_TARGET_LIBRARIES})

根据CMakeLists.txt的说明，主要运行节点为front_end_node.cpp,下面我们一起看看

front_end_node主要流程

---

/*
 * @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"

using namespace lidar_localization;

int main(int argc, char *argv[]) {
    //初始化日志函数  接受一个char*类型的参数也就是argv[0]
    google::InitGoogleLogging(argv[0]);
    //FLAGS_log_dir设置日志输出目录
    FLAGS_log_dir = WORK_SPACE_PATH + "/Log";
    //是否将所有日志输出到文件 这里选择是
    FLAGS_alsologtostderr = 1;
    //初始化ros节点 与 句柄
    ros::init(argc, argv, "front_end_node");
    ros::NodeHandle nh;

    //接收点云、imu、gnss、雷达与imu之间的标定信息话题
    //make_shared<CloudSubscriber>: make_shared使用过程就是调用CloudSubscriber中的模板函数推断出创建类型
    //shared_ptr 对象可以与相同的指针相关联，并在内部使用引用计数机制 计数为0是自动释放内存
    //std::make_shared 一次性为int对象和用于引用计数的数据都分配了内存，而new操作符只是为int分配了内存。
    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);
    std::shared_ptr<TFListener> lidar_to_imu_ptr = std::make_shared<TFListener>(nh, "velo_link", "imu_link");
    //发布当前扫描、局部地图、全局地图、激光里程计、guss里程计话题 
    std::shared_ptr<CloudPublisher> cloud_pub_ptr = std::make_shared<CloudPublisher>(nh, "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");
    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>();
    //定义用来保存点云 imu gnss 当前时刻的信息容器
    //CloudData为保存点云的类
    std::deque<CloudData> cloud_data_buff;
    std::deque<IMUData> imu_data_buff;
    std::deque<GNSSData> gnss_data_buff;
    //初始化lidar与imu坐标系之间的转化关系
    Eigen::Matrix4f lidar_to_imu = Eigen::Matrix4f::Identity();
    bool transform_received = false;
    bool gnss_origin_position_inited = false;
    bool front_end_pose_inited = false;
    //定义类指针 用来存储局部地图  全局地图  当前帧扫描点云
    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());
    
    double run_time = 0.0;
    double init_time = 0.0;
    bool time_inited = false;
    bool has_global_map_published = false;
    //设置循环频率100hz
    ros::Rate rate(100);
    while (ros::ok()) {
        ros::spinOnce();
        //把新点云、imu、gnss数据保存到cloud_data_buff imu_data_buff gnss_data_buff
        cloud_sub_ptr->ParseData(cloud_data_buff);
        imu_sub_ptr->ParseData(imu_data_buff);
        gnss_sub_ptr->ParseData(gnss_data_buff);
        //导入lidar与imu位姿关系
        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;
                }
                //保证点云数据与imu、gnss数据时间同步
                double d_time = cloud_data.time - imu_data.time;
                if (d_time < -0.05) {
                    cloud_data_buff.pop_front();
                } else if (d_time > 0.05) {
                    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();
                    //初始化gnss数据
                    if (!gnss_origin_position_inited) {
                        gnss_data.InitOriginPosition();
                        gnss_origin_position_inited = true;
                    }
                    //更新gnss数据 转化到imu坐标系下，并发布
                    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) {
                        front_end_pose_inited = true;
                        front_end_ptr->SetInitPose(odometry_matrix);
                    }
                    //用里程计去预测位姿
                    front_end_ptr->SetPredictPose(odometry_matrix);
                    //更新点云信息并发布
                    Eigen::Matrix4f laser_matrix = front_end_ptr->Update(cloud_data);
                    laser_odom_pub_ptr->Publish(laser_matrix);

                    front_end_ptr->GetCurrentScan(current_scan_ptr);
                    cloud_pub_ptr->Publish(current_scan_ptr);
                    //发布局部地图
                    if (front_end_ptr->GetNewLocalMap(local_map_ptr))
                        local_map_pub_ptr->Publish(local_map_ptr);
                }
                //时间超过460秒，则发布一次最新的全局地图
                if (run_time > 460.0 && !has_global_map_published) {
                    if (front_end_ptr->GetNewGlobalMap(global_map_ptr)) {
                        global_map_pub_ptr->Publish(global_map_ptr);
                        has_global_map_published = true;
                    }
                }
            }
        }

        rate.sleep();
    }

    return 0;
}

 各个部分都模块化是很容易阅读和扩展的一种方式，下面以点云数据为例

点云类型的定义模块

---

---

namespace lidar_localization {
class CloudData {
  public:
    using POINT = pcl::PointXYZ;
    //多个点组成点云容器
    using CLOUD = pcl::PointCloud<POINT>;
    //指向点云容器的指针
    using CLOUD_PTR = CLOUD::Ptr;

  public:
    //cloud_ptr(new CLOUD()) 等价于 cloud_ptr = new CLOUD 给cloud_ptr 分配内存  
    //cloud_ptr后面就是指向点云容器的指针   cloud_ptr是后面用到的
    CloudData()
      :cloud_ptr(new CLOUD()) {
    }

  public:
    double time = 0.0;
    CLOUD_PTR cloud_ptr;
};
}

 点云话题发布者模块

---

namespace lidar_localization {
CloudPublisher::CloudPublisher(ros::NodeHandle& nh,
                               std::string topic_name,
                               size_t buff_size,
                               std::string frame_id)
    //用nh_代替nh，frame_id_代替frame_id
    :nh_(nh), frame_id_(frame_id) {
    //发布者格式
    publisher_ = nh_.advertise<sensor_msgs::PointCloud2>(topic_name, buff_size);
}

void CloudPublisher::Publish(CloudData::CLOUD_PTR  cloud_ptr_input) {
    sensor_msgs::PointCloud2Ptr cloud_ptr_output(new sensor_msgs::PointCloud2());
    //转化成ros格式进行发布
    pcl::toROSMsg(*cloud_ptr_input, *cloud_ptr_output);
    //提取时间辍，ros::Time::now()随当前时间变化
    cloud_ptr_output->header.stamp = ros::Time::now();
    cloud_ptr_output->header.frame_id = frame_id_;
    publisher_.publish(*cloud_ptr_output);
}

} // namespace data_output

点云话题订阅者模块

---

---

namespace lidar_localization {
    //订阅者接受数据函数
CloudSubscriber::CloudSubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size)
    :nh_(nh) {
    subscriber_ = nh_.subscribe(topic_name, buff_size, &CloudSubscriber::msg_callback, this);
}

//回调函数接受点云数据，并把点云从ros格式转化为pcl格式，然后存储在new_cloud_data_中
void CloudSubscriber::msg_callback(const sensor_msgs::PointCloud2::ConstPtr& cloud_msg_ptr) {
    //cloud_data类 包含时间信息和点云容器
    CloudData cloud_data;
    cloud_data.time = cloud_msg_ptr->header.stamp.toSec();
    pcl::fromROSMsg(*cloud_msg_ptr, *(cloud_data.cloud_ptr));

    new_cloud_data_.push_back(cloud_data);
}
//用于数据的传输，把新来的点云数据保存到当前点云数据中
//cloud_data_buff由很多CloudData类组成的容器
void CloudSubscriber::ParseData(std::deque<CloudData>& cloud_data_buff) {
    if (new_cloud_data_.size() > 0) {
        //新来的点云被插入在尾部
        cloud_data_buff.insert(cloud_data_buff.end(), new_cloud_data_.begin(), new_cloud_data_.end());
        //清空接受新点云的内存
        new_cloud_data_.clear();
    }
}
} // namespace data_input

 GNSS定义模块

---

主要作用就是把经纬高坐标系转化为东北天ENU坐标系

namespace lidar_localization {
class GNSSData {
  public:
    double time = 0.0;
    //经度 维度 高度
    double longitude = 0.0;
    double latitude = 0.0;
    double altitude = 0.0;
    //RTK获得的经纬高坐标要转换为东北天坐标，才能用于局部的导航和定位
    //东北天（ENU）坐标系
    //U轴垂直于过原点的参 考椭球面，指向天顶；N与U轴相互垂直，N轴指向为参考椭球短半轴；E 轴完成左手系。
    double local_E = 0.0;
    double local_N = 0.0;
    double local_U = 0.0;
    int status = 0;
    int service = 0;

  private:
    static GeographicLib::LocalCartesian geo_converter;
    static bool origin_position_inited;

  public: 
    void InitOriginPosition();
    void UpdateXYZ();
};
}

 GNSS坐标系转化实现模块

---

//静态成员变量必须在类外初始化
bool lidar_localization::GNSSData::origin_position_inited = false;
//经纬度原点初始化
GeographicLib::LocalCartesian lidar_localization::GNSSData::geo_converter;

namespace lidar_localization {

void GNSSData::InitOriginPosition() {
    //经纬度原点初始化
    //Reset 函数的作用是重置原点，LocalCartesian构造函数是默认在(0,0,0)也就是地心
    geo_converter.Reset(latitude, longitude, altitude);
    origin_position_inited = true;
}

void GNSSData::UpdateXYZ() {
    if (!origin_position_inited) {
        LOG(WARNING) << "GeoConverter has not set origin position";
    }
    //经纬度转ENU
    //Forward (lat, lon, alt, x, y, z)函数就是把经纬高转换为ENU，前三个传入，后三个传出
    geo_converter.Forward(latitude, longitude, altitude, local_E, local_N, local_U);
}
}

 imu定义模块

namespace lidar_localization {
class IMUData {
  public:
  //线速度
    struct LinearAcceleration {
      double x = 0.0;
      double y = 0.0;
      double z = 0.0;
    };
  //角速度
    struct AngularVelocity {
      double x = 0.0;
      double y = 0.0;
      double z = 0.0;
    };
  //角度
    struct Orientation {
      double x = 0.0;
      double y = 0.0;
      double z = 0.0;
      double w = 0.0;
    };

    double time = 0.0;
    LinearAcceleration linear_acceleration;
    AngularVelocity angular_velocity;
    Orientation orientation;
  
  public:
    // 把四元数转换成旋转矩阵送出去
    Eigen::Matrix3f GetOrientationMatrix() {
      Eigen::Quaterniond q(orientation.w, orientation.x, orientation.y, orientation.z);
      Eigen::Matrix3f matrix = q.matrix().cast<float>();

      return matrix;
    }
};
}

tf坐标变换姿态变换模块

---

namespace lidar_localization {
TFListener::TFListener(ros::NodeHandle& nh, std::string base_frame_id, std::string child_frame_id) 
    :nh_(nh), base_frame_id_(base_frame_id), child_frame_id_(child_frame_id) {
}

bool TFListener::LookupData(Eigen::Matrix4f& transform_matrix) {
    try {
        tf::StampedTransform transform;
        //ros::Time(0):查找最近两个时间坐标，计算坐标关系
        //child_frame_id_相对于base_frame_id_的坐标关系
        listener_.lookupTransform(base_frame_id_, child_frame_id_, ros::Time(0), transform);
        //姿态关系转化成4x4旋转矩阵
        TransformToMatrix(transform, transform_matrix);
        return true;
    } catch (tf::TransformException &ex) {
        return false;
    }
}

bool TFListener::TransformToMatrix(const tf::StampedTransform& transform, Eigen::Matrix4f& transform_matrix) {
    //定义平移变量
    Eigen::Translation3f tl_btol(transform.getOrigin().getX(), transform.getOrigin().getY(), transform.getOrigin().getZ());
    
    double roll, pitch, yaw;
    //定义旋转变量
    tf::Matrix3x3(transform.getRotation()).getEulerYPR(yaw, pitch, roll);
    Eigen::AngleAxisf rot_x_btol(roll, Eigen::Vector3f::UnitX());
    Eigen::AngleAxisf rot_y_btol(pitch, Eigen::Vector3f::UnitY());
    Eigen::AngleAxisf rot_z_btol(yaw, Eigen::Vector3f::UnitZ());

    // 此矩阵为 child_frame_id 到 base_frame_id 的转换矩阵 得到4x4旋转矩阵
    transform_matrix = (tl_btol * rot_z_btol * rot_y_btol * rot_x_btol).matrix();

    return true;
}
}