slam定位学习笔记(一)

主要是跟着知乎大佬任乾的文章学习，感谢大佬无私奉献。

本篇文章主要是为设计的slam系统搭建一个框架，主要部分是编写了接收模块、发送模块以及对应的算法接口。

接收模块：具体包括bag包中的GNSS信息、IMU信息、LIDAR点云信息和各传感器之间的标定信息。

发送模块：发送当前帧的点云、全局地图、局部地图、里程计信息还有运动轨迹。

接口：具体就是要设计合理的数据结构，能够把算法需要的输入信息和算法的输出信息条例合理地规划好，以使输入输出更清晰方便。

一、CMakeLists.txt部分

这篇文章里面最让我受益匪浅的是关于CMakeLists.txt文件的编写，十分简洁，将多数库的调用都单独写了一份.cmake文件，然后直接调用.cmake文件就行了，接下来进行分析学习。

cmake_minimum_required(VERSION 2.8.3)
project(lidar_localization)

SET(CMAKE_BUILD_TYPE "Release")
SET(CMAKE_CXX_FLAGS_RELEASE "$ENV{CXXFLAGS} -O3 -Wall")
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 "")

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()


file(GLOB_RECURSE ALL_SRCS "*.cpp")
file(GLOB_RECURSE NODE_SRCS "src/*_node.cpp")
file(GLOB_RECURSE THIRD_PARTY_SRCS "third_party/*.cpp")
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})

SET(CMAKE_BUILD_TYPE "Release")

SET(CMAKE_CXX_FLAGS_RELEASE "$ENV{CXXFLAGS} -O3 -Wall")

这两行是对于CMAKE指定构建类型。详细理解可以参考：文章一，文章二

设置了Release就是不包括调试模式，具体的解释是：

Release：用于构建的优化的库或可执行文件，不包含调试符号。

第二行是常用的配置设置：

set(CMAKE_CXX_FLAGS_RELEASE "$ENV{CXXFLAGS} -O3 -Wall")            # 启用优化（1～3）

不太明白，但目前而言不是重点。

add_compile_options(-std=c++11)
add_definitions(-std=c++11)

主要是添加编译选项，因为前面的CMAKE_CXX_FLAGS_RELEASE是C++所以这里需要确定c++的版本为11，有时候也是14或者17标准。

详细阅读可以参考：文章一、文章二

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  pcl_ros
  geometry_msgs
  tf
  eigen_conversions
)

find_package()命令，主要是为了查找依赖包。

详细阅读可以参考：文章

set(ALL_TARGET_LIBRARIES "")

这里是设置了一个ALL_TARGET_LIBRARIES的变量，但是空的，稍后添加具体的值。

include(cmake/glog.cmake)
include(cmake/PCL.cmake)
include(cmake/eigen.cmake)
include(cmake/geographic.cmake)

include指令一般用于语句的复用，也就是说，如果有一些语句需要在很多CMakeLists.txt文件中使用，为避免重复编写，可以将其写在.cmake文件中，然后在需要的CMakeLists.txt文件中进行include操作就行了。

详细阅读可以参考：文章

include_directories(include ${catkin_INCLUDE_DIRS})

include_directories(include ${catkin_INCLUDE_DIRS})

将指定目录添加到编译器的头文件搜索路径之下，指定的目录被解释成当前源码路径的相对路径。

详细阅读可以参考：文章

catkin_package()

是为catkin提供宏功能，用于为catkin提供构建、生成pkg-config和CMake文件所需要的信息。

详细阅读可以参考：文章

file(GLOB_RECURSE ALL_SRCS "*.cpp")
file(GLOB_RECURSE NODE_SRCS "src/*_node.cpp")
file(GLOB_RECURSE THIRD_PARTY_SRCS "third_party/*.cpp")

file(GLOB_RECURSE ALL_SRCS "*.cpp")

这里的意思是搜索所有的.cpp文件，在将它们保存在ALL_SRCS里面，搜索的位置是想对于cmakelists.txt文件的位置而言。

详细阅读可以参考：文章一、文章二

list(REMOVE_ITEM ALL_SRCS ${NODE_SRCS})
list(REMOVE_ITEM ALL_SRCS ${THIRD_PARTY_SRCS})

主要使用list命令来对与ALL_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})

这里就是很常规的添加执行文件及为添加的执行文件链接库。

有些设置了很多单独的cpp文件就需要，使用第三个参数${ALL_SRCS}这样，来保证生成的一些数据结构的源码可以使用。

