1.mian函數(shù)：初始化參數(shù)

初始化previous_pose,current _pose,ndt_pose,guess_pose,diff（前后兩幀的位置與角度變化）,offset（位置與角度的偏差矯正）

根據(jù)launch文件初始化參數(shù)method_type,use_odom,use_imu,imu_upside_down,imu_topic,incremental_voxel_update

申明發(fā)布的話題有ndt_map,current_pose；

訂閱的話題有config/ndt_mapping，回調(diào)函數(shù)param_callback

config/ndt_mapping_output，回調(diào)函數(shù)output_callback

points_raw，回調(diào)函數(shù)points_callback

/vehicle/odom，回調(diào)函數(shù)odom_callback

_imu_topic，回調(diào)函數(shù)imu_callback

2.param_callback函數(shù)：設(shè)置NDT參數(shù)

設(shè)置NDT算法配準時的參數(shù)：牛頓優(yōu)化的最大步長step_size，用于判斷是否收斂至與之的trans_eps；優(yōu)化迭代的最大次數(shù)max_iter；體素濾波葉的大小voxel_leaf_size用于原始點云過濾；激光點云數(shù)據(jù)有效掃描距離min_scan_range，max_scan_range；前后兩幀距離閾值min_add_scan_shift

3.output_callback函數(shù)：輸出點云建圖

從參數(shù)文件讀取過濾分辨率參數(shù)filter_res，把原始地圖map實例化給map_ptr，聲明過濾地圖的指針map_filtered

如果分辨率為0，map_ptr轉(zhuǎn)化為ros數(shù)據(jù)map_msg_ptr

否則把map_ptr體素濾波后到map_filtered，再轉(zhuǎn)換到map_msg_ptr

發(fā)布*map_msg_ptr到ndt_map話題

最后把點云數(shù)據(jù)寫入到pcd文件中

4.odom_callback函數(shù)：根據(jù)odom得到NDT初值

接受里程計數(shù)據(jù)時間戳作為輸入?yún)?shù)，調(diào)用odom_calc函數(shù)得到NDT初始位姿估計

5.imu_callback函數(shù)：處理imu數(shù)據(jù)

計算前后兩幀時間差，根據(jù)輸入的四元數(shù)轉(zhuǎn)化為rpy角得到前后兩幀rpy角度偏差，然后計算的阿斗imu三個軸上的角速度，調(diào)用imu_calc計算位置初值

6.points_callback函數(shù)：處理點云數(shù)據(jù)

輸入sensor_msgs::PointCloud2::ConstPtr& input

pcl::fromROSMsg(*input,tmp);//將當(dāng)前點云信息轉(zhuǎn)化為pcl類型存入tmp

然后把tmp中的點逐一處理，在一定范圍內(nèi)的點云push_back進scan,再實例化到scan_ptr

如果當(dāng)前地圖為空，通過tf_btol把scan_ptr轉(zhuǎn)換到全局地圖中的transformed_scan_ptr，然后map+=*transformed_scan_ptr，再把標志位置為1

對scan_ptr進行體素化濾波到filtered_scan_ptr

把點云map實例化給map_ptr（pcl::PointCloud<pcl::PointXYZI>::Ptr）

進行NDT參數(shù)設(shè)置，把map_ptr設(shè)置為目標點云

預(yù)測當(dāng)前位置（imu,里程計，imu+里程計）給NDT作為初值

根據(jù)_method_type的類型進行NDT配準結(jié)果輸出到output_cloud點云中，并得到轉(zhuǎn)換矩陣t_localizer，從而根據(jù)t_base_link=t_localizer*tf_ltob得到當(dāng)前點云到全局坐標的轉(zhuǎn)換t_base_link

把原始點云scan_ptr經(jīng)過NDT變換后到transformed_scan_ptr

根據(jù)轉(zhuǎn)換矩陣t_base_link更新當(dāng)前幀小車在全局下的位姿ndt_pose，然后設(shè)置為當(dāng)前位置current_pose

計算當(dāng)前幀與上一幀的距離，如果大于min_add_scan_shift，則將transformed_scan_ptr加入到map中

再把map_ptr轉(zhuǎn)換到ROS數(shù)據(jù)類型map_msg_ptr發(fā)布到ndt_map話題，再把當(dāng)前位姿發(fā)布到current_pose話題上