Autoware：ndt_mapping节点

关于地图map的部分

1. 地图的定义：定义全局变量map；

static pcl::PointCloud<pcl::PointXYZI> map;

2. 在main函数中，创建与地图有关的发布者对象ndt_map_pub，发布的消息名称为：/ndt_map；

  ndt_map_pub = nh.advertise<sensor_msgs::PointCloud2>("/ndt_map", 1000);

3. 在main函数中，创建一个订阅者对象output_sub，订阅话题config/ndt_mapping_output，输该输入是一个地图map，并通过回调函数output_callback进行操作；

  ros::Subscriber output_sub = nh.subscribe("config/ndt_mapping_output", 10, output_callback);

4. 在output_callback函数中，主要目的是对输入的地图进行滤波处理，体素滤波完后，再利用前面创建的发布者对象ndt_map_pub将地图发布出去，消息名称为：/ndt_map。（tips: 因为节点句柄为 nh，因此该消息的命名空间就是：/话题名）；

pcl::toROSMsg(*map_filtered, *map_msg_ptr);
// 发布之前先进行从pcl数据格式到ros消息类型的转换
ndt_map_pub.publish(*map_msg_ptr);

处理每一帧点云的points_callback函数

1. 在主函数中进行定义，创建订阅者对象points_sub，用于接收每一帧点云数据points_raw，并通过函数points_callback对点云进行处理；

ros::Subscriber points_sub = nh.subscribe("points_raw", 100000, points_callback);

看一下该回调函数（此函数是ndt_mapping节点的最主要部分，代码量也很多，450~850一共400多行），此函数的输入是points_raw，即input：

static void points_callback(const sensor_msgs::PointCloud2::ConstPtr& input)

2. 首先对点云进行过远点和过近点的滤除，得到新的点云scan_ptr；