另为还是cmake文件里面对于不同库的配置统一在里以它们库名称命名的.cmake后缀文件里面

 里面的内容也是对于库的配置文件，上面说的比较详细了，主要目的是起到代码整洁的作用。

 二、关于所使用的基本传感器数据类型的设计

主要是sensor_data部分，头文件放在了include/lidar_localization/sensor_data里面，原文件放在了src/sensor_data里面。主要有三个基本传感器数据类型：cloud_data、gnss_data、imu_data。具体分析：

对于cloud_data:

cloud_data.h

#ifndef LIDAR_LOCALIZATION_SENSOR_DATA_CLOUD_DATA_HPP_
#define LIDAR_LOCALIZATION_SENSOR_DATA_CLOUD_DATA_HPP_

#include 
#include 

namespace lidar_localization {
class CloudData {
  public:
    using POINT = pcl::PointXYZ;
    using CLOUD = pcl::PointCloud;
    using CLOUD_PTR = CLOUD::Ptr;

  public:
    CloudData()
      :cloud_ptr(new CLOUD()) {
    }

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

#endif

使用了class设定相应的数据结构，其中使用using命令来引用相关的关键字，可以使代码简洁。主要内容是定义了一个CloudData的类，类中有两个数据time和cloud_ptr，然后构造函数使用new对于cloud_ptr初始化。

关于imu_data.h

#ifndef LIDAR_LOCALIZATION_SENSOR_DATA_IMU_DATA_HPP_
#define LIDAR_LOCALIZATION_SENSOR_DATA_IMU_DATA_HPP_

#include 

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();

      return matrix;
    }
};
}
#endif

设置了IMU_DATA的类，其中包含了三个数据结构分别是LinearAcceleration、AngularVelocity和Orientation。这样的设定可以做到在生成对象的时候可以xxx.linear_accleration.x这样。类中还有一个函数GetOrientationMatrix()它里面使用了Eigen库里面的四元数构造函数，可以将orientation的4个值赋予q，最后使用成员函数q.matrix().cast()将其转化成3*3的矩阵。

关于gmss_data.hpp

#ifndef LIDAR_LOCALIZATION_SENSOR_DATA_GNSS_DATA_HPP_
#define LIDAR_LOCALIZATION_SENSOR_DATA_GNSS_DATA_HPP_

#include 
#include 

#include "Geocentric/LocalCartesian.hpp"

using std::vector;
using std::string;

namespace lidar_localization {
class GNSSData {
  public:
    double time = 0.0;
    double longitude = 0.0;
    double latitude = 0.0;
    double altitude = 0.0;
    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();
};
}
#endif

设置了一个时间戳、相关的地点信息还有一个status、service标志，两个私有变量，两个成员函数InitOriginPosition()，UpdateXYZ()。

关于gnss_data.cpp

#include "lidar_localization/sensor_data/gnss_data.hpp"

#include "glog/logging.h"

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

namespace lidar_localization {

void GNSSData::InitOriginPosition() {
    // 初始化
    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";
    }
    // 将latitude, longitude, altitude转化成local_E, local_N, local_U上
    geo_converter.Forward(latitude, longitude, altitude, local_E, local_N, local_U);
}
}

这里比较重要的是使用GeographicLib::LocalCartesian的对象来初始化latitude, longitude, altitude的位置数据。然后转换完成后使用UpdateXYZ()将latitude, longitude, altitude转化成local_E, local_N, local_U上。这是一个东北天坐标系统。

sensor_data主要包含了这三个，主要是为了之后的subscribe和publish的源码提供数据格式解析及处理的数据格式。

三、订阅kitti发布的基本数据格式

关于subscriber部分也是分为include/lidar_loccalization/subscriber里面的头文件和src/subscriber里面的源文件。

关于cloud_subscriber.hpp

#ifndef LIDAR_LOCALIZATION_SUBSCRIBER_CLOUD_SUBSCRIBER_HPP_
#define LIDAR_LOCALIZATION_SUBSCRIBER_CLOUD_SUBSCRIBER_HPP_

#include 

#include 
#include 
#include 
#include 
#include 

#include "lidar_localization/sensor_data/cloud_data.hpp"

namespace lidar_localization {
class CloudSubscriber {
  public:
    CloudSubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size);
    CloudSubscriber() = default;
    void ParseData(std::deque& deque_cloud_data);

