《自动驾驶编程原理与应用》课件 第4章高精地图创建与加载

1主讲：黄炜自动驾驶编程原理与应用2第四章高精地图创建与加载知识点：了解主流建图方法；掌握自动驾驶系统高精地图创建与加载功能包相关功能、实现原理和使用方法。重点：重点掌握NDT点云匹配算法原理，以及Autoware高精地图创建与加载模块中ROS节点ndt_mapping等。难点：高精地图创建与加载功能模块与示例节点源码解析。自动驾驶编程原理与应用3引言高精地图（HighlyAutomatedDrivingMap）主要用于为自动驾驶车辆提供所处环境和交通运行等数据信息，被认为是自动驾驶应用的重要支撑性基础设施之一。不同于普通导航电子地图，高精地图通常指一类服务于自动驾驶系统用于实现高精度定位的专题地图。4引言特点：高精地图具备高精度、高维度、高丰富度、高新鲜度的特征，具体体现在数据精度可达分米甚至厘米级、车道级数据维度、要素丰富、数据更新频次高。高精地图不受天气环境、障碍物和探测距离等限制，因此可以有效弥补车载传感器的性能边界，为规划决策提供重要的先验信息。目前常见的高精地图格式有vectormap、lanelet2、opendrive、nds等，不同格式的高精地图之间一般可以相互转化。5引言高精地图数据体空间信息：基于移动测量车辆采集道路点云数据提取的车道模型信息。语义信息：道路部件信息，如转向路标、速度标识等交通标志标线所传递的引导、限制、警告、指示信息。时间信息：道路属性信息，如潮汐车道信息、红绿灯信息等。6本章节目录4.1地图创建与加载方法概述常见建图方法简介Autoware地图创建基本步骤Autoware地图加载基本步骤4.2点云地图构建功能包NDT点云匹配地图构建方法节点ndt_mapping与源码解析4.3地图加载功能包节点points_map_loader与源码解析节点vector_map_loader与源码解析74.1地图创建与加载方法概述84.1地图创建与加载方法概述创建高精地图需要通过装备激光雷达传感器、全球导航卫星系统、视觉传感器、惯性测量单元等感知设备的移动测量车辆采集数据以获取周围环境的点云、定位和图像等信息。94.1地图创建与加载方法概述对采集的数据进行整理分类与清洗获得点云数据。将点云数据拼接配准得到三维点云地图，并进行语义信息标注，标注对象通常包括人行横道、车道、交通灯等要素。104.1地图创建与加载方法概述常见建图方法简介作为自动驾驶领域常见的地图构建技术，同步定位与建图（SimultaneousLocalizationandMapping,SLAM）的概念起源最早可追溯至1986年，相关未知环境同步定位与地图构建问题首次在电气与电子工程师协会（IEEE）主办的机器人与自动化会议上正式被提出并讨论。在没有环境先验信息的情况下，SLAM技术可以充分利用车辆移动过程中车载传感器获得的感知信息构建增量式地图，同时实现自主定位。114.1地图创建与加载方法概述常见建图方法简介根据使用的传感器类型不同，目前主流发展的SLAM技术可以大致分为激光SLAM（基于激光雷达传感器返回的点云信息）和视觉SLAM（基于视觉传感器返回的图像信息）两类。在应用效果上，两种SLAM技术各擅胜场：a）激光SLAM建图精度高且累计误差小，但是相应成本也较高；b）视觉SLAM方便从图像中提取语义信息，但是受环境光照强度影响大且无法识别无纹理区域。124.1地图创建与加载方法概述常见建图方法简介激光SLAM技术二维激光点云地图算法三维激光点云地图算法scan-to-scanscan-to-map134.1地图创建与加载方法概述常见建图方法简介三维激光SLAM建图算法可以分为scan-to-scan和scan-to-map两类。其中，scan-to-map指激光雷达扫描数据直接与地图进行匹配，而scan-to-scan则是指通过求解坐标转换关系将连续扫描的两帧或多帧激光点云统一到同一坐标系下。scan-to-scan的典型代表是LOAM系列算法，包括LEGO-LOAM、LIO-SAM等；而scan-to-map的典型代表是Cartographer和NDT等算法。144.1地图创建与加载方法概述常见建图方法简介在构建激光点云地图过程中需要获取激光雷达扫描的点云数据作为输入，输入的点云数据一般基于激光雷达坐标系，同时激光雷达坐标系和车辆坐标系为刚性连接。因此，将车辆起始位置当成地图坐标系的原点，在车辆移动过程中通过准确获取车辆的位姿变化将原本基于激光雷达坐标系的点云信息转换到地图坐标系下，进而就构成了点云地图的一部分，以此往复逐步构建出未知环境的点云地图。思考：为什么需要点云匹配？点云匹配的作用是什么？154.1地图创建与加载方法概述常见建图方法简介在构建点云地图过程中可以使用人工智能语义识别方法检测周围环境中的静态对象（人行横道、车道、交通灯等要素）同步对其进行自动语义信息标注绘制语义地图，也可以在点云地图构建完成后再通过手动标注语义信息的方式绘制或是编辑语义地图。目前手动标注语义信息主流工具有AutowareTools、Unity插件MapToolbox、VTD、RoadRunner等。后续将以语义标注工具MapToolbox为例介绍手动标注语义信息过程与导出语义地图文件方法。164.1地图创建与加载方法概述常见建图方法简介实际的高精地图生产流程大致可以概括为数据采集、数据编辑、安全处理、地图审查、地图发布五个部分。生成的高精地图还需要进行安全处理以保证国家地理信息安全并通过相关部门备案审核方能对外发布商用。174.1地图创建与加载方法概述常见建图方法简介对于高精地图质量的要求主要体现在其数据完整性、逻辑一致性、位置准确度、专题准确度、时间质量等方面，在我国现行测绘行业标准《道路高精度电子导航地图质量规范》中对高精地图质量相关检查指标和评定方法都作了详细规定和要求。18Autoware地图创建基本步骤（1）点云地图构建在终端输入以下命令，首先在官方示例autoware_quickstart_examples路径下找到工作目录“./launch/rosbag_demo”，然后在rosbag_demo文件夹中创建启动文件my_mapping.launch：$

