autoware的icp_maching解读

param_callback：变量初始化函数：

根据gnss数据的变动和最初定位数据的变化来更新最初定位

initial_pose ：最初定位

localizer_pose：icp定位出来的位置

previous_pose：以前的位置

current_pose：当前的位置

 

map_callback：地图更新：

只接收一次地图，并添加到icp配准器里面

 

gnss_callback：惯导回调数据：

获取当前惯导的gps位置和位姿数据，持续更新previous_gnss_pose为当前惯导数据，当开启gnss并且没有初始化变量 或者 icp配准打分超过阈值时更新以下数据：

previous_pose:更新为previous_gnss_pose数据

current_pose：更新为当前惯导数据

offset：current_pose-previous_pose

 

initialpose_callback：

不知谁来发送，不知发送频率

会利用收到的数据来更新previous_pose和current_pose还有offset

 

points_callback：icp配准回调

使用previous_pose加offset_x 来更新predict_pose预测值

setMaximumIterations：设置迭代次数

setTransformationEpsilon：设置终止条件设置最小转换差异

setMaxCorrespondenceDistance：设置点云距离最大对应距离，距离过大的将被忽略

setEuclideanFitnessEpsilon：设置欧几里得距离差

setRANSACOutlierRejectionThreshold：未知

 

 

计算icp匹配出来的icp_pose和previous_pose的误差

本来是根据匹配出来的误差来判定 使用 icp_pose还是previous_pose来更新current_pose 但是这个代码强制使用icp_pose来更新current_pose，不明白为什么。

    predict_pose_error = sqrt((icp_pose.x - predict_pose.x) * (icp_pose.x - predict_pose.x) +
                              (icp_pose.y - predict_pose.y) * (icp_pose.y - predict_pose.y) +
                              (icp_pose.z - predict_pose.z) * (icp_pose.z - predict_pose.z));

    if (predict_pose_error <= PREDICT_POSE_THRESHOLD)
    {
      use_predict_pose = 0;
    }
    else
    {
      use_predict_pose = 1;
    }
    use_predict_pose = 0;

    if (use_predict_pose == 0)
    {
      current_pose.x = icp_pose.x;
      current_pose.y = icp_pose.y;
      current_pose.z = icp_pose.z;
      current_pose.roll = icp_pose.roll;
      current_pose.pitch = icp_pose.pitch;
      current_pose.yaw = icp_pose.yaw;
    }
    else
    {
      current_pose.x = predict_pose.x;
      current_pose.y = predict_pose.y;
      current_pose.z = predict_pose.z;
      current_pose.roll = predict_pose.roll;
      current_pose.pitch = predict_pose.pitch;
      current_pose.yaw = predict_pose.yaw;
    }

然后计算这次和上次位置的误差来更新当前的一些数据

scan_duration：这次匹配所用时间

diff：这次运动的距离

current_velocity：当前的速度

current_velocity_smooth：取连续3次速度的平均值，如果小于0.2 就设为0.0

current_accel:当前加速度

estimated_vel_mps：当前速度m/s，并发布出去

estimated_vel_kmph：当前速度km/h，并发布出去

发布predict_pose_pub，icp_pose_pub，localizer_pose_pub

发布"/map" 到 "/base_link"的tf变换

发布time_icp_matching_pub（icp匹配所花时间）

发布estimate_twist_pub，estimated_vel_pub（当前时刻速度）

发布icp_stat_pub（icp的一些数据）

写入icp日志

更新offset（此次移动的距离）

更新previous_pose，previous_scan_time，previous_velocity等数据