  private:
    void msg_callback(const sensor_msgs::PointCloud2::ConstPtr& cloud_msg_ptr);

  private:
    ros::NodeHandle nh_;
    ros::Subscriber subscriber_;

    std::deque new_cloud_data_;
};
}

#endif

使用了类的构造函数形式来完成ros初始化和订阅的初始化。三个私有成员变量，分别是ros::NodeHandle nh_，ros::Subscriber subscriber_，

std::deque new_cloud_data_一个点云数据的队列。

一个私有的成员函数msg_callback(const sensor_msgs::PointCloud2::ConstPtr& cloud_msg_ptr)，处理订阅到的点云数据，接受数据格式为点云指针，我发现所有的这个callback函数的参数都是指针的形式。

关于cloud_subscriber.cpp

#include "lidar_localization/subscriber/cloud_subscriber.hpp"

#include "glog/logging.h"

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);
}

void CloudSubscriber::msg_callback(const sensor_msgs::PointCloud2::ConstPtr& cloud_msg_ptr) {
    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);
}

void CloudSubscriber::ParseData(std::deque& 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

是对构造函数、成员函数的定义。

构造函数还有对于订阅subscriber函数进行地定义，msg_callback()函数先创建空cloud_data对象然后用msg_callback()函数里面的参数进行赋值，使用pcl::fromROSMsg()函数将第一个参数赋值给第二个参数，然后将cloud_data送入之前的队列里面。ParseData函数主要将上一个函数中点云队列的数据放入第一个参数中然后将队列清零。

关于gnss_subscriber.hpp

#ifndef LIDAR_LOCALIZATION_SUBSCRIBER_GNSS_SUBSCRIBER_HPP_
#define LIDAR_LOCALIZATION_SUBSCRIBER_GNSS_SUBSCRIBER_HPP_

#include 
#include 
#include "sensor_msgs/NavSatFix.h"
#include "lidar_localization/sensor_data/gnss_data.hpp"

namespace lidar_localization {
class GNSSSubscriber {
  public:
    GNSSSubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size);
    GNSSSubscriber() = default;
    void ParseData(std::deque& deque_gnss_data);

  private:
    void msg_callback(const sensor_msgs::NavSatFixConstPtr& nav_sat_fix_ptr);

  private:
    ros::NodeHandle nh_;
    ros::Subscriber subscriber_;

    std::deque new_gnss_data_;
};
}
#endif

和cloud_subscriber.hpp相似

关于gnss_subscriber.cpp

#include "lidar_localization/subscriber/gnss_subscriber.hpp"

#include "glog/logging.h"

namespace lidar_localization {
GNSSSubscriber::GNSSSubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size) 
    :nh_(nh) {
    subscriber_ = nh_.subscribe(topic_name, buff_size, &GNSSSubscriber::msg_callback, this);
}

void GNSSSubscriber::msg_callback(const sensor_msgs::NavSatFixConstPtr& nav_sat_fix_ptr) {
    GNSSData gnss_data;
    gnss_data.time = nav_sat_fix_ptr->header.stamp.toSec();
    gnss_data.latitude = nav_sat_fix_ptr->latitude;
    gnss_data.longitude = nav_sat_fix_ptr->longitude;
    gnss_data.altitude = nav_sat_fix_ptr->longitude;
    gnss_data.status = nav_sat_fix_ptr->status.status;
    gnss_data.service = nav_sat_fix_ptr->status.service;

    new_gnss_data_.push_back(gnss_data);
}

void GNSSSubscriber::ParseData(std::deque& gnss_data_buff) {
    if (new_gnss_data_.size() > 0) {
        gnss_data_buff.insert(gnss_data_buff.end(), new_gnss_data_.begin(), new_gnss_data_.end());
        new_gnss_data_.clear();
    }
}
}

和cloud_subscriber.cpp结构类似。

关于imu_subscriber.hpp和imu_subscriber.cpp都是结构类似。

总结一下，订阅不同的数据结构subscriber部分的类的设计结构都是相同的，首先是一个构造函数，然后构造函数的参数分别是ros句柄，话题名，接受长度。然后成员函数就是对于相关传感器数据队列的处理，私有函数是subscriber里面callback函数私有变量是句柄，subscriber和相关的队列。

四、发布处理后的数据格式

也是头文件和源文件分离的形式。

关于cloud_publisher.hpp:

#ifndef LIDAR_LOCALIZATION_PUBLISHER_CLOUD_PUBLISHER_HPP_
#define LIDAR_LOCALIZATION_PUBLISHER_CLOUD_PUBLISHER_HPP_