cdautoware.ai/src/autoware/documentation/autoware_quickstart_examples$cdlaunch/rosbag_demo$geditmy_mapping.launch4.1地图创建与加载方法概述<launch><!--loadparametersforndt_matching--><rosparamcommand="load"file="$(findautoware_quickstart_examples)/config/headless_setup.yaml"/><!--TF--><includefile="$(envHOME)/.autoware/data/tf/tf.launch"/><!--Setup--><includefile="$(findruntime_manager)/launch_files/setup_tf.launch"><argname="x"value="1.2"/><argname="y"value="0.0"/><argname="z"value="2.0"/><argname="yaw"value="0.0"/><argname="pitch"value="0.0"/><argname="roll"value="0.0"/><argname="frame_id"value="/base_link"/><argname="child_frame_id"value="/velodyne"/><argname="period_in_ms"value="10"/></include><includefile="$(findvehicle_description)/launch/vehicle_model.launch"/><!--pointsdownsampler--><includefile="$(findpoints_downsampler)/launch/points_downsample.launch"/><!--PointCloud--><includefile="$(findlidar_localizer)/launch/ndt_mapping.launch"/>

</launch>19Autoware地图创建基本步骤（1）点云地图构建打开新建的启动文件my_mapping.launch文本，编辑然后保存，该新建的启动文件可以通过导入“ndt_mapping.launch”等多个启动文件到当前文件实现点云地图构建过程中所需相关功能的调用。4.1地图创建与加载方法概述my_mapping.launch$