// 对输入每一帧点云的操作
static void points_callback(const sensor_msgs::PointCloud2::ConstPtr& input)
{double r;pcl::PointXYZI p;pcl::PointCloud<pcl::PointXYZI> tmp, scan;// 用于接收从雷达消息过来的pcl数据格式的转换pcl::PointCloud<pcl::PointXYZI>::Ptr filtered_scan_ptr(new pcl::PointCloud<pcl::PointXYZI>());pcl::PointCloud<pcl::PointXYZI>::Ptr transformed_scan_ptr(new pcl::PointCloud<pcl::PointXYZI>());tf::Quaternion q;Eigen::Matrix4f t_localizer(Eigen::Matrix4f::Identity());Eigen::Matrix4f t_base_link(Eigen::Matrix4f::Identity());static tf::TransformBroadcaster br;tf::Transform transform;current_scan_time = input->header.stamp;pcl::fromROSMsg(*input, tmp);for (pcl::PointCloud<pcl::PointXYZI>::const_iterator item = tmp.begin(); item != tmp.end(); item++){p.x = (double)item->x;p.y = (double)item->y;p.z = (double)item->z;p.intensity = (double)item->intensity;r = sqrt(pow(p.x, 2.0) + pow(p.y, 2.0));// 滤除掉距离过近或过远的点，存到新的点云scan中if (min_scan_range < r && r < max_scan_range){scan.push_back(p);}}// 点云scan转换为智能指针scan_ptrpcl::PointCloud<pcl::PointXYZI>::Ptr scan_ptr(new pcl::PointCloud<pcl::PointXYZI>(scan));

3. 检查地图是否完成了初始化，如果没有，就利用初始的变换（零变换）对第一帧点云进行变换，变换后的点云为transformed_scan_ptr，再将transformed_scan_ptr加入到地图map中，完成第一帧对地图的初始化；

// 检查地图是否进行初始化，也即是否为空if (initial_scan_loaded == 0){pcl::transformPointCloud(*scan_ptr, *transformed_scan_ptr, tf_btol);// 如果map为空，则将新的点云scan_ptr作为输入点云，tf_btol作为变换（全局变量），变换后的点云为transformed_scan_ptrmap += *transformed_scan_ptr;// 再将transformed_scan_ptr加入到地图map中，完成第一帧对地图的初始化initial_scan_loaded = 1;}

4. 随后再对点云进行体素滤波处理，滤波完成后的点云为filtered_scan_ptr；

// 运用体素滤波对每一帧点云进行滤波，滤波完成后的点云为filtered_scan_ptrpcl::VoxelGrid<pcl::PointXYZI> 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);

5. 配置点云配准的参数；

  pcl::PointCloud<pcl::PointXYZI>::Ptr map_ptr(new pcl::PointCloud<pcl::PointXYZI>(map));// 进行点云配准的方法与参数：迭代步长、分辨率、次数以及输入的源点云filtered_scan_ptrif (_method_type == MethodType::PCL_GENERIC){ndt.setTransformationEpsilon(trans_eps);ndt.setStepSize(step_size);ndt.setResolution(ndt_res);ndt.setMaximumIterations(max_iter);ndt.setInputSource(filtered_scan_ptr);}

6. 用于配准的输入点云的变化：如下图所示；

输入的一帧点云经过上述变换后，才成为ndt配准的点云；

7. 用于迭代的初始位姿计算，以没有odom没有imu的情况为例：

迭代的初始位姿 = 上一时刻的位姿 + 变化量

变化量diff采用匀速运动模型进行计算：

变化量diff = 当前位姿current_pose - 上一时刻位姿previous_pose

 // 用于迭代的初始位姿 = 上一时刻的位姿 + 变化量// guess_pose为静态的全局变量，只在此文件内起作用// 关于几个pose：guess_pose, guess_pose_imu, guess_pose_odom, guess_pose_imu_odom// 分别为没有任何设备下的迭代初始位姿和只有imu、只有里程计和有imu和里程计的迭代初始位姿guess_pose.x = previous_pose.x + diff_x;// diff_x = current_pose.x - previous_pose.x; 匀速运动模型// previous_pose为通过配准得到的位姿guess_pose.y = previous_pose.y + diff_y;guess_pose.z = previous_pose.z + diff_z;guess_pose.roll = previous_pose.roll;guess_pose.pitch = previous_pose.pitch;guess_pose.yaw = previous_pose.yaw + diff_yaw;

将用于迭代的位姿映射到ndt算法中：

  pose guess_pose_for_ndt;if (_use_imu == true && _use_odom == true)guess_pose_for_ndt = guess_pose_imu_odom;else if (_use_imu == true && _use_odom == false)guess_pose_for_ndt = guess_pose_imu;else if (_use_imu == false && _use_odom == true)guess_pose_for_ndt = guess_pose_odom;elseguess_pose_for_ndt = guess_pose;// 将没有任何先验信息的位姿估计赋值给guess_pose_for_ndt

8. 用于坐标变换的矩阵:

变换矩阵4*4 = 平移向量 * 旋转向量 * 缩放向量

其中旋转向量通过欧拉角来定义，公式如下：

Eigen::AngleAxisf init_rotation_x(guess_pose_for_ndt.roll, Eigen::Vector3f::UnitX());Eigen::AngleAxisf init_rotation_y(guess_pose_for_ndt.pitch, Eigen::Vector3f::UnitY());Eigen::AngleAxisf init_rotation_z(guess_pose_for_ndt.yaw, Eigen::Vector3f::UnitZ());Eigen::Translation3f init_translation(guess_pose_for_ndt.x, guess_pose_for_ndt.y, guess_pose_for_ndt.z);// 构建迭代初始位姿的坐标变换矩阵4*4： 平移向量*旋转向量*缩放向量Eigen::Matrix4f init_guess =(init_translation * init_rotation_z * init_rotation_y * init_rotation_x).matrix() * tf_btol;

至此，用于ndt_mapping算法的所有准备工作就绪

9. 进行点云配准：

pcl::PointCloud<pcl::PointXYZI>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZI>);if (_method_type == MethodType::PCL_GENERIC){ndt.align(*output_cloud, init_guess);fitness_score = ndt.getFitnessScore();t_localizer = ndt.getFinalTransformation();has_converged = ndt.hasConverged();final_num_iteration = ndt.getFinalNumIteration();transformation_probability = ndt.getTransformationProbability();}

其中，输入点云经过点云配准，由init_guess变换为矩阵output_cloud，这一过程的变换矩阵存储于t_localizer中，t_localizer表示从map到雷达定位器的坐标转换；