#include 
#include 
#include 
#include 

#include "lidar_localization/sensor_data/cloud_data.hpp"

namespace lidar_localization {
class CloudPublisher {
  public:
    CloudPublisher(ros::NodeHandle& nh,
                   std::string topic_name,
                   size_t buff_size,
                   std::string frame_id);
    CloudPublisher() = default;
    void Publish(CloudData::CLOUD_PTR cloud_ptr_input);
  
  private:
    ros::NodeHandle nh_;
    ros::Publisher publisher_;
    std::string frame_id_;
};
} 
#endif

私有变量有句柄，发布类生成的对象，和发布的名称，构造函数共有四个参数，第一个参数是句柄，第二个是发布的话题名称，这个是自己命名，第三个参数是发布的长度，第四个是发布点云的名字。还有一个成员函数，主要的作用就是发布数据。

cloud_publisher.cpp:

#include "lidar_localization/publisher/cloud_publisher.hpp"

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) {
    publisher_ = nh_.advertise(topic_name, buff_size);
}

void CloudPublisher::Publish(CloudData::CLOUD_PTR  cloud_ptr_input) {
    sensor_msgs::PointCloud2Ptr cloud_ptr_output(new sensor_msgs::PointCloud2());
    pcl::toROSMsg(*cloud_ptr_input, *cloud_ptr_output);
    cloud_ptr_output->header.stamp = ros::Time::now();
    cloud_ptr_output->header.frame_id = frame_id_;
    publisher_.publish(*cloud_ptr_output);
}

} // namespace data_output

对于发布函数进行了定义，Publish()的参数是指向点云的指针，函数中生成了ros中点云格式的对象cloud_ptr_output,然后调用了pcl::toROSMsg(),这个函数的作用是将pcl点云格式的点云赋值给ros格式的点云，然后将ros格式的点云设置时间戳，和点云名称。最后在使用publisher_.publish()发布出去。主要作用是将pcl点云数据转换成ros格式的点云数据然后在发布出去。

关于odometry_publisher.hpp:

#ifndef LIDAR_LOCALIZATION_PUBLISHER_ODOMETRY_PUBLISHER_HPP_
#define LIDAR_LOCALIZATION_PUBLISHER_ODOMETRY_PUBLISHER_HPP_

#include 

#include 
#include 
#include 

namespace lidar_localization {
class OdometryPublisher {
  public:
    OdometryPublisher(ros::NodeHandle& nh, 
                      std::string topic_name, 
                      std::string base_frame_id,
                      std::string child_frame_id,
                      int buff_size);
    OdometryPublisher() = default;

    void Publish(const Eigen::Matrix4f& transform_matrix);

  private:
    ros::NodeHandle nh_;
    ros::Publisher publisher_;
    nav_msgs::Odometry odometry_;
};
}
#endif

关于发布里程计类的头文件设计，三个私有对象，ros句柄、ros发布对象和里程计类生成对象。构造函数中第一个参数是句柄，第二个是话题名，第三个是基类名称，第四个是子名称

，第五个是队列长度。一个成员函数Publish()参数是4*4的T矩阵。

关于Odometry_publisher.cpp:

#include "lidar_localization/publisher/odometry_publisher.hpp"

namespace lidar_localization {
OdometryPublisher::OdometryPublisher(ros::NodeHandle& nh, 
                                     std::string topic_name, 
                                     std::string base_frame_id,
                                     std::string child_frame_id,
                                     int buff_size)
    :nh_(nh) {

    publisher_ = nh_.advertise(topic_name, buff_size);
    odometry_.header.frame_id = base_frame_id;
    odometry_.child_frame_id = child_frame_id;
}

void OdometryPublisher::Publish(const Eigen::Matrix4f& transform_matrix) {
    odometry_.header.stamp = ros::Time::now();

    //set the position
    odometry_.pose.pose.position.x = transform_matrix(0,3);
    odometry_.pose.pose.position.y = transform_matrix(1,3);
    odometry_.pose.pose.position.z = transform_matrix(2,3);

    Eigen::Quaternionf q;
    q = transform_matrix.block<3,3>(0,0);
    odometry_.pose.pose.orientation.x = q.x();
    odometry_.pose.pose.orientation.y = q.y();
    odometry_.pose.pose.orientation.z = q.z();
    odometry_.pose.pose.orientation.w = q.w();

    publisher_.publish(odometry_);
}
}