colconbuild––cmake–args-DCMAKE_BUILD_TYPE=Release––packages-selectautoware_quickstart_examples20Autoware地图创建基本步骤（1）点云地图构建在官方示例autoware_quickstart_examples中创建启动文件my_mapping.launch后，重新编译工作区，在终端输入以下命令：4.1地图创建与加载方法概述仅编译指定包（Release模式）21Autoware地图创建基本步骤（1）点云地图构建a)在终端输入以下命令启动my_mapping.launch开始构建点云地图：4.1地图创建与加载方法概述$cdautoware.ai$sourceinstall/setup.bash$roslaunchautoware_quickstart_examplesmy_mapping.launch22Autoware地图创建基本步骤（1）点云地图构建b)使用快捷键Shift+Ctrl+T在终端新建标签，在新标签中输入以下命令打开可视化工具RViz：4.1地图创建与加载方法概述$sourceinstall/setup.bash$rviz23Autoware地图创建基本步骤（1）点云地图构建c)在RViz中点击显示项目区域左侧下方的Add添加相应插件订阅消息。首先在对话框中选择“Bytopic”，再选择“/ndt_map:PointCloud2”，然后选择OK实现对点云数据的可视化显示。添加完成后，在显示项目区域会列出已经添加的显示插件PointCloud2，在其下拉菜单中将Size(m)设置为0.1。4.1地图创建与加载方法概述24Autoware地图创建基本步骤（1）点云地图构建d)使用快捷键Shift+Ctrl+T在终端新建标签，在新标签中输入以下命令回放示例文件sample_moriyama_150324.bag中的点云数据：4.1地图创建与加载方法概述$cd~/.autoware$rosbagplaysample_moriyama_150324.bag25Autoware地图创建基本步骤（1）点云地图构建e)当RViz中显示点云地图构建快完成时可以使用rosbag工具开始记录话题数据，在点云地图构建后在数据记录运行的终端界面使用快捷键Ctrl+C强制中断程序，终止数据记录。4.1地图创建与加载方法概述26Autoware地图创建基本步骤（1）点云地图构建e)使用快捷键Shift+Ctrl+T在终端新建标签，在终端输入以下命令抓取需要记录的话题数据并打包存放在自定义的文件夹“bagfiles”，然后使用rosbag工具记录指定的话题数据“/ndt_map”，并保存在自定义的数据包“demo_map.bag”中：4.1地图创建与加载方法概述$mkdir~/bagfiles$cd~/bagfiles$rosbagrecord-Odemo_map.bag/ndt_map27Autoware地图创建基本步骤（1）点云地图构建f)在终端输入以下命令查看demo_map.bag中记录的话题及其记录的消息的类型，然后通过点云文件解析工具对数据包demo_map.bag中存储点云信息相关的话题数据/ndt_map进行解析，将数据包（.bag）转为点云数据文件（.pcd），最终生成点云地图并保存在自定义的文件夹“demo_pcd”中：4.1地图创建与加载方法概述$rosbaginfodemo_map.bag$rosrunpcl_rosbag_to_pcddemo_map.bag/ndt_mapdemo_pcd28Autoware地图创建基本步骤（2）语义地图绘制Autoware官方语义地图绘制插件MapToolbox支持在Windows10操作系统下使用Unity软件实现语义信息标注以及vectormap（.csv）格式高精地图生成等功能。在学习使用插件MapToolbox绘制语义地图之前，首先需要在Windows10操作系统下安装Unity软件。访问Unity官网下载并安装管理工具UnityHub，再通过UnityHub安装Unity软件（以2021.3.45f1c1版本为例）。4.1地图创建与加载方法概述294.1地图创建与加载方法概述Autoware地图创建基本步骤（2）语义地图绘制在管理工具UnityHub主界面登入个人账户后，进入Preferences点击Licenses获取FreePersonalLicenses。退出Preferences选项后，点击管理工具UnityHub主界面Installs中的InstallEditor，选择2021.3.45f1c1版本进行安装。然后，在Unity软件中安装语义地图绘制插件MapToolbox。在Unity软件中点击NewProject新建项目，选择3D(Built-InRenderPipeline)核心模板并输入文件名、保存地址后点击Createproject创建项目。30Autoware地图创建基本步骤（2）语义地图绘制Unity操作主界面主要由六部分组成。4.1地图创建与加载方法概述Unity操作主界面a)层级面板（Hierarchy）b)场景面板（Scene）c)游戏面板（Game）d)属性面板（Inspector）e)项目面板（Project）f)控制面板（Console）31Autoware地图创建基本步骤（2）语义地图绘制a)层级面板（Hierarchy）：展示当前中场景的对象。b)场景面板（Scene）：用于编辑场景，以及显示3D或2D透视图。c)游戏面板（Game）：显示场景摄像机最终渲染的效果。d)属性面板（Inspector）：用于查看和编辑当前所选对象的所有属性。e)项目面板（Project）：显示可在项目中使用的资源库。f)控制面板（Console）：显示编辑器生成的错误、警告和其他消息。4.1地图创建与加载方法概述324.1地图创建与加载方法概述Autoware地图创建基本步骤（2）语义地图绘制在操作主界面标题菜单中选择Window→PackageManagement。在PackageManagement窗口中点击➕，选择下拉菜单中的AddpackagefromgitURL...，在文本框中输入com.unity.entities后点击Add添加并安装Entities插件，等待Entities插件安装完成后接着在文本框中输入/autocore-ai/MapToolbox.git#vector_map后点击Add添加MapToolbox插件（需要提前安装Git软件）并完成安装。334.1地图创建与加载方法概述Autoware地图创建基本步骤（2）语义地图绘制插件安装完成后返回操作主界面点击➕，选择下拉菜单中的Autoware→AutowareADASMap，属性面板出现如图所示工具箱。344.1地图创建与加载方法概述Autoware地图创建基本步骤（2）语义地图绘制在绘制语义地图前，需要导入先前构建好的点云地图。首先点击切换至项目面板，鼠标右键点击项目面板中的Assets，通过选择ImportNewAsset添加点云文件.pcd，加载完成后在场景面板里会出现加载的点云地图。为了方便绘制，可在场景面板右上方点击y轴正方向，固定点云视角。然后将点云文件.pcd从项目面板Assets文件夹中拖到层级面板中，右键点击层级面板空白处，选择Autoware→AutowareADASMap创建新的地图。354.1地图创建与加载方法概述Autoware地图创建基本步骤（2）语义地图绘制标注语义地图中“车道”的方法如下：选择属性面板AutowareADASMap工具箱中车道相应的选项AddLane，即可在场景面板区域内标注对应位置的车道信息。点击选项AddLane后在场景面板中会出现两个重叠的坐标系，分别拖动两个坐标系的xoz面（绿色）使其坐标原点到对应的位置即可形成一条直线车道。车道一般标注于两条车道线中间，是全局路径规划的重要依据，需要注意的是车道的行驶方向须与箭头方向一致。364.1地图创建与加载方法概述Autoware地图创建基本步骤（2）语义地图绘制点击层级面板中AutowareADASMap→CollectionLane→ADASGoSlicesLane→ADASGoLane，然后点击属性面板中AddBefore或AddAfter，即可以当前车道的起点或终点作为起点画下一条车道。374.1地图创建与加载方法概述Autoware地图创建基本步骤（2）语义地图绘制由于全局路径规划的目的地只能选择语义地图中每条车道的起终点，无法选择车道中间作为目的地，因此需要将车道分割成小线段。点击层级面板中AutowareADASMap→CollectionLane→ADASGoSlicesLane→ADASGoLane，然后点击属性面板中Subdivision，选择Normalway，即可将直线车道切割成小线段。384.1地图创建与加载方法概述Autoware地图创建基本步骤（2）语义地图绘制对于曲线车道，需要在分别拖动场景面板中两个坐标系原点到曲线车道起终点位置后点击属性面板中Subdivision，然后再分别拖动场景面板中两个坐标系的xoz面（绿色）使其坐标原点到对应的位置即可形成一条曲线车道，最后同样选择Normalway将曲线车道切割成小线段。394.1地图创建与加载方法概述Autoware地图创建基本步骤（2）语义地图绘制标注语义地图中“车道线”的方法如下：选择属性面板AutowareADASMap工具箱中车道线相应的选项AddWhiteLine，即可在场景面板区域内标注对应位置的车道线信息。点击选项AddWhiteLine后在场景面板中会出现两个重叠的坐标系，分别拖动两个坐标系的xoz面（绿色）使其坐标原点到对应的位置即可形成一条车道线。404.1地图创建与加载方法概述Autoware地图创建基本步骤（2）语义地图绘制点击层级面板中AutowareADASMap→CollectionWhiteLine→ADASGoSlicesWhiteLine→ADASGoWhiteLine，然后点击属性面板中AddBefore或AddAfter，即可以当前车道线的起点或终点作为起点画下一条车道线。414.1地图创建与加载方法概述Autoware地图创建基本步骤（2）语义地图绘制标注语义地图中“人行横道”的方法如下：选择项目面板AutowareADASMap工具箱中人行横道相应的选项AddCrossWalk,即可在场景面板区域内标注对应位置的人行横道信息。点击选项AddCrossWalk后在层级面板AutowareADASMap→CollectionCrossWalk→ADASGoCrossWalk中出现四个ADASGoLine表示人行横道的四条边，通过拖动两个坐标系的xoz面（绿色）调整四边形的四条边位置即可形成一条人行横道。424.1地图创建与加载方法概述Autoware地图创建基本步骤（2）语义地图绘制在完成语义地图绘制后，需要对语义地图进行保存。首先点击层级面板中AutowareADASMap，然后点击属性面板中SaveAutowareADASMapfromfolder选择文件夹路径保存语义地图。43Autoware地图加载基本步骤以运行官方示例数据为例，在终端多标签中输入以下命令加载地图：4.1地图创建与加载方法概述$cdautoware.ai$sourceinstall/setup.bash$roslaunchautoware_quickstart_examplesmy_map.launch44Autoware地图加载基本步骤启动文件my_map.launch的文本用于加载点云文件.pcd和语义地图文件.csv，读取对应的点云数据和场景语义描述信息。4.1地图创建与加载方法概述<launch><!--loadparametersforndt_matching--><rosparamcommand="load"file="$(findautoware_quickstart_examples)/config/headless_setup.yaml"/><!--TF--><includefile="$(envHOME)/.autoware/data/tf/tf.launch"/><!--PointCloud--><nodepkg=“map_file”type=“points_map_loader”name=“points_map_loader”args=“noupdate$(envHOME)/.autoware/data/map/pointcloud_map/bin_Laser-00167_-00864.pcd$(envHOME)/.autoware/data/map/pointcloud_map/bin_Laser-00153_-00852.pcd$(envHOME)/.autoware/data/map/pointcloud_map/bin_Laser-00159_-00859.pcd$(envHOME)/.autoware/data/map/pointcloud_map/bin_Laser-00160_-00861.pcd$(envHOME)/.autoware/data/map/pointcloud_map/bin_Laser-00148_-00849.pcd$(envHOME)/.autoware/data/map/pointcloud_map/bin_Laser-00168_-00866.pcd…"/><!--VectorMap--><nodepkg="map_file"type="vector_map_loader"name="vector_map_loader"args="$(envHOME)/.autoware/data/map/vector_map/lane.csv$(envHOME)/.autoware/data/map/vector_map/line.csv$(envHOME)/.autoware/data/map/vector_map/utilitypole.csv$(envHOME)/.autoware/data/map/vector_map/curb.csv$(envHOME)/.autoware/data/map/vector_map/node.csv…"/></launch>my_map.launch454.2点云地图构建功能包46NDT点云匹配地图构建方法由于激光雷达点云具有近密远疏的采样特性，同时受限于激光雷达自身的扫描距离和扫描角度，导致激光雷达无法通过一次扫描获得整个环境完整的、具有足够采样分辨率的点云数据，因此激光雷达需要在车辆移动过程中连续多次地扫描周围环境，然后将每一次扫描同一场景所得到的点云数据统一投影到同一个坐标系，从而逐渐形成完整且具有足够采样分辨率的点云数据以获取有效的周围环境信息。4.2点云地图构建功能包47

