自动驾驶系统进阶与项目实战（六）基于NDT的自动驾驶高精度定位和ROS项目实战
摘要
本文介绍了基于ROS框架的自动驾驶高精度定位技术及其实现方法。重点阐述了利用Normal Distributions Transform（NDT）算法结合IMU、轮速计和激光雷达数据完成车辆高精度定位的过程。
核心内容：
NDT算法基础
NDT是一种基于概率分布的目标函数优化算法，在自动驾驶中用于将激光雷达采集的数据与预先构建的地图进行配准。
数据预处理
使用SC-LEGO-LOAM方法构建较大的城区三维地图，并通过LOAM优化提升数据质量。
关键步骤
- 降采样处理：使用VoxelGrid降采样技术降低输入点云密度。
- 初始化与配准：通过Rviz手动指定初始姿态或利用线性模型预测估计值。
- 实时平移计算：结合IMU和轮速计数据计算车辆平移量。
工具链与配置- 使用ros::NodeHandler管理ROS节点启动与停止。
- 配置tf2_ros实现静态变换发布功能。
- 通过point cloud message conversion实现PCL与ROS之间的数据交换。
代码实现- map_loader负责接收并处理点云数据并将其转换为特定格式的消息。
- transform_map用于初始化地图并建立坐标系间的变换关系。
- points_downsample对输入点云进行降采样以提高效率。
- ndt_localizer实现了关键的NDT配准逻辑。
实现意义
该技术方案能够高效地结合多种传感器数据（包括高分辨率地图），并在实时性要求较高的自动驾驶场景下提供可靠的位置估计服务（L4级别）。
自动驾驶系统进阶与项目实战（六）：以NDT为基础的自动驾驶高精度定位及ROS开发实践
对于高级自动驾驶系统而言，在定位模块中通常会整合 IMU、车速传感器（底盘设备）和激光雷达 odometry 等多种数据源，并运用滤波算法（EKF、UKF 等）对数据进行融合处理，以实现平滑且精确到厘米级的绝对定位。其中，在融合定位过程中具有显著优势的是基于高精度点云地图与激光雷达配准定位（Lidar Odometry），其特点在于定位精度高且可靠性强，在整体融合定位体系中占据重要比重，在自动驾驶领域可被视为较为可靠的一种“绝对定位”数据来源。本文旨在探讨如何通过 NDT 配准方法实现自动驾驶汽车的高精度定位，并结合之前介绍的 SC-LEGO-LOAM 生成的点云地图实践单站 NDT 算法的应用场景，在 ROS 平台搭建相关项目的基础上完成本文内容的学习与实践。通过本篇文章的学习与实践操作后，请您能够完成如图所示的点云配准定位任务。
前言
在之前的综述文章（无人驾驶技术入门系列第13篇及自动驾驶进阶应用第2篇）中对NDT算法及其点云配准技术进行了全面阐述。然而，在实际应用中尚未有一个完整的解决方案来实现基于点云地图的NDT定位技术。本文将深入探讨这一技术，并提供一个简洁明了的基于NDT算法的定位实现方案。
NDT定位作为Autoware自动驾驶开源项目的主定位算法，在实际应用中发现其模块间耦合度较高。若仅用于学习激光雷达配准定位技术，则需要自行编译完整套源码进行测试。值得注意的是，在Autoware 1.x版本中所采用的NDT算法采用了基于过程式编程框架实现，并且由于其代码结构较为复杂难以阅读和理解。因此，在本文中我们将深入研究并重构这一技术思路，在不依赖原有复杂库的情况下开发一个结构清晰、代码易读的ROS节点（即为N-Dimensions Targeting Ros）。如果你觉得这篇介绍对你有所帮助的话，请别忘了点赞支持！
点云地图准备
自动驾驶汽车的主要定位方式主要基于在离线环境下预先生成了具有高度精确性的三维空间数据模型。主要原因在于这样做能够显著提升定位系统的稳定性和准确性，在复杂的动态环境中也能保持高效的运行效率。
- L4级及以上自动驾驶系统对定位精度与稳定性的要求较高，在线制图与精确定位需确保绝对误差控制在20厘米以内；
- 纯SLAM技术目前尚无法满足自动驾驶系统的定位精度及可靠性要求, 即使采用现有研究方法, 在线实时完成高精度地图构建仍然面临诸多挑战（如闭环优化、全局优化及误差累积修正等问题）；
- 构建高质量的地图数据不仅需要强大的计算能力还需要大量的人力投入, 而现有的解决方案多集中于离线生成阶段（涉及时间和人力投入两个关键环节）；
- 通过结合IMU与轮速计的数据能显著提升激光雷达在车辆自主导航中的精确定位可靠性。
基于以上客观因素分析，在现有的L4级及大部分L3级自动驾驶系统定位模块中仍以先期构建的高精度地图为基础完成配准定位。该配准所采用的传感器中，少数厂商采用的是基于camera技术（如MobileEye）。然而，在当前技术背景下，默认情况下大多数厂商仍采用激光雷达作为基础实现配准定位的技术路径。值得注意的是，在这种技术架构下构建出来的点云地图本质上就是激光雷达配准所需的基础性数据支持——即预先构建好的"用于实现位置确定的地图"。
在《自动驾驶系统进阶与项目实战五》这篇文章里，我们通过SC-LEGO-LOAM方法生成了一套相对较大的点云数据，并运用Scan Context技术完成了闭环检测并对姿态图进行了优化。接下来，在本文中我们将采用上篇文章产生的点云数据作为我们的定位地图。
从上一篇文章中生成的pcd文件（为了方便那些不愿动手的同学，请您直接访问文末提供的链接即可下载该pcd文件）会被复制至本项目的位于[仓库地址]的map目录中，并以kaist02.pcd命名）。其中MapLoader节点用于地图加载。
MapLoader::MapLoader(ros::NodeHandle &nh){
std::string pcd_file_path, map_topic;
nh.param<std::string>("pcd_path", pcd_file_path, "");
nh.param<std::string>("map_topic", map_topic, "point_map");
init_tf_params(nh);
pc_map_pub_ = nh.advertise<sensor_msgs::PointCloud2>(map_topic, 10, true);
file_list_.push_back(pcd_file_path);
auto pc_msg = CreatePcd();
auto out_msg = TransformMap(pc_msg);
if (out_msg.width != 0) {
out_msg.header.frame_id = "map";
pc_map_pub_.publish(out_msg);
}
}搭建一个函数解析器来处理PCD文件的路径信息以及对应的"地图主题"数据，并根据需要初始化地图坐标系转换参数字段（如果无需对地图进行坐标系转换操作，则将所有坐标系转换参数字段设为零值）。将上述初始化后的转换参数值配置并保存到名为"map_load.launch"的任务启动脚本文件中。
<launch>
<arg name="roll" default="0.0" />
<arg name="pitch" default="0.0" />
<arg name="yaw" default="0.0" />
<arg name="x" default="0.0" />
<arg name="y" default="0.0" />
<arg name="z" default="0.0" />
<!--- Run Rviz-->
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find ndt_localizer)/cfgs/rock-auto.rviz" />
<!--- MapLoader -->
<arg name="pcd_path" default="$(find ndt_localizer)/map/kaist02.pcd"/>
<arg name="map_topic" default="/points_map"/>
<node pkg="ndt_localizer" type="map_loader" name="map_loader" output="screen">
<param name="pcd_path" value="$(arg pcd_path)"/>
<param name="map_topic" value="$(arg map_topic)"/>
<param name="roll" value="$(arg roll)" />
<param name="pitch" value="$(arg pitch)" />
<param name="yaw" value="$(arg yaw)" />
<param name="x" value="$(arg x)" />
<param name="y" value="$(arg y)" />
<param name="z" value="$(arg z)" />
</node>
</launch>该函数用于加载pcd文件。通过调用TransformMap()函数进行地图的平移与旋转操作。点云变换操作借助于Eigen库，并调用pcl::transformPointCloud()函数完成。
sensor_msgs::PointCloud2 MapLoader::TransformMap(sensor_msgs::PointCloud2 & in){
pcl::PointCloud<pcl::PointXYZ>::Ptr in_pc(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(in, *in_pc);
pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_pc_ptr(new pcl::PointCloud<pcl::PointXYZ>);
Eigen::Translation3f tl_m2w(tf_x_, tf_y_, tf_z_); // tl: translation
Eigen::AngleAxisf rot_x_m2w(tf_roll_, Eigen::Vector3f::UnitX()); // rot: rotation
Eigen::AngleAxisf rot_y_m2w(tf_pitch_, Eigen::Vector3f::UnitY());
Eigen::AngleAxisf rot_z_m2w(tf_yaw_, Eigen::Vector3f::UnitZ());
Eigen::Matrix4f tf_m2w = (tl_m2w * rot_z_m2w * rot_y_m2w * rot_x_m2w).matrix();
pcl::transformPointCloud(*in_pc, *transformed_pc_ptr, tf_m2w);
SaveMap(transformed_pc_ptr);
sensor_msgs::PointCloud2 output_msg;
pcl::toROSMsg(*transformed_pc_ptr, output_msg);
return output_msg;
}输入点云降采样
该算法的目标函数基于输入点云与目标点云的概率分布相似性；该配准算法的计算复杂度与这两个因素呈正相关关系
- 输入点云的点的密度
- 初始姿态估计的偏差
随着点云密度的升高，在进行NDT配准时所需的计算复杂度将显著增加；当初始姿态偏差越大（即与真实姿态存在较大差异）时，在满足精度要求的前提下所需计算时间也会显著增加。此外，在实际应用中若初始姿态偏离过大会导致NDT算法无法收敛。由于自动驾驶激光雷达定位需要满足较高的实时性要求，在保证定位精度的前提下缩短配准所需时间显得尤为重要。因此，在本研究中我们采用了一种高效的降采样方法——VoxelGrid降采样技术——来降低输入点云的空间密度（具体实现见下文）。该方法的主要实现思路如下：
static void scan_callback(const sensor_msgs::PointCloud2::ConstPtr& input)
{
pcl::PointCloud<pcl::PointXYZ> scan;
pcl::fromROSMsg(*input, scan);
if(measurement_range != MAX_MEASUREMENT_RANGE){
scan = removePointsByRange(scan, 0, measurement_range);
}
pcl::PointCloud<pcl::PointXYZ>::Ptr scan_ptr(new pcl::PointCloud<pcl::PointXYZ>(scan));
pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_scan_ptr(new pcl::PointCloud<pcl::PointXYZ>());
sensor_msgs::PointCloud2 filtered_msg;
// if voxel_leaf_size < 0.1 voxel_grid_filter cannot down sample (It is specification in PCL)
if (voxel_leaf_size >= 0.1)
{
// Downsampling the velodyne scan using VoxelGrid filter
pcl::VoxelGrid<pcl::PointXYZ> voxel_grid_filter;
voxel_grid_filter.setLeafSize(voxel_leaf_size, voxel_leaf_size, voxel_leaf_size);
voxel_grid_filter.setInputCloud(scan_ptr);
voxel_grid_filter.filter(*filtered_scan_ptr);
pcl::toROSMsg(*filtered_scan_ptr, filtered_msg);
}
else
{
pcl::toROSMsg(*scan_ptr, filtered_msg);
}
filtered_msg.header = input->header;
filtered_points_pub.publish(filtered_msg);
}在获取点云数据之后，在此基础上会对数据进行截取处理，并仅选择距离源不超过 MAX_MEASUREMENT_RANGE 米（此处定义为120米）的数据用于定位计算。随后会对获取到的点云数据进行截取处理，并仅选择距离源不超过 MAX_MEASUREMENT_RANGE 米（此处定义为120米）的数据用于定位计算。VoxelGrid降采样过程中的核心参数设定为 voxel_leaf_size ，该参数决定了分割单元体的空间尺度。每个分割后的立方体内仅保留一个代表点，在 points_downsample.launch 文件中可配置此相关参数设置其具体值。
<arg name="leaf_size" default="3.0" />如上所述, 本文采用了3米的leaf size参数, 其具体取值可根据实际应用中使用的激光雷达所探测点的数量来确定. 在追求配准实时性的同时, 我们也不希望过分牺牲定位精度, 因此在设置输入点云的降采样程度时需要在实时性与定位精度之间找到一个恰当的平衡点. 根据经验, 如果您选用的是16线激光雷达设备, 建议将leaf size设定在1至2米之间较为合适. 当激光雷达的数量达到32线及以上时, 可将leaf size参数调整为2至3米.
降采样后的点云会被输出至 /filtered_points 这个子话题中，并用于后续的NDT配准和定位
使用NDT为自动驾驶车提供高精度定位
在本次项目中，我主要负责实现了NDT定位的逻辑，并将其编码到ndt.cpp文件中。接下来我们将对这一模块进行详细分析。
初始姿态获取
所有依赖预先构建的地图进行配准定位的技术都需要提供初始姿态信息。在工业界的实际应用中，通常会通过GNSS（全球导航卫星系统）来获取这一初始姿态。然而，在本文的研究中为了简化流程，在Rviz软件中手动指定初始姿态。需要注意的是，在NDTLocalizer构造函数中编写一个listener节点来监听该主题：在 industrial practice 中, initial pose information is often obtained through gnss receivers, which ensures high accuracy and robustness. However, in this study, to simplify the process, we manually specify the initial pose within Rviz software. The initial pose set by Rviz will be automatically transmitted to the /initialpose topic. Finally, a listener node is implemented within the NDTLocalizer constructor to subscribe to the aforementioned topic.
initial_pose_sub_ = nh_.subscribe("initialpose", 100, &NdtLocalizer::callback_init_pose, this);改写说明
void NdtLocalizer::callback_init_pose(
const geometry_msgs::PoseWithCovarianceStamped::ConstPtr & initial_pose_msg_ptr)
{
if (initial_pose_msg_ptr->header.frame_id == map_frame_) {
initial_pose_cov_msg_ = *initial_pose_msg_ptr;
} else {
// get TF from pose_frame to map_frame
geometry_msgs::TransformStamped::Ptr TF_pose_to_map_ptr(new geometry_msgs::TransformStamped);
get_transform(map_frame_, initial_pose_msg_ptr->header.frame_id, TF_pose_to_map_ptr);
// transform pose_frame to map_frame
geometry_msgs::PoseWithCovarianceStamped::Ptr mapTF_initial_pose_msg_ptr(
new geometry_msgs::PoseWithCovarianceStamped);
tf2::doTransform(*initial_pose_msg_ptr, *mapTF_initial_pose_msg_ptr, *TF_pose_to_map_ptr);
// mapTF_initial_pose_msg_ptr->header.stamp = initial_pose_msg_ptr->header.stamp;
initial_pose_cov_msg_ = *mapTF_initial_pose_msg_ptr;
}
// if click the initpose again, re init！
init_pose = false;
}在NDT配准中，我们主要关注四个坐标系间的变化，分别是：
基于全球的坐标系统（使用world作为其唯一的framer\_id），该系统提供了统一的空间参考基准。
基于地图的地理系统（使用map作为其唯一的framer\_id），该系统主要用于描述地面物体的位置。
针对车辆的基础参考框架（采用base\_link作为其唯一的framer\_id），该系统为车辆运动学建模提供了关键的基础。
本文采用了ouster激光雷达框架作为基准，并依据实际应用需求进行了相应的优化设计。需要注意的是，在实际应用中若更换不同型号的激光雷达，则对应的\texttt{framer\_id}也会相应调整。
在本项目中, 我们通过调用程序static_tf.launch进行操作, 将world数据发送至map节点, 并将ouster数据发送至base_link节点. 这两个固定变换的具体实现细节可在后续章节中找到.
<node pkg="tf2_ros" type="static_transform_publisher" name="localizer_to_base_link" args="0 0 1.9 3.1415926 0 0 base_link ouster"/>
<node pkg="tf2_ros" type="static_transform_publisher" name="world_to_map" args="0 0 0 0 0 0 map world" />为简化问题起见，在实际自动驾驶系统开发过程中我们假设地图和世界坐标系处于同一参考框架内；而在具体的实现过程中则需要基于WGS84坐标系下的经纬度信息，并配合UTM通用横轴墨卡托投影方法来确定当前局部坐标系与世界坐标系之间的平移量及其ENU（East North Up）旋转参数。其中局部izer_to_base_link这一变换关系即激光雷达到base link的转换关系是激光雷达的重要外参参数之一；它属于静态变换范畴。
该变换关系属于激光雷达的重要外参参数之一
完成初始姿态获取后，在获得基于Rviz的手动指定位姿后首先要完成的是对所有空间基准点进行统一配置工作。若当前位姿位于地图坐标系中，则可以直接将其保存以便后续操作使用；但如果当前位姿位于其他非全局基准参考框架中，则需要先将其转换至地图基准下，并通过调用函数 get_transform 确定位姿与地图之间的转换关系。该函数定义如下：
bool NdtLocalizer::get_transform(
const std::string & target_frame, const std::string & source_frame,
const geometry_msgs::TransformStamped::Ptr & transform_stamped_ptr)
{
if (target_frame == source_frame) {
transform_stamped_ptr->header.stamp = ros::Time::now();
transform_stamped_ptr->header.frame_id = target_frame;
transform_stamped_ptr->child_frame_id = source_frame;
transform_stamped_ptr->transform.translation.x = 0.0;
transform_stamped_ptr->transform.translation.y = 0.0;
transform_stamped_ptr->transform.translation.z = 0.0;
transform_stamped_ptr->transform.rotation.x = 0.0;
transform_stamped_ptr->transform.rotation.y = 0.0;
transform_stamped_ptr->transform.rotation.z = 0.0;
transform_stamped_ptr->transform.rotation.w = 1.0;
return true;
}
try {
*transform_stamped_ptr =
tf2_buffer_.lookupTransform(target_frame, source_frame, ros::Time(0), ros::Duration(1.0));
} catch (tf2::TransformException & ex) {
ROS_WARN("%s", ex.what());
ROS_ERROR("Please publish TF %s to %s", target_frame.c_str(), source_frame.c_str());
transform_stamped_ptr->header.stamp = ros::Time::now();
transform_stamped_ptr->header.frame_id = target_frame;
transform_stamped_ptr->child_frame_id = source_frame;
transform_stamped_ptr->transform.translation.x = 0.0;
transform_stamped_ptr->transform.translation.y = 0.0;
transform_stamped_ptr->transform.translation.z = 0.0;
transform_stamped_ptr->transform.rotation.x = 0.0;
transform_stamped_ptr->transform.rotation.y = 0.0;
transform_stamped_ptr->transform.rotation.z = 0.0;
transform_stamped_ptr->transform.rotation.w = 1.0;
return false;
}
return true;
}在获得map的transform之后（简称tf），可以直接调用tf2::doTransform函数完成初始姿态相对于地图坐标的变换。
初始化地图
目标点云即是我们通过SC-LEGO-LOAM生成的点云地图。随后，我们订阅该节点发布的消息，并在此基础上执行如下操作：
void NdtLocalizer::callback_pointsmap(
const sensor_msgs::PointCloud2::ConstPtr & map_points_msg_ptr)
{
const auto trans_epsilon = ndt_.getTransformationEpsilon();
const auto step_size = ndt_.getStepSize();
const auto resolution = ndt_.getResolution();
const auto max_iterations = ndt_.getMaximumIterations();
pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt_new;
ndt_new.setTransformationEpsilon(trans_epsilon);
ndt_new.setStepSize(step_size);
ndt_new.setResolution(resolution);
ndt_new.setMaximumIterations(max_iterations);
pcl::PointCloud<pcl::PointXYZ>::Ptr map_points_ptr(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(*map_points_msg_ptr, *map_points_ptr);
ndt_new.setInputTarget(map_points_ptr);
// create Thread
// detach
pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
ndt_new.align(*output_cloud, Eigen::Matrix4f::Identity());
// swap
ndt_map_mtx_.lock();
ndt_ = ndt_new;
ndt_map_mtx_.unlock();
}其中最为关键的一点是，在获得点云地图之后, 将ndt_new的目标点云设为map_points_ptr, 同时这也是该算法的重要参数之一。
- ndt_newConfigure a transformation epsilon value (trans_epsilon)；该值代表搜索过程中的最小变化量。
- ndt_newConfigure a step size parameter (step_size)；该参数决定了每次迭代所移动的距离。
- ndt_newDefine a resolution parameter (resolution)；该参数表示目标点云中的ND体素尺寸（单位为米）。
- ndt_newSet the maximum number of iterations for optimization using the Newton method (max_iterations)；该参数指定了牛顿法优化的最大迭代次数。
具体参数的作用以及NDT算法的基本理论可查阅我前面的两篇博客自动驾驶系统进阶与项目实战（二）和无人驾驶汽车系统入门（十三）]()
NDT配准定位
配准定位主要实现于以下回调中：
void callback_pointcloud(const sensor_msgs::PointCloud2::ConstPtr & sensor_points_sensorTF_msg_ptr);此回调关注降采样的点云数据，并随后解析来自Point Cloud 2的消息为pcl中的Point Cloud类型。
const std::string sensor_frame = sensor_points_sensorTF_msg_ptr->header.frame_id;
const auto sensor_ros_time = sensor_points_sensorTF_msg_ptr->header.stamp;
boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> sensor_points_sensorTF_ptr(
new pcl::PointCloud<pcl::PointXYZ>);该点云位于激光雷达坐标系下，并随后在base_link坐标系下进行投射操作中。
// get TF base to sensor
geometry_msgs::TransformStamped::Ptr TF_base_to_sensor_ptr(new geometry_msgs::TransformStamped);
get_transform(base_frame_, sensor_frame, TF_base_to_sensor_ptr);
const Eigen::Affine3d base_to_sensor_affine = tf2::transformToEigen(*TF_base_to_sensor_ptr);
const Eigen::Matrix4f base_to_sensor_matrix = base_to_sensor_affine.matrix().cast<float>();
boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> sensor_points_baselinkTF_ptr(
new pcl::PointCloud<pcl::PointXYZ>);
pcl::transformPointCloud(
*sensor_points_sensorTF_ptr, *sensor_points_baselinkTF_ptr, base_to_sensor_matrix);设置为NDT的输入点云：
// set input point cloud
ndt_.setInputSource(sensor_points_baselinkTF_ptr);
if (ndt_.getInputTarget() == nullptr) {
ROS_WARN_STREAM_THROTTLE(1, "No MAP!");
return;
}最后一步中，我们需要为本次配准设置初始姿态估计，并将其划分为两种情况进行处理
// align
Eigen::Matrix4f initial_pose_matrix;
if (!init_pose){
Eigen::Affine3d initial_pose_affine;
tf2::fromMsg(initial_pose_cov_msg_.pose.pose, initial_pose_affine);
initial_pose_matrix = initial_pose_affine.matrix().cast<float>();
// for the first time, we don't know the pre_trans, so just use the init_trans,
// which means, the delta trans for the second time is 0
pre_trans = initial_pose_matrix;
init_pose = true;
}else
{
// use predicted pose as init guess (currently we only impl linear model)
initial_pose_matrix = pre_trans * delta_trans;
}如果一个机器人系统首次进行定位配准时，则会在Rviz软件中手动设置初始姿态；否则，则采用基于线性运动模型（匀速匀角速度）所预测出来的初始估计值来进行定位计算。由于pcl库中的NDT算法要求定位结果必须以齐次变换矩阵的形式（即Eigen::Matrix4f类型）进行表示（这种矩阵形式包含了平移和旋转信息），因此，在代码实现过程中会将初始姿态数据转换为该矩阵格式以便后续处理运算——具体来说，在初次定位阶段需要将获取到的位姿信息（Pose对象）转换为Eigen::Matrix4f类型的齐次变换矩阵；而对于非初次定位的情况，则是将前一次定位结果与当前一次定位结果之间的相对变换（分别由变换矩阵pre_trans和delta_trans表示）相加得到最终的位置估计值
在上述线性代数模型中：向量AB代表的是上一次定位过程中的全局坐标系转换关系（即base_link坐标系相对于地图原点O的位置与朝向），而向量BC则描述了当前坐标系相对于前一次坐标系的变化关系（即从前一次坐标系到当前坐标系的位姿变化）。根据结合律原理，在空间变换关系中可得：全局到当前坐标的总变换关系AC等于全局到前一坐标的AB变换与前一到当前BC变换之积
AC = AB * BC
基于此, 对当前位置的信息进行预估时, 我们即可表示为 pre\_trans \times delta\_trans 的形式。在此基础上, 采用该预估结果作为基础, 并运用NDT算法来进行配准:
ndt_.align(*output_cloud, initial_pose_matrix);
const Eigen::Matrix4f result_pose_matrix = ndt_.getFinalTransformation();最终我们成功获得了定位过程中涉及的坐标系转换矩阵result_pose_matrix（基于base_link与地图坐标系之间的转换关系），并将其依次转化为Pose消息格式并发送至TargetFrameTopic节点以实现目标定位任务。
Eigen::Affine3d result_pose_affine;
result_pose_affine.matrix() = result_pose_matrix.cast<double>();
const geometry_msgs::Pose result_pose_msg = tf2::toMsg(result_pose_affine);
// publish
geometry_msgs::PoseStamped result_pose_stamped_msg;
result_pose_stamped_msg.header.stamp = sensor_ros_time;
result_pose_stamped_msg.header.frame_id = map_frame_;
result_pose_stamped_msg.pose = result_pose_msg;
if (is_converged) {
ndt_pose_pub_.publish(result_pose_stamped_msg);
}
// publish tf(map frame to base frame)
publish_tf(map_frame_, base_frame_, result_pose_stamped_msg);此外，我们还需要计算下一次用于初始姿态估计的delta_trans:
// calculate the delta tf from pre_trans to current_trans
delta_trans = pre_trans.inverse() * result_pose_matrix;
pre_trans = result_pose_matrix;\delta_{\text{trans}} 表示为当前变换与上一时刻变换之间的差值（包含平移量与旋转量的变化）。随后将这一变化记入 \text{pre\_trans} 并用于后续的姿态初始化估计。
可视化和车辆描述模型
通过生成可视化结果展示, 我们采用 URDF 模型表示车辆, 并将其分解为三个关键组件.
- lexus.urdf为汽车实体的基于URDF的描述文件。
- lexus.dae为计算机辅助设计（CAD）中的三维模型数据。
- lexus.jpg是用于表示3D模型表面材质信息的标准表格形式。
以上文件均位于项目urdf目录内。
通过 launch脚本 lexus.launch 启动两个 ROS 节点 joint_state Publisher 和 robot_state Publisher 以初始化车辆模型。
<!-- -->
<launch>
<arg name="base_frame" default="/base_link"/>
<arg name="topic_name" default="vehicle_model"/>
<arg name="offset_x" default="1.2"/>
<arg name="offset_y" default="0.0"/>
<arg name="offset_z" default="0.0"/>
<arg name="offset_roll" default="0.0"/> <!-- degree -->
<arg name="offset_pitch" default="0.0"/> <!-- degree -->
<arg name="offset_yaw" default="0.0"/> <!-- degree -->
<arg name="model_path" default="$(find ndt_localizer)/urdf/lexus.urdf" />
<arg name="gui" default="False" />
<param name="robot_description" textfile="$(arg model_path)" />
<param name="use_gui" value="$(arg gui)"/>
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
</launch>NDT激光雷达定位项目实战
请下载ndt_localizer代码，请将ndt_localizer仓库放置于你的ros_workspace/src/目录下。
git clone https://github.com/AbangLZU/ndt_localizer.git进行数据准备流程，在本节中我们将采用与上一篇博客相同的数据来源（即Mulran数据集中的KAIST02包）。获取下载链接请移步文末提供的百度网盘链接。详细方法请参考我之前的文章《自动驾驶系统进阶与项目实战（五）》中关于SC-Lego-LOAM用于构建大规模城市三维地图的部分。由于采用的是与前述相同的播包方式，在此从略说明细节。保证你能够使用Mulran 的KAIST02数据集播出rostopic为/os1_points的点云数据即可。
基于我上一篇博客的内容进行实践操作时所产出的点云数据集（如实在无法偷懒的朋友也可通过此链接快速获取），我们仍然鼓励大家按照上文所述的方法自行生成并处理数据集以加深理解
为了确保您能够全面掌握点云数据集的生成与应用过程
# inside your catkin workspace
catkin_make编译完成后，运行launch文件启动所有节点：
source devel/setup.bash
roslaunch ndt_localizer ndt_localizer.launch此时Rviz会被打开，等待点云地图加载完成，如下图所示：
使用TopDownOrtho视角模式时，请执行以下操作：首先将场景缩放至地图原点位置；随后单击RViz界面顶端的2DPoseEstimate按钮；接着在地图界面中设定初始姿态；如图所示即为此操作流程。
打开file_player开始播包：
接收到点云数据后，NDT本地化器正常运行，在Rviz中呈现的定位信息如下展示。
当配置得当且采用合适的降采样策略时，在计算资源方面具有较低的需求。值得注意的是，在完成一次点云配准任务所需的时间（如图所示）
基本在30ms左右，满足自动驾驶配准定位的要求。
相关链接
代码存储库位置：https://github.com/AbangLZU/ndt_localizer
可获取的PCD地图数据集位置：
百度网盘中的数据包下载地址（文件大小为4.7GB）：https://pan.baidu.com/s/16Ps9_5uXpgyE-L4WYJwWwQ 提取码: juup
文件播放器源代码位置：https://github.com/irapkaist/file_player