构造函数完成了对于几个私有变量的初始化，其中对于发布对象的设置是重点，然后将基名称和子名称分别赋值给了里程计的对应量。对于Publish()函数参数为T矩阵然后将私有变量，里程计赋值当前时间戳，位姿中的t为T矩阵的最后一列的前三行，使用了Eigen库的四元数构造函数，将R部分转化为四元数q，然后分别赋值给里程计的旋转部分。最后发布出去。

分别对比了发布的这两个cloud_publisher和Odometry_publisher发现它们的类设计大部分的结构都是类似的，但对于构造函数的设计中有一点不同。cloud_publish中将发送的frame_id也放在了构造函数中，而Odomtry_publisher将frame_id和child_frame_id都放进去了，没有把odometry_放进去。刚开始看的时候觉得很奇怪，现在明白了，初始化中把odometry的frame_id和child_frame_id初始化赋值也是对于odometry的初始化。

使用一个命令

rosmsg show nav_msgs/Odometry

可以看出ros中整个Odometry的数据结构：

直接解释了为什么位姿赋值为什么要加两个pose，第一个应该是什么协方差，第二个才是pose。

根据这个命令发现了新大陆，原来这个是一个个数据结构结合起来了。比如输入命令：
 

rosmsg show geometry_msgs/PoseWithCovariance

 原来一层层抽象上去的。

五、TF坐标部分

同样是头文件和源文件分离的模式。

关于tf_listener.cpp:

#ifndef LIDAR_LOCALIZATION_TF_LISTENER_HPP_
#define LIDAR_LOCALIZATION_TF_LISTENER_HPP_

#include 

#include 
#include 
#include 

namespace lidar_localization {
class TFListener {
  public:
    TFListener(ros::NodeHandle& nh, std::string base_frame_id, std::string child_frame_id);
    TFListener() = default;

    bool LookupData(Eigen::Matrix4f& transform_matrix);
  
  private:
    bool TransformToMatrix(const tf::StampedTransform& transform, Eigen::Matrix4f& transform_matrix);

  private:
    ros::NodeHandle nh_;
    tf::TransformListener listener_;
    std::string base_frame_id_;
    std::string child_frame_id_;
};
}

#endif

设定的tf_Listener类私有成员有四个，ros句柄、tf::TransformListener 生成的对象、基类名称和子类名称。构造函数参数也是这个四个。有一个公有函数LookupData()参数是4*4的T矩阵，一个私有函数TransformToMatrix(),第一个参数是tf格式的transform，第二个参数是4*4的T矩阵。

使用命令查看tf格式的transform：

rosmsg show tf/tfMessage

有了这个好理解多了。

关于tf_listener.cpp:
 

#include "lidar_localization/tf_listener/tf_listener.hpp"

#include 

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;
        listener_.lookupTransform(base_frame_id_, child_frame_id_, ros::Time(0), transform);
        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 的转换矩阵
    transform_matrix = (tl_btol * rot_z_btol * rot_y_btol * rot_x_btol).matrix();

    return true;
}
}

构造函数的定义没什么好说的，函数TransformMatrix()第一个参数是tf格式的transform，第二个参数是4×4的T矩阵。函数的主要作用是将第一个参数赋值给第二个参数，其中有将第一个参数提取R矩阵转化为三个欧拉角，roll、pitch、yaw。然后转化为转话为Eigen库中对于旋转轴的参数，最后将tf格式获得的t向量乘以z轴旋转量乘以y轴旋转量乘以x轴旋转量然后变化成4×4矩阵的形式。函数LookupData()参数是4×4的T矩阵，函数的作用是创建tf格式的transform矩阵，然后利用函数tf订阅函数，listen_.lookupTransform()来获取从child_frame_id到base_frame_id的矩阵，这个矩阵的类型是tf类型的。注意它的参数的位置，是参数2到参数1的变换。

关于这个函数的使用，我之前专门写了一篇学习笔记。

简单来说，这个listener类的作用是订阅制定的变换并转换为4×4的T矩阵，方便后续的使用。

六、应用框架节点部分

注：写到这里的时候，突然想起了CMakeLists.txt文件中关于file命令和list命令中把所有的_node.cpp都从ALL_SRCS中去除了，那么最后的生成可执行文件怎么判定来源呢？后来仔细一看在生成可执行文件那里，将_node.cpp直接放在那里，这样可以避免冲突。