4.2点云地图构建功能包参考文献：[4.1]TheNormalDistributionsTransform:ANewApproachtoLaserScanMatching48

4.2点云地图构建功能包需要注意的是，每个网格内点的数量不能少于3个，一般至少保证有5个点。49

4.2点云地图构建功能包50

4.2点云地图构建功能包

51

4.2点云地图构建功能包52

4.2点云地图构建功能包53

4.2点云地图构建功能包54

4.2点云地图构建功能包<launch><!--loadparametersforndt_matching--><rosparamcommand="load"file="$(findautoware_quickstart_examples)/config/headless_setup.yaml"/><!--TF--><includefile="$(envHOME)/.autoware/data/tf/tf.launch"/><!--Setup--><includefile="$(findruntime_manager)/launch_files/setup_tf.launch"><argname="x"value="1.2"/><argname="y"value="0.0"/><argname="z"value="2.0"/><argname="yaw"value="0.0"/><argname="pitch"value="0.0"/><argname="roll"value="0.0"/><argname="frame_id"value="/base_link"/><argname="child_frame_id"value="/velodyne"/><argname="period_in_ms"value="10"/></include><includefile="$(findvehicle_description)/launch/vehicle_model.launch"/><!--pointsdownsampler--><includefile="$(findpoints_downsampler)/launch/points_downsample.launch"/><!--PointCloud--><includefile="$(findlidar_localizer)/launch/ndt_mapping.launch"/>

