广告
基于NDT的自动驾驶高精度定位和ROS项目实战
2020-08-11 17:56:15· 来源：焉知自动驾驶、hdmap
对于高级自动驾驶系统而言，定位模块通常会融合IMU、轮速计（车辆底盘）以及激光雷达odometry多种测量，使用滤波算法（EKF、UKF等）以获得平滑、厘米级别的绝对
对于高级自动驾驶系统而言，定位模块通常会融合IMU、轮速计（车辆底盘）以及激光雷达odometry多种测量，使用滤波算法（EKF、UKF等）以获得平滑、厘米级别的绝对定位，其中基于高精度点云地图和激光雷达的配准定位（Lidar Odometry）因其精度高、可靠性好，在整个融合定位中通常占很大的权重，是自动驾驶定位系统中相对可靠的“绝对定位”数据来源，本文我们学习如何使用NDT配准实现自动驾驶汽车的高精度定位，并且结合前面文章中使用SC-LEGO-LOAM生产的点云地图，实践单纯NDT算法的自动驾驶高精度定位ROS项目，完成本文，你将能实现如下图所示的点云配准定位：
前言
在前面的文章中（无人驾驶汽车系统入门系列第13篇、自动驾驶系统进阶与项目实战第2篇）有详细介绍NDT算法和点云配准的相关知识，但是一直没有完整地介绍如何使用点云地图和NDT配准完成自动驾驶汽车定位，本文将给出完整介绍和一个干净的NDT定位实现。
NDT定位是Autoware自动驾驶开源项目的核心定位算法，但是Autoware 1.x各个模块耦合性很强，如果单纯是为了学习激光雷达配准定位，你可能需要编译整个Autoware项目才能测试实验NDT定位，另外，Autoware 1.x中实现的NDT算法采用面向过程编程，代码相当难读，因此，本文中，我们将基于Autoware 1.x中的NDT配准定位思路，实现一个相对干净、清晰的NDT ROS项目，整个项目仅依赖ROS，面向对象编程，如果项目对你有帮助，就来个小心心吧！
点云地图准备
自动驾驶汽车的激光雷达定位通常依赖于提前离线构建好的高精度点云地图，之所以这么做原因有以下几个方面：
- L4级别以上自动驾驶系统对定位精度和稳定性要求很高，绝对误差需要控制在20cm以内
- 纯SLAM目前来说无法达到自动驾驶对于定位精度、可靠性的要求，即我们现在的研究很难实现自动驾驶车的在线制图和定位（问题包括闭环优化，全局优化，误差累计修正等等）
- 高精度地图制造商的完整生产流程需要较大的算力和人工，他们能够生产非常理想的点云地图和语义地图，但是需要离线生产（时间和人力）；
- 利用高精度地图可以相对简单地实现激光雷达定位，在融合了IMU和轮速计以后这类定位方法的精度和可靠性基本满足自动驾驶汽车定位的需求。
所以综合以上的客观原因，目前的L4和大部分L3自动驾驶系统定位模块仍然是以事先构建的高精度地图为基础进行的配准定位，这个配准使用的传感器，少数厂商使用的是camera(如mobileeye)，绝大多数厂商目前仍然采用的是激光雷达配准思路。点云地图就是激光雷达配准所需事先构建的“用来定位的地图”。
在上一篇文章（自动驾驶系统进阶与项目实战五）中，我们使用SC-LEGO-LOAM方法构建了一张相对较大的点云图，并且使用Scan Context方法对点云图进行了闭环检测和姿态图优化，本文我们将直接使用上一篇文章中生产的点云图作为定位地图。
将上一篇文章中生成的pcd文件(偷懒的同学也可以直接通过文末链接下载该pcd文件)拷贝到本项目（仓库地址见文末链接）的map目录下，命名为kaist02.pcd，项目中的map_loader节点主要用于载入地图：
MapLoader::MapLoader(ros::NodeHandle &nh){
std::string pcd_file_path, map_topic;
nh.param("pcd_path", pcd_file_path, "");
nh.param("map_topic", map_topic, "point_map");
init_tf_params(nh);
pc_map_pub_ = nh.advertise(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);
}
}
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
构造函数中读取pcd文件的路径和map topic，并且初始化map的变换参数（如果不需要对map进行变换，则数值都设置为０），在map_load.launch中设置这些参数：
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
函数CreatePcd()用于加载pcd，TransformMap()用于平移和旋转地图，我们使用Eigen和pcl::transformPointCloud()实现点云的变换：
sensor_msgs::PointCloud2 MapLoader::TransformMap(sensor_msgs::PointCloud2 & in){
pcl::PointCloud
::Ptr in_pc(new pcl::PointCloud
);
pcl::fromROSMsg(in, *in_pc);
pcl::PointCloud
::Ptr transformed_pc_ptr(new pcl::PointCloud
);
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;
}
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
输入点云降采样
NDT算法优化的目标函数主要是输入点云和目标点云概率分布的相似性，这种配准算法的计算复杂度和两个要素正相关：
- 输入点云的点的密度
- 初始姿态估计的偏差
输入点云点越密集，NDT配准所需的计算复杂度就越大；初始姿态估计越差（越偏离真实的姿态），相应的计算复杂度也越大，初始姿态过差的话NDT甚至无法收敛。自动驾驶激光雷达定位对实时性有较高的要求，点云配准所用的时间显然越少越好，所以我们可以通过降采样输入点云以提高NDT配准的速度，本文中我们采用VoxelGrid降采样方法降低输入点云的密度，代码在项目的 voxel_grid_filter.cpp中，主要代码如下：
static void scan_callback(const sensor_msgs::PointCloud2::ConstPtr& input)
{
pcl::PointCloud
scan;
pcl::fromROSMsg(*input, scan);
if(measurement_range != MAX_MEASUREMENT_RANGE){
scan = removePointsByRange(scan, 0, measurement_range);
}
pcl::PointCloud
::Ptr scan_ptr(new pcl::PointCloud
(scan));
pcl::PointCloud
::Ptr filtered_scan_ptr(new pcl::PointCloud
());
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
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);
}
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
在得到点云以后，首先对点云进行截取，我们只保留 MAX_MEASUREMENT_RANGE 距离以内的点用于定位（本文中MAX_MEASUREMENT_RANGE = 120 米），VoxelGrid降采样的主要参数就是voxel_leaf_size，该参数设定了降采样选取的立方体的边长（单位为米），在一个这样的立方体内只保留１个点，可以在 points_downsample.launch文件中配置该参数：
- 1
如上所示，本文采用了３米的leaf size，这个参数可以根据你实际使用的激光雷达点的密度决定，虽然我们追求配准的实时性，但同时我们也不希望牺牲太多定位的精度，所以对输入点云降采样的度需要平衡实时性和定位精度，根据经验，如果你猜用的是16线的激光雷达，那么降采样的leaf size控制在１-２m较为合适，当采用的激光雷达为32线及以上,可以将leaf size设置为2-3m。
降采样后的点云将被输出至 /filtered_points 话题，以供后续的NDT配准定位使用。
使用NDT为自动驾驶车提供高精度定位
NDT定位的逻辑我主要实现在ndt.cpp中，下面我们仔细分析一下这一部分的代码。
初始姿态获取
一切使用预先构建的地图进行配准定位的方法都需要提供初始姿态，在工业界的实践中，这一初始姿态通常是通过gnss获得，本文中我们简化这一步，在Rviz中手动指定初始姿态，Rviz中设定的初始姿态通常会被默认发送至/initialpose topic上，在NdtLocalizer构造函数中，写一个subscriber监听该topic：
initial_pose_sub_ = nh_.subscribe("initialpose", 100, &NdtLocalizer::callback_init_pose, this);
- 1
当有初始姿态（geometry_msgs::PoseWithCovarianceStamped）传来的时候，执行的是以下回调：
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;
}
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
在NDT配准中，我们主要关注四个坐标系间的变化，分别是：
- 世界坐标系（frame_id = world）
- 地图坐标系（frame_id = map）
- 车辆基础坐标系（frame_id = base_link）
- 激光雷达坐标系（本文中frame_id = ouster，根据你使用的激光雷达不同，frame id也会不一样）
在本项目中，我们使用static_tf.launch发布world 到 map以及ouster到base_link这两个固定变换，如下：
- 1
- 2
注：在这里我们假定map和world为同一坐标系以简化问题，在具体的自动驾驶系统研发中，你需要根据WGS84坐标系下的经纬度配合通用横轴墨卡托投影（Universal Transverse Mercator，UTM）以获得当前Map到世界坐标系的平移关系以及东北天（East North Up, ENU）坐标系下的旋转量。
localizer_to_base_link即激光雷达到base link的变换关系，是激光雷达的外参之一，也是一个静态变换。
回到初始姿态获取，得到Rviz上手动指定的初始姿态后，首先对坐标系进行统一，如果该pose是在地图坐标系，那么保存用于后续使用，如果是其他坐标系，则先将该pose转换至地图坐标系，通过函数 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;
}
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
在获得到map的tf以后，直接通过tf2::doTransform将初始姿态转换到地图坐标系下。
初始化地图
NDT配准中的目标点云就是我们事先使用SC-LEGO-LOAM构建的点云地图了，编写Subscriber监听mapLoader节点发来的点云地图massage，执行如下回调：
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
ndt_new.setTransformationEpsilon(trans_epsilon);
ndt_new.setStepSize(step_size);
ndt_new.setResolution(resolution);
ndt_new.setMaximumIterations(max_iterations);
pcl::PointCloud
::Ptr map_points_ptr(new pcl::PointCloud
);
pcl::fromROSMsg(*map_points_msg_ptr, *map_points_ptr);
ndt_new.setInputTarget(map_points_ptr);
// create Thread
// detach
pcl::PointCloud
::Ptr output_cloud(new pcl::PointCloud
);
ndt_new.align(*output_cloud, Eigen::Matrix4f::Identity());
// swap
ndt_map_mtx_.lock();
ndt_ = ndt_new;
ndt_map_mtx_.unlock();
}
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
其中最关键的一步就是ndt_new.setInputTarget(map_points_ptr);在获取点云地图以后，设置ndt的目标点云为该点云地图，同时也是这里NDT算法的基本参数：
ndt_new.setTransformationEpsilon(trans_epsilon);搜索的最小变化ndt_new.setStepSize(step_size);搜索的步长ndt_new.setResolution(resolution);目标点云的ND体素的尺寸，单位为米ndt_new.setMaximumIterations(max_iterations);使用牛顿法优化的迭代次数
具体参数的意义以及NDT算法的理论基础可以参考我前面的两篇博客自动驾驶系统进阶与项目实战（二） 和 无人驾驶汽车系统入门（十三）
NDT配准定位
配准定位主要实现于以下回调中：
void callback_pointcloud(const sensor_msgs::PointCloud2::ConstPtr & sensor_points_sensorTF_msg_ptr);
- 1
该回调监听降采样后的点云，首先解析PointCloud2消息为pcl的PointCloud结构：
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
new pcl::PointCloud
);
- 1
- 2
- 3
- 4
- 5
该点云是在激光雷达坐标系下，所以接着将数据投射到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();
boost::shared_ptr
new pcl::PointCloud
);
pcl::transformPointCloud(
*sensor_points_sensorTF_ptr, *sensor_points_baselinkTF_ptr, base_to_sensor_matrix);
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
设置为NDT的输入点云：
// set input point cloud
ndt_.setInputSource(sensor_points_baselinkTF_ptr);
if (ndt_.getInputTarget() == nullptr) {
ROS_WARN_STREAM_THROTTLE(1, "No MAP!");
return;
}
- 1
- 2
- 3
- 4
- 5
- 6
- 7
最后我们还需要设定这次配准的初始姿态估计，这里需要分为以下两种情况：
// 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();
// 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;
}
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
如果是第一次配准，则使用我们在Rviz中手工指定的初始姿态，否则使用线性模型（匀速匀角速度）预测的初始估计。pcl实现的NDT要求初始姿态估计使用Eigen::Matrix4f表示(也就是标准的齐次变换矩阵)，所以上面的代码中，如果是初次配准，需要将Pose转换为Eigen::Matrix4f，使用tf2::fromMsg()函数完成，对于非初次配准，我们的思路是用上一次NDT的定位结果（变换矩阵pre_trans） + 线性变换量（变换矩阵delta_trans）。在线性代数中，如果用向量 ABABAB 描述上一次定位的变换（即上一次定位base_link到地图原点的变换，即pre_trans）， BCBCBC 表示当前一次定位到上一次定位的变换（即delta_trans）,那么当前的定位 ACACAC就可以表示为：
AC=AB∗BC
AC = AB * BC
A
C
=
A
B
∗
B
C
所以对当前位置的初始姿态估计就可以用 pre_trans * delta_trans表示。接着我们设置该初始估计，并且使用ndt进行配准：
ndt_.align(*output_cloud, initial_pose_matrix);
const Eigen::Matrix4f result_pose_matrix = ndt_.getFinalTransformation();
- 1
- 2
最终我们获得了定位的变换矩阵（base_link 到map的变换）result_pose_matrix，将之转换为Pose msg以及TF发布出去，完成本次定位：
Eigen::Affine3d result_pose_affine;
result_pose_affine.matrix() = result_pose_matrix.cast();
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);
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
此外，我们还需要计算下一次用于初始姿态估计的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;
- 1
- 2
- 3
- 4
delta_trans即当前变换和上一次变换的差值（平移量和旋转量的差值）。最后将当前的变换保存为pre_trans供下一次初始姿态估计使用。至此NDT配准的流程结束。
可视化和车辆描述模型
为了可视化定位结果，我们使用一个URDF模型来可视化车辆，该模型主要包含以下三个文件：
- lexus.urdf: 即车辆模型的URDF描述文件
- lexus.dae: 即车辆的3D模型文件
- lexus.jpg: 3D模型的表面材料
以上文件均包含于项目的urdf文件夹下，使用lexus.launch启动joint_state_publisher和robot_state_publisher两个ros node以启用车辆模型，launch文件定义如下：
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
NDT激光雷达定位项目实战
首先下载ndt_localizer代码,将ndt_localizer仓库克隆至你的ros_workspace/src/目录下：
git clone https://github.com/AbangLZU/ndt_localizer.git
- 1
准备数据，这里我们仍然使用和上一篇博客同样的数据（Mulran数据集的KAIST02数据包），下载路径见文末百度网盘链接。该数据包使用方法见我上一篇博客自动驾驶系统进阶与项目实战（五）中的“基于SC-Lego-LOAM构建较大规模的城区三维地图”一节，因为使用数据包播包方式完全相同，在此不赘述，保证你能够使用Mulran 的KAIST02数据集播出rostopic为/os1_points的点云数据即可。
使用我上一篇博客最后实践生产出的点云地图（偷懒的同学也可以通过此链接下载），当然还是建议同学们跟着我上一篇博文的内容自行生产地图，这样能够实践点云地图的生产和使用的全流程。将点云地图命名为kaist02.pcd，并且复制到项目的map目录下。编译整个项目：
# inside your catkin workspace
catkin_make
- 1
- 2
编译完成后，运行launch文件启动所有节点：
source devel/setup.bash
roslaunch ndt_localizer ndt_localizer.launch
- 1
- 2
此时Rviz会被打开，等待点云地图加载完成，如下图所示：
在TopDownOrtho视角下，缩放到地图原点位置，点击rviz上方的2D Pose Estimate按钮，在地图中指定初始姿态，如下图所示：
打开file_player开始播包：
收到点云数据后，ndt_localizer开始工作，Rviz中显示的定位结果如下所示:
配置得当的话（合适的降采样），NDT对计算资源要求很低，甚至能够在TX2上稳定运行，如下图NDT执行一次点云配准的耗时：
广告 
广告
编辑推荐
最新资讯
-
荷兰Zepp氢燃料电池卡车-Europa
2024-12-22 10:13
-
NCACFE -车队油耗经济性报告（2024版）
2024-12-22 10:11
-
R54法规对商用车轮胎的要求（上）
2024-12-22 10:10
-
蔚来ET9数字架构解析
2024-12-22 09:53
-
4G/5G网络新时代的高效紧急呼叫系统NG-eCal
2024-12-20 22:33
广告
广告