t_localizer = anh_ndt.getFinalTransformation();// 保存坐标变换的矩阵，变换为map到雷达坐标系

为了计算小车底盘到map的变换，需要引入车底盘到雷达定位器的坐标变换：

  t_base_link = t_localizer * tf_ltob;// t_base_link为map到base_link的坐标变换// tf_ltob为base_link到雷达坐标系的变换

10. 地图拼接

得到了坐标变换后，将输入点云通过该变换，变换到map中，实现地图的拼接；

pcl::transformPointCloud(*scan_ptr, *transformed_scan_ptr, t_localizer);// transformed_scan_ptr为scan_ptr经过t_localizer变换后的点云// 这一步主要是将点云变换后，拼接到map中

11. 更新，主要是用于更新下一次的计算初值；

// 更新雷达定位器的位姿xyz，定位localizer_pose.x = t_localizer(0, 3);localizer_pose.y = t_localizer(1, 3);localizer_pose.z = t_localizer(2, 3);mat_l.getRPY(localizer_pose.roll, localizer_pose.pitch, localizer_pose.yaw, 1);// 更新NDT位姿，NDT建图位姿ndt_pose.x = t_base_link(0, 3);ndt_pose.y = t_base_link(1, 3);ndt_pose.z = t_base_link(2, 3);mat_b.getRPY(ndt_pose.roll, ndt_pose.pitch, ndt_pose.yaw, 1);// 将NDT建图位姿映射为当前位姿current_pose.x = ndt_pose.x;current_pose.y = ndt_pose.y;current_pose.z = ndt_pose.z;current_pose.roll = ndt_pose.roll;current_pose.pitch = ndt_pose.pitch;current_pose.yaw = ndt_pose.yaw;diff_x = current_pose.x - previous_pose.x;diff_y = current_pose.y - previous_pose.y;diff_z = current_pose.z - previous_pose.z;diff_yaw = calcDiffForRadian(current_pose.yaw, previous_pose.yaw);diff = sqrt(diff_x * diff_x + diff_y * diff_y + diff_z * diff_z);current_velocity_x = diff_x / secs;current_velocity_y = diff_y / secs;current_velocity_z = diff_z / secs;// 更新操作，将当前位姿存储为上一时刻位姿，用于下一次计算的点云配准迭代过程初始化previous_pose.x = current_pose.x;previous_pose.y = current_pose.y;previous_pose.z = current_pose.z;previous_pose.roll = current_pose.roll;previous_pose.pitch = current_pose.pitch;previous_pose.yaw = current_pose.yaw;

12. 计算配准偏移量shift: 当前时刻和上一时刻之间的偏移距离，目的是：

距离过近时，不往地图上添加，一方面频繁的添加使得计算量偏大，而地图更新不明显；
此外，当小车静止不动时，不往map中添加点云；

// 计算added_pos和current_pos之间的偏移，也即当前时刻和上一时刻之间的偏移距离，欧式距离double shift = sqrt(pow(current_pose.x - added_pose.x, 2.0) + pow(current_pose.y - added_pose.y, 2.0));if (shift >= min_add_scan_shift)// 默认值：static double min_add_scan_shift = 1.0;// 距离过近时，不往地图上添加，一方面频繁的添加使得计算量偏大，而地图更新不明显；// 此外，当小车静止不动时，不往map中添加点云{map += *transformed_scan_ptr;// 当前点云帧与map进行拼接// 更新操作，用于下一时刻计算距离偏移量added_pose.x = current_pose.x;added_pose.y = current_pose.y;added_pose.z = current_pose.z;added_pose.roll = current_pose.roll;added_pose.pitch = current_pose.pitch;added_pose.yaw = current_pose.yaw;

13. 发布消息：

  // 数据类型转换为消息类型，为发布消息做准备sensor_msgs::PointCloud2::Ptr map_msg_ptr(new sensor_msgs::PointCloud2);pcl::toROSMsg(*map_ptr, *map_msg_ptr);ndt_map_pub.publish(*map_msg_ptr);// 发布地图消息current_pose_pub.publish(current_pose_msg);// 发布当前位姿