</launch>55节点ndt_mapping与源码解析启动文件my_mapping.launch中调用了功能包“~/autoware.ai/src/autoware/core_perception/lidar_localizer”目录下的启动文件“~/launch/ndt_mapping.launch”，用于启动点云地图构建过程中所需的多个节点。4.2点云地图构建功能包my_mapping.launch<!----><launch><argname="method_type"default="0"/><!--pcl_generic=0,pcl_anh=1,pcl_anh_gpu=2,pcl_openmp=3--><argname="use_odom"default="false"/><argname="use_imu"default="false"/><argname="imu_upside_down"default="false"/><argname="imu_topic"default="/imu_raw"/><argname="incremental_voxel_update"default="false"/><!--rosrunlidar_localizerndt_mapping--><nodepkg="lidar_localizer"type="queue_counter"name="queue_counter"output="screen"/><nodepkg="lidar_localizer"type="ndt_mapping"name="ndt_mapping"output="screen"><paramname="method_type"value="$(argmethod_type)"/><paramname="use_imu"value="$(arguse_imu)"/><paramname="use_odom"value="$(arguse_odom)"/><paramname="imu_upside_down"value="$(argimu_upside_down)"/><paramname="imu_topic"value="$(argimu_topic)"/><paramname="incremental_voxel_update"value="$(argincremental_voxel_update)"/></node></launch>56节点ndt_mapping与源码解析运行启动文件“ndt_mapping.launch”会启动节点queue_counter和节点ndt_mapping。其中，节点ndt_mapping主要用于激光点云配准，通过将当前扫描得到的激光点云变换到目标点云后进行拼接实现建图。4.2点云地图构建功能包ndt_mapping.launch……add_executable(ndt_mappingnodes/ndt_mapping/ndt_mapping.cpp)target_link_libraries(ndt_mapping${catkin_LIBRARIES})add_dependencies(ndt_mapping${catkin_EXPORTED_TARGETS})……57节点ndt_mapping与源码解析双击打开功能包lidar_localizer目录下的CMakeLists.txt文件。节点ndt_mapping的源文件是功能包lidar_localizer目录下的“nodes/ndt_mapping/ndt_mapping.cpp”。4.2点云地图构建功能包CMakeLists.txt58节点ndt_mapping与源码解析（1）主函数流程解析在VSCode软件中打开文件夹autoware.ai下的代码空间src，然后在侧边栏中依次点击文件夹autoware下的子文件夹core_perception找到功能包lidar_localizer，选择节点文件夹nodes中的节点ndt_mapping，最后双击打开源文件ndt_mapping.cpp。源文件ndt_mapping.cpp中主函数的流程如图所示。4.2点云地图构建功能包……intmain(intargc,char**argv){……previous_pose.x=0.0;……ndt_pose.x=0.0;……current_pose.x=0.0;……guess_pose.x=0.0;……added_pose.x=0.0;……diff_x=0.0;……offset_imu_x=0.0;……offset_odom_x=0.0;……}59节点ndt_mapping与源码解析（1）主函数流程解析4.2点云地图构建功能包ndt_mapping.cpp文本中主函数的位姿初始化部分初始化60节点ndt_mapping与源码解析（1）主函数流程解析对位姿的描述包括previous_pose（前一帧点云车体位姿）、ndt_pose（NDT点云配准得到的车体位姿）、current_pose（当前帧点云车体位姿）、guess_pose（NDT点云配准需要提供一个较好的车体初始位姿以提高配准速度，车体初始位姿一般可以通过IMU、ODOM以及二者联合获得）、added_pose（用于计算点云地图更新后的变化）、diff（前后两次传感器感知到的位姿变化）、offset_imu（IMU位姿偏差校正）、offset_odom（ODOM位姿偏差校正）等进行初始化设置。4.2点云地图构建功能包初始化61节点ndt_mapping与源码解析（1）主函数流程解析通过私有句柄调用getParam函数从参数服务器上获取method_type_tmp、_imu_upside_down、_imu_topic、_incremental_voxel_update等参数。4.2点云地图构建功能包参数获取……intmain(intargc,char**argv){……//settingparametersintmethod_type_tmp=0;private_nh.getParam("method_type",method_type_tmp);_method_type=static_cast<MethodType>(method_type_tmp);private_nh.getParam("imu_upside_down",_imu_upside_down);private_nh.getParam("imu_topic",_imu_topic);private_nh.getParam("incremental_voxel_update",_incremental_voxel_update);……}ndt_mapping.cpp文本中主函数的参数获取部分62节点ndt_mapping与源码解析（1）主函数流程解析世界坐标系、地图坐标系、车体坐标系和车载传感器坐标系是自动驾驶系统中常用的四类基本坐标系。其中，世界坐标系与地图坐标系之间的坐标变换关系是固定的，可以直接调用tf坐标变换库实现车体坐标系与传感器坐标系之间的静态坐标变换；因为车体坐标系与车载传感器坐标系之间的坐标变换关系也是固定的，所以同样可以提前设置相对位置关系确定二者之间的变换矩阵。4.2点云地图构建功能包坐标变换节点ndt_mapping与源码解析（1）主函数流程解析通过设置平移向量（tl_btol）和旋转向量（rot_x_btol，rot_y_btol，rot_z_btol）计算变换矩阵的方式来表示车辆底盘后轴中心点与激光雷达中心点的相对位置关系。4.2点云地图构建功能包坐标变换……intmain(intargc,char**argv){……Eigen::Translation3ftl_btol(_tf_x,_tf_y,_tf_z);//tl:translationEigen::AngleAxisfrot_x_btol(_tf_roll,Eigen::Vector3f::UnitX());//rot:rotationEigen::AngleAxisfrot_y_btol(_tf_pitch,Eigen::Vector3f::UnitY());Eigen::AngleAxisfrot_z_btol(_tf_yaw,Eigen::Vector3f::UnitZ());tf_btol=(tl_btol*rot_z_btol*rot_y_btol*rot_x_btol).matrix();tf_ltob=tf_btol.inverse();……}ndt_mapping.cpp文本中主函数的变换矩阵计算部分63……intmain(intargc,char**argv){……ndt_map_pub=nh.advertise<sensor_msgs::PointCloud2>("/ndt_map",1000);current_pose_pub=nh.advertise<geometry_msgs::PoseStamped>("/current_pose",1000);ros::Subscriberparam_sub=nh.subscribe("config/ndt_mapping",10,param_callback);ros::Subscriberoutput_sub=nh.subscribe("config/ndt_mapping_output",10,output_callback);ros::Subscriberpoints_sub=nh.subscribe("points_raw",100000,points_callback);ros::Subscriberodom_sub=nh.subscribe("/vehicle/odom",100000,odom_callback);ros::Subscriberimu_sub=nh.subscribe(_imu_topic,100000,imu_callback);……}64节点ndt_mapping与源码解析（1）主函数流程解析通过查找源文件ndt_mapping.cpp中主函数发布和订阅的话题，可以快速确认节点ndt_mapping的输出和输入信息。4.2点云地图构建功能包ndt_mapping.cpp文本中主函数的话题发布和订阅部分输入信息输出地图发布和订阅65节点ndt_mapping与源码解析（1）主函数流程解析基于节点ndt_mapping的点云匹配方法的输入信息主要来源于车载传感器采集的激光雷达点云数据（话题“points_raw”）、ODOM里程计数据（话题“/vehicle/odom”）以及IMU惯性测量单元数据（话题“_imu_topic”）等，然后基于NDT点云匹配算法计算出旋转和平移矩阵，最后输出pcd文件（PointCloudData，点云数据）生成点云地图（话题“/ndt_map”）。通常调用开源PCL（PointCloudLibrary，点云库）来实现NDT点云配准。4.2点云地图构建功能包发布和订阅66节点ndt_mapping与源码解析（1）主函数流程解析在主函数中订阅信息，然后在回调函数output_callback和points_callback中分别通过发布者ndt_map_pub和current_pose_pub发布点云数据（*map_msg_ptr）和位姿信息（current_pose_msg）。4.2点云地图构建功能包发布和订阅67节点ndt_mapping与源码解析（2）关键子函数解析param_callback函数主要用于NDT配准算法的参数设置，具体通过autoware_config_msgs::ConfigNDTMapping文件进行设置。4.2点云地图构建功能包……intmain(intargc,char**argv){……ndt_map_pub=nh.advertise<sensor_msgs::PointCloud2>("/ndt_map",1000);current_pose_pub=nh.advertise<geometry_msgs::PoseStamped>("/current_pose",1000);ros::Subscriberparam_sub=nh.subscribe("config/ndt_mapping",10,param_callback);ros::Subscriberoutput_sub=nh.subscribe("config/ndt_mapping_output",10,output_callback);ros::Subscriberpoints_sub=nh.subscribe("points_raw",100000,points_callback);ros::Subscriberodom_sub=nh.subscribe("/vehicle/odom",100000,odom_callback);ros::Subscriberimu_sub=nh.subscribe(_imu_topic,100000,imu_callback);……}ndt_mapping.cpp文本中主函数的话题发布和订阅部分param_callback函数68节点ndt_mapping与源码解析（2）关键子函数解析4.2点云地图构建功能包param_callback函数……staticvoidparam_callback(constautoware_config_msgs::ConfigNDTMapping::ConstPtr&input){ndt_res=input->resolution;step_size=input->step_size;trans_eps=input->trans_epsilon;max_iter=input->max_iterations;voxel_leaf_size=input->leaf_size;min_scan_range=input->min_scan_range;max_scan_range=input->max_scan_range;min_add_scan_shift=input->min_add_scan_shift;std::cout<<"param_callback"<<std::endl;std::cout<<"ndt_res:"<<ndt_res<<std::endl;std::cout<<"step_size:"<<step_size<<std::endl;std::cout<<"trans_epsilon:"<<trans_eps<<std::endl;std::cout<<"max_iter:"<<max_iter<<std::endl;std::cout<<"voxel_leaf_size:"<<voxel_leaf_size<<std::endl;std::cout<<"min_scan_range:"<<min_scan_range<<std::endl;std::cout<<"max_scan_range:"<<max_scan_range<<std::endl;std::cout<<"min_add_scan_shift:"<<min_add_scan_shift<<std::endl;}……ndt_mapping.cpp文本中param_callback函数部分设置的参数主要包括分辨率（ndt_res）、步长（step_size）、连续变化差值（trans_eps）、最大迭代次数（max_iter）、体素叶大小（voxel_leaf_size）、激光有效扫描范围（min_scan_range，max_scan_range）等。69节点ndt_mapping与源码解析（2）关键子函数解析其中，ndt_res使用点云网格化时的网格边长表示，其数值若设置过大则会影响精度，若设置过小则会影响内存使用，故需要选择合适的数值；step_size用于设置通过牛顿法优化的最大步长；trans_eps用于设置两个连续变换的最大差值作为阈值判断是否收敛；max_iter用于设置最大迭代次数；voxel_leaf_size用于设置体素叶的大小；min_scan_range和max_scan_range用于设置激光雷达最小和最大的扫描范围，从而过滤距离车辆较近和较远的点云数据。4.2点云地图构建功能包param_callback函数70节点ndt_mapping与源码解析（2）关键子函数解析output_callback函数主要用于地图点云数据的滤波处理和输出。4.2点云地图构建功能包……intmain(intargc,char**argv){……ndt_map_pub=nh.advertise<sensor_msgs::PointCloud2>("/ndt_map",1000);current_pose_pub=nh.advertise<geometry_msgs::PoseStamped>("/current_pose",1000);ros::Subscriberparam_sub=nh.subscribe("config/ndt_mapping",10,param_callback);ros::Subscriberoutput_sub=nh.subscribe("config/ndt_mapping_output",10,output_callback);ros::Subscriberpoints_sub=nh.subscribe("points_raw",100000,points_callback);ros::Subscriberodom_sub=nh.subscribe("/vehicle/odom",100000,odom_callback);ros::Subscriberimu_sub=nh.subscribe(_imu_topic,100000,imu_callback);……}ndt_mapping.cpp文本中主函数的话题发布和订阅部分output_callback函数71节点ndt_mapping与源码解析（2）关键子函数解析4.2点云地图构建功能包……staticvoidoutput_callback(constautoware_config_msgs::ConfigNDTMappingOutput::ConstPtr&input){doublefilter_res=input->filter_res;std::stringfilename=input->filename;std::cout<<"output_callback"<<std::endl;std::cout<<"filter_res:"<<filter_res<<std::endl;std::cout<<"filename:"<<filename<<std::endl;pcl::PointCloud<pcl::PointXYZI>::Ptrmap_ptr(newpcl::PointCloud<pcl::PointXYZI>(map));pcl::PointCloud<pcl::PointXYZI>::Ptrmap_filtered(newpcl::PointCloud<pcl::PointXYZI>());map_ptr->header.frame_id="map";map_filtered->header.frame_id="map";sensor_msgs::PointCloud2::Ptrmap_msg_ptr(newsensor_msgs::PointCloud2);……}……ndt_mapping.cpp文本中output_callback函数Part1output_callback函数定义了双精度浮点型（double）变量“filter_res”和字符型（string）变量“filename”以及指针变量“*map_ptr”、“*map_filtered”、“*map_msg_ptr”，其中指针变量采用指针类模板进行赋值。72节点ndt_mapping与源码解析（2）关键子函数解析4.2点云地图构建功能包……staticvoidoutput_callback(constautoware_config_msgs::ConfigNDTMappingOutput::ConstPtr&input){……if(filter_res==0.0){std::cout<<"Original:"<<map_ptr->points.size()<<"points."<<std::endl;pcl::toROSMsg(*map_ptr,*map_msg_ptr);}else{pcl::VoxelGrid<pcl::PointXYZI>voxel_grid_filter;voxel_grid_filter.setLeafSize(filter_res,filter_res,filter_res);voxel_grid_filter.setInputCloud(map_ptr);voxel_grid_filter.filter(*map_filtered);std::cout<<"Original:"<<map_ptr->points.size()<<"points."<<std::endl;std::cout<<"Filtered:"<<map_filtered->points.size()<<"points."<<std::endl;pcl::toROSMsg(*map_filtered,*map_msg_ptr);}ndt_map_pub.publish(*map_msg_ptr);……}……ndt_mapping.cpp文本中output_callback函数Part2output_callback函数运用PCL库体素滤波器pcl::VoxelGrid对原始点云数据进行过滤，减少点云数量。使用条件语句进行判断是否进行体素滤波。73节点ndt_mapping与源码解析（2）关键子函数解析4.2点云地图构建功能包……staticvoidoutput_callback(constautoware_config_msgs::ConfigNDTMappingOutput::ConstPtr&input){……if(filter_res==0.0){std::cout<<"Original:"<<map_ptr->points.size()<<"points."<<std::endl;pcl::toROSMsg(*map_ptr,*map_msg_ptr);}else{pcl::VoxelGrid<pcl::PointXYZI>voxel_grid_filter;voxel_grid_filter.setLeafSize(filter_res,filter_res,filter_res);voxel_grid_filter.setInputCloud(map_ptr);voxel_grid_filter.filter(*map_filtered);std::cout<<"Original:"<<map_ptr->points.size()<<"points."<<std::endl;std::cout<<"Filtered:"<<map_filtered->points.size()<<"points."<<std::endl;pcl::toROSMsg(*map_filtered,*map_msg_ptr);}ndt_map_pub.publish(*map_msg_ptr);……}……ndt_mapping.cpp文本中output_callback函数Part2output_callback函数如果未进行体素滤波处理（filter_res==0.0），则直接输出原始点云数量，并利用pcl::toROSMsg()函数将pcl::PointCloud<pcl::PointXYZI>类型的原始点云数据*map_ptr转化为ROS支持的sensor_msgs::PointCloud2类型的点云数据*map_msg_ptr，通过发布者ndt_map_pub进行发布；节点ndt_mapping与源码解析（2）关键子函数解析4.2点云地图构建功能包……staticvoidoutput_callback(constautoware_config_msgs::ConfigNDTMappingOutput::ConstPtr&input){……if(filter_res==0.0){std::cout<<"Original:"<<map_ptr->points.size()<<"points."<<std::endl;pcl::toROSMsg(*map_ptr,*map_msg_ptr);}else{pcl::VoxelGrid<pcl::PointXYZI>voxel_grid_filter;voxel_grid_filter.setLeafSize(filter_res,filter_res,filter_res);voxel_grid_filter.setInputCloud(map_ptr);voxel_grid_filter.filter(*map_filtered);std::cout<<"Original:"<<map_ptr->points.size()<<"points."<<std::endl;std::cout<<"Filtered:"<<map_filtered->points.size()<<"points."<<std::endl;pcl::toROSMsg(*map_filtered,*map_msg_ptr);}ndt_map_pub.publish(*map_msg_ptr);……}……ndt_mapping.cpp文本中output_callback函数Part2output_callback函数如果进行体素滤波处理，则输出过滤处理后的点云数据*map_filtered，同样也是需要将体素滤波后的点云数据类型转化为ROS支持的sensor_msgs::PointCloud2类型，再通过发布者ndt_map_pub进行发布。体素滤波降采样的原理是将点云空间切割成大小相等的网格（可以是长方体或是正方体），用网格中所有点的重心替代所有的点，从而减少点云的数量。7475节点ndt_mapping与源码解析（2）关键子函数解析4.2点云地图构建功能包……staticvoidoutput_callback(constautoware_config_msgs::ConfigNDTMappingOutput::ConstPtr&input){……if(filter_res==0.0){std::cout<<"Original:"<<map_ptr->points.size()<<"points."<<std::endl;pcl::toROSMsg(*map_ptr,*map_msg_ptr);}else{pcl::VoxelGrid<pcl::PointXYZI>voxel_grid_filter;voxel_grid_filter.setLeafSize(filter_res,filter_res,filter_res);voxel_grid_filter.setInputCloud(map_ptr);voxel_grid_filter.filter(*map_filtered);std::cout<<"Original:"<<map_ptr->points.size()<<"points."<<std::endl;std::cout<<"Filtered:"<<map_filtered->points.size()<<"points."<<std::endl;pcl::toROSMsg(*map_filtered,*map_msg_ptr);}ndt_map_pub.publish(*map_msg_ptr);……}……ndt_mapping.cpp文本中output_callback函数Part2output_callback函数1）利用pcl::VoxelGrid<pcl::PointXYZI>声明体素滤波对象voxel_grid_filter；2）利用voxel_grid_filter.setLeafSize设置体素滤波网格大小，代码中将网格设置为边长filter_res的正方体；3）利用voxel_grid_filter.setInputCloud将map_ptr作为输入地图；4）利用voxel_grid_filter.filter进行体素滤波降采样，并将滤波结果保存至*map_filtered；5）使用std::cout输出原始点云尺寸和体素滤波降采样后的点云尺寸。76节点ndt_mapping与源码解析（2）关键子函数解析4.2点云地图构建功能包……staticvoidoutput_callback(constautoware_config_msgs::ConfigNDTMappingOutput::ConstPtr&input){……//WritingPointClouddatatoPCDfileif(filter_res==0.0){pcl::io::savePCDFileASCII(filename,*map_ptr);std::cout<<"Saved"<<map_ptr->points.size()<<"datapointsto"<<filename<<"."<<std::endl;}else{pcl::io::savePCDFileASCII(filename,*map_filtered);std::cout<<"Saved"<<map_filtered->points.size()<<"datapointsto"<<filename<<"."