autoware下ndt_mapping节点解读

利用激光雷达进行建图，首先需要得到稠密点云，然后进行体素滤波进行过滤得到包含特征的点云数据。接着利用每一帧扫描的点云地图进行ndt配准逐帧拼接，最后能够得到激光雷达扫描路径下的整体点云地图。

ndt_mapping

首先看ndt_matching的launch文件，设定相关参数，并且运行ndt_mapping以及queue_counter节点。

<!-- -->
<launch>
  <arg name="method_type" default="0" /> <!-- pcl_generic=0, pcl_anh=1, pcl_anh_gpu=2, pcl_openmp=3 -->
  <arg name="use_odom" default="false" />
  <arg name="use_imu" default="false" />
  <arg name="imu_upside_down" default="false" />
  <arg name="imu_topic" default="/imu_raw" />
  <arg name="incremental_voxel_update" default="false" />
  <!-- rosrun lidar_localizer ndt_mapping  -->
  <node pkg="lidar_localizer" type="queue_counter" name="queue_counter" output="screen"/>
  <node pkg="lidar_localizer" type="ndt_mapping" name="ndt_mapping" output="screen">
    <param name="method_type" value="$(arg method_type)" />
    <param name="use_imu" value="$(arg use_imu)" />
    <param name="use_odom" value="$(arg use_odom)" />
    <param name="imu_upside_down" value="$(arg imu_upside_down)" />
    <param name="imu_topic" value="$(arg imu_topic)" />
    <param name="incremental_voxel_update" value="$(arg incremental_voxel_update)" />
  </node>
</launch>

2：然后关注main函数部分
main函数首先对设定的各种位置进行了初始化，previous_pose表示前一帧点云图车辆的位置，current_pose表示当前帧点云图车辆的位置，ndt_pose表示使用ndt匹配算法，guess_pose表示提供的初始位置，diff表示前后两帧的位置与角度变化，offset表示位置与角度的偏差矫正。

previous_pose.x = 0.0;
  previous_pose.y = 0.0;
  previous_pose.z = 0.0;
  previous_pose.roll = 0.0;
  previous_pose.pitch = 0.0;
  previous_pose.yaw = 0.0;

  ndt_pose.x = 0.0;
  ndt_pose.y = 0.0;
  ndt_pose.z = 0.0;
  ndt_pose.roll = 0.0;
  ndt_pose.pitch = 0.0;
  ndt_pose.yaw = 0.0;

  current_pose.x = 0.0;
  current_pose.y = 0.0;
  current_pose.z = 0.0;
  current_pose.roll = 0.0;
  current_pose.pitch = 0.0;
  current_pose.yaw = 0.0;

  current_pose_imu.x = 0.0;
  current_pose_imu.y = 0.0;
  current_pose_imu.z = 0.0;
  current_pose_imu.roll = 0.0;
  current_pose_imu.pitch = 0.0;
  current_pose_imu.yaw = 0.0;

  guess_pose.x = 0.0;
  guess_pose.y = 0.0;
  guess_pose.z = 0.0;
  guess_pose.roll = 0.0;
  guess_pose.pitch = 0.0;
  guess_pose.yaw = 0.0;

  added_pose.x = 0.0;
  added_pose.y = 0.0;
  added_pose.z = 0.0;
  added_pose.roll = 0.0;
  added_pose.pitch = 0.0;
  added_pose.yaw = 0.0;

  diff_x = 0.0;
  diff_y = 0.0;
  diff_z = 0.0;
  diff_yaw = 0.0;

  offset_imu_x = 0.0;
  offset_imu_y = 0.0;
  offset_imu_z = 0.0;
  offset_imu_roll = 0.0;
  offset_imu_pitch = 0.0;
  offset_imu_yaw = 0.0;

  offset_odom_x = 0.0;
  offset_odom_y = 0.0;
  offset_odom_z = 0.0;
  offset_odom_roll = 0.0;
  offset_odom_pitch = 0.0;
  offset_odom_yaw = 0.0;

  offset_imu_odom_x = 0.0;
  offset_imu_odom_y = 0.0;
  offset_imu_odom_z = 0.0;
  offset_imu_odom_roll = 0.0;
  offset_imu_odom_pitch = 0.0;
  offset_imu_odom_yaw = 0.0;

通过/ndt_map话题将点云建图发布出来，以及通过回调函数订阅建图，里程计以及imu信息。

ndt_map_pub = nh.advertise<sensor_msgs::PointCloud2>("/ndt_map", 1000);//
  current_pose_pub = nh.advertise<geometry_msgs::PoseStamped>("/current_pose", 1000);
  ros::Subscriber param_sub = nh.subscribe("config/ndt_mapping", 10, param_callback);
  ros::Subscriber output_sub = nh.subscribe("config/ndt_mapping_output", 10, output_callback);
  ros::Subscriber points_sub = nh.subscribe("points_raw", 100000, points_callback);
  ros::Subscriber odom_sub = nh.subscribe("/vehicle/odom", 100000, odom_callback);
  ros::Subscriber imu_sub = nh.subscribe(_imu_topic, 100000, imu_callback);

定义了结构体表示位置，枚举使用的方法类型，方法类型可以在使用autoware的时候进行选择。

struct pose//定义结构体用来表示位置
{
   
   
  double x;
  double y;
  double z;
  double roll;
  double pitch;
  double yaw;
};
enum class MethodType//计算方法类型
{
   
   
  PCL_GENERIC = 0,
  PCL_ANH = 1,
  PCL_ANH_GPU = 2,
  PCL_OPENMP = 3,
};

参数回调函数利用ConfigNDTMapping文件进行参数设置，设置参数有ndt分辨率，步长，最大迭代次数，体素叶大小，最大最小扫描范围(用来过滤距离车辆较近以及较远的点云数据)

static void param_callback(const autoware_config_msgs::ConfigNDTMapping::ConstPtr& input)
{
   
   
  ndt_res = input->resolution;
  step_size = input->step_size;
  trans_eps = i