关于test_frame_node.cpp:

#include 
#include 
#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"

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, "test_frame_node");
    ros::NodeHandle nh;

    std::shared_ptr cloud_sub_ptr = std::make_shared(nh, "/kitti/velo/pointcloud", 100000);
    std::shared_ptr imu_sub_ptr = std::make_shared(nh, "/kitti/oxts/imu", 1000000);
    std::shared_ptr gnss_sub_ptr = std::make_shared(nh, "/kitti/oxts/gps/fix", 1000000);
    std::shared_ptr lidar_to_imu_ptr = std::make_shared(nh, "velo_link", "imu_link");

    std::shared_ptr cloud_pub_ptr = std::make_shared(nh, "current_scan", 100, "/map");
    std::shared_ptr odom_pub_ptr = std::make_shared(nh, "lidar_odom", "map", "lidar", 100);

    std::deque cloud_data_buff;
    std::deque imu_data_buff;
    std::deque gnss_data_buff;
    Eigen::Matrix4f lidar_to_imu = Eigen::Matrix4f::Identity();
    bool transform_received = false;
    bool gnss_origin_position_inited = 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;
                // LOG(INFO) << "lidar to imu transform matrix is:" << std::endl << lidar_to_imu;
            }
        } 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();

                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;

                    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;

                    pcl::transformPointCloud(*cloud_data.cloud_ptr, *cloud_data.cloud_ptr, odometry_matrix);

                    cloud_pub_ptr->Publish(cloud_data.cloud_ptr);
                    odom_pub_ptr->Publish(odometry_matrix);
                }
            }
        }

        rate.sleep();
    }
    return 0;
}

使用智能指针来创建类指针，创建了点云、IMU、gnss类指针，创建了TF订阅类指针，这里有一点疑问，指针的名字是lidar_to_imu_ptr但从构造函数的参数的位置看，应该是imu_to_lidar_ptr合理一点。为了证明这个疑问，搜索了一些资料，在网上看到了这篇文章，讲的很好，这里借用一下他的思路。

这是kitti数据集采集车设备架设图：

从图中可以看出来IMU在lidar坐标系下的坐标应该是(-0.81, 0.32, -0.80)。

而输出源码中获得的transform：

            if (lidar_to_imu_ptr->LookupData(lidar_to_imu)) {
                transform_received = true;
                LOG(INFO) << "lidar to imu transform matrix is:" << std::endl << lidar_to_imu;
            }

lidar to imu transform matrix is:
00000.999998 00.000755307 0-0.00203583 000-0.808676
-0.000785403 000000.99989 000-0.014823 00000.319556
000.00202441 0000.0148245 00000.999888 000-0.799723
000000000000 000000000000 000000000000 000000000001

发现就是imu_to_lidar_ptr,所以我的感觉正确。

使用智能指针创建了点云与里程计的发布类，创建了传感器数据收集的对列。

进入while循环，然后利用传感器类的成员函数ParseData()来将订阅到的数据转化为ros格式，接下来主要的是对于三个传感器队列数据的处理，处理一个就推出一个。然后就是设定里程计的4×4的矩阵应该是一个T矩阵，它的t向量由gnss提供，旋转部分由imu提供，然后就是重点的代码：

                    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;

关于odometry_matrix *= lidar_to_imu,我是这样理解的，因为imu和gnss都在一个位置，所以odometry_matrix算出来的原点应该也是那个位置，所以乘上imu到lidar的变换矩阵，就可以把odometry_matrix坐标系换算到lidar坐标系中。但想了下这个变换矩阵的命名本来的意思应该是把这个lidar转换到imu坐标系下，因为这个是imu是在世界坐标系下算出的点，所以最后应该去逆。或者在tf_lisener设计构造函数的时候就应该把两个位置交换一下：

纠正一：

std::shared_ptr lidar_to_imu_ptr = std::make_shared(nh, "imu_link", "lidar_link");

纠正二：

                    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.inverse();

最后是利用得到的里程计变换矩阵将每一帧的点云坐标变换，这样在rviz中显示的就是在gnss运行时，有点云在之上。然后将点云和里程计发布出去。值得注意的是发布的是odometry不是4×4的矩阵，Publish函数的作用是将4×4的矩阵的值赋给odometry_变量。

世界坐标系一直是gnss传感器获得。

终于写完了，接下来，准备运用这篇笔记中学到的技巧，订阅一个rosbag中的点云数据和imu数据，转化为里程计格式发布。