apollo 定位localization代码NDT算法模块解析(三)

lidar_locator_ndt.cc

---

前言

这个是NDT算法调用的入口函数；

---

相关参数

online_resolution： 激光体素滤波分辨率
ndt_target_resolution：地图体素滤波分辨率
ndt_warnning_ndt_score：NDT最终匹配结果的阈值，警告值（谨慎使用定位结果）
ndt_error_ndt_score：错误阈值，该定位结果不可用
ndt_max_iterations： 最多迭代次数

代码调用关系图：

一、Update()

用组合导航来进行局部定位，利用组合导航数据差值作为当前时刻的预测值；
对输入点云数据进行滤波；
使用预测值，获取对应位置的点云地图，将地图点云转化到激光坐标系,
在激光坐标系下进行激光点云与地图点云的匹配；

int LidarLocatorNdt::Update(unsigned int frame_idx, const Eigen::Affine3d& pose,
                            const LidarFrame& lidar_frame) 
  //当前与上次时间间隔内的组合导航位置差，局部定位
  Eigen::Vector3d trans_diff =
      pose.translation() - pre_input_location_.translation();
  //上一时刻NDT定位结果+组合导航差值作为预测
  Eigen::Vector3d trans_pre_local =
      pre_estimate_location_.translation() + trans_diff;
  Eigen::Quaterniond quatd(pose.linear());
  Eigen::Translation3d transd(trans_pre_local);
  Eigen::Affine3d center_pose = transd * quatd;//平移+旋转//得到全局位置的仿射变换矩阵

  Eigen::Quaterniond pose_qbn(pose.linear());
  AINFO << "original pose: " << std::setprecision(15) << pose.translation()[0]
        << ", " << pose.translation()[1] << ", " << pose.translation()[2]
        << ", " << pose_qbn.x() << ", " << pose_qbn.y() << ", " << pose_qbn.z()
        << ", " << pose_qbn.w();

  // Get lidar pose Twv = Twb * Tbv
  Eigen::Affine3d transform = center_pose * velodyne_extrinsic_;//变换矩阵加上激光与组合导航的安装误差（相当于先将点云坐标系laser转化到组合导航本体坐标系gnss再转化到全局UTM坐标系）
  predict_location_ = center_pose;//激光点云定位的预测位置

// Pre-load the map nodes
//加载当前预测位置对应的周边地图
#ifdef USE_PRELOAD_MAP_NODE
  bool map_is_ready =
      map_.LoadMapArea(center_pose.translation(), resolution_id_, zone_id_,
                       filter_x_, filter_y_);
  map_.PreloadMapArea(center_pose.translation(), trans_diff, resolution_id_,
                      zone_id_);
#endif

  // Online pointcloud are projected into a ndt map node. (filtered)
  //获取在地图上的位置，不同的高精度地图格式有不同的表达形式
  double lt_x = pose.translation()[0];
  double lt_y = pose.translation()[1];
  double map_resolution = map_.GetMapConfig().map_resolutions_[resolution_id_];
  lt_x -= (map_.GetMapConfig().map_node_size_x_ * map_resolution / 2.0);
  lt_y -= (map_.GetMapConfig().map_node_size_y_ * map_resolution / 2.0);

  // Start Ndt method
  // Convert online points to pcl pointcloud
  apollo::common::time::Timer online_filtered_timer;
  online_filtered_timer.Start(); //滤波开始时间
  //激光点云数据
  pcl::PointCloud<pcl::PointXYZ>::Ptr online_points(
      new pcl::PointCloud<pcl::PointXYZ>());
  for (unsigned int i = 0; i < lidar_frame.pt_xs.size(); ++i) {
    pcl::PointXYZ p(lidar_frame.pt_xs[i], lidar_frame.pt_ys[i],
                    lidar_frame.pt_zs[i]);
    online_points->push_back(p);
  }
  // Filter online points
  //使用体素滤波对点云数据进行滤波或下采样
  //直接调用pcl库函数
  AINFO << "Online point cloud leaf size: " << proj_reslution_;
  pcl::PointCloud<pcl::PointXYZ>::Ptr online_points_filtered(
      new pcl::PointCloud<pcl::PointXYZ>());
  pcl::VoxelGrid<pcl::PointXYZ> sor;//创建滤波对象
  sor.setInputCloud(online_points);//输入点云，//设置需要过滤的点云给滤波对象
  sor.setLeafSize(proj_reslution_, proj_reslution_, proj_reslution_);//体素滤波大小//设置滤波时创建的体素体积为1m的立方体,1m感觉有点大
  sor.filter(*online_points_filtered);//执行滤波处理，存储输出
  AINFO << "Online Pointcloud size: " << online_points->size() << "/"
        << online_points_filtered->size();
  online_filtered_timer.End("online point calc end.");

  //  Obtain map pointcloud
  //获取地图点云，并将地图点云数据转化到激光坐标系
  apollo::common::time::Timer map_timer;
  map_timer.Start();
  Eigen::Vector2d left_top_coord2d(lt_x, lt_y);
  ComposeMapCells(left_top_coord2d, zone_id_, resolution_id_,
                  map_.GetMapConfig().map_resolutions_[resolution_id_],
                  transform.inverse());// Convert map pointcloud to local corrdinate
  pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_map_point_cloud(
      new pcl::PointCloud<pcl::PointXYZ>());
  for (unsigned int i = 0; i < cell_map_.size(); ++i) {
    Leaf& le = cell_map_[i];
    float mean_0 = static_cast<float>(le.mean_(0));
    float mean_1 = static_cast<float>(le.mean_(1));
    float mean_2 = static_cast<float>(le.mean_(2));
    pcl_map_point_cloud->push_back(pcl::PointXYZ(mean_0, mean_1, mean_2));
  }
  map_timer.End("Map create end.");
  // Set left top corner for reg
  reg_.SetLeftTopCorner(map_left_top_corner_);
  // Ndt calculation
  reg_.SetInputTarget(cell_map_, pcl_map_point_cloud); //地图点云，目标点
  reg_.SetInputSource(online_points_filtered); //输入实时激光点云，输入点

  apollo::common::time::Timer ndt_timer;
  ndt_timer.Start(); //ndt 开始时间
  Eigen::Matrix3d inv_R = transform.inverse().linear();//逆矩阵的角度
  Eigen::Matrix4d init_matrix = Eigen::Matrix4d::Identity();
  init_matrix.block<3, 3>(0, 0) = inv_R.inverse(); //角度的逆矩阵

  pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(
      new pcl::PointCloud<pcl::PointXYZ>);
  reg_.Align(output_cloud, init_matrix.cast<float>());
  ndt_timer.End("Ndt Align End.");//ndt 结束时间

  fitness_score_ = reg_.GetFitnessScore();
  bool has_converged = reg_.HasConverged();
  int iteration = reg_.GetFinalNumIteration();
  Eigen::Matrix4d ndt_pose = reg_.GetFinalTransformation().cast<double>();
  AINFO << "Ndt summary:";
  AINFO << "Fitness Score: " << fitness_score_;
  AINFO << "Has_converged: " << has_converged;
  AINFO << "Iteration: %d: " << iteration;
  AINFO << "Relative Ndt pose: " << ndt_pose(0, 3) << ", " << ndt_pose(1, 3)
        << ", " << ndt_pose(2, 3);

  // Twv
  Eigen::Affine3d lidar_location = Eigen::Affine3d::Identity();
  lidar_location = transform.matrix() * init_matrix.inverse() * ndt_pose;

  // Save results
  location_ = lidar_location * velodyne_extrinsic_.inverse();
  pre_input_location_ = pose;
  pre_estimate_location_ = location_;
  pre_imu_height_ = location_.translation()(2);

  if (map_is_ready) {
    return 0;
  } else {
    return -1;
  }
  return 0;
}

需要调用ndt_solve 中的以下函数:
// Ndt calculation
reg_.SetInputTarget(cell_map_, pcl_map_point_cloud); //地图点云，目标点
reg_.SetInputSource(online_points_filtered); //输入实时激光点云，输入点
reg_.Align(output_cloud, init_matrix.cast());//匹配
前2个函数将完成NDT算法匹配数据输入。
Align完成算法流程；

关于体素滤波，可参考：
https://www.cnblogs.com/CV-life/p/10142581.html
博客描述简单易懂，也很形象，建议看看。

2, ComposeMapCells()

算法第一步：参考点云网格化；
按照一定的方式分成了9块；
分块方式如下图（手画了一个，对角线连接表示一块）：

分块完成后，会遍历整个区域，由于事先对地图进行了1m * 1m* 1m的体素化处理,所以迭代次数不会特别多。最终得到每个网格的质心坐标（x,y,z），网格内点的个数，均值，方差，并保存在cell_map_中。

inverse_transform ：全局->局部（map->laser）

void LidarLocatorNdt::ComposeMapCells(
    const Eigen::Vector2d& left_top_coord2d, int zone_id,
    unsigned int resolution_id, float map_pixel_resolution,
    const Eigen::Affine3d& inverse_transform) {
  apollo::common::time::Timer timer;
  timer.Start();

  unsigned int map_node_size_x = map_.GetMapConfig().map_node_size_x_;
  unsigned int map_node_size_y = map_.GetMapConfig().map_node_size_y_;
  unsigned int filter_size_x = filter_x_;
  unsigned int filter_size_y = filter_y_;

  // get the left top coordinate of input online pointcloud
  //左边界，当前位置-滤波取值范围
  Eigen::Vector2d coord2d = left_top_coord2d;
  coord2d[0] -= map_pixel_resolution * static_cast<float>(filter_size_x / 2);
  coord2d[1] -= map_pixel_resolution * static_cast<float>(filter_size_y / 2);

  // get the node index of left top corner and global coordinate
  MapNodeIndex map_id = MapNodeIndex::GetMapNodeIndex(
      map_.GetMapConfig(), coord2d, resolution_id, zone_id);
  NdtMapNode* map_node_lt =
      dynamic_cast<NdtMapNode*>(map_.GetMapNodeSafe(map_id));
  assert(map_.IsMapNodeExist(map_id));
  unsigned int map_coord_x = 0;
  unsigned int map_coord_y = 0;
  //获取左边界地图像素点坐标
  map_node_lt->GetCoordinate(coord2d, &map_coord_x, &map_coord_y);

  std::vector<std::vector<int>> map_nodes_zones;

  int top_left_start_x = static_cast<int>(map_coord_x);
  int top_left_start_y = static_cast<int>(map_coord_y);
  //右边界，地图最右边
  int top_left_end_x = static_cast<int>(map_node_size_x) - 1;
  int top_left_end_y = static_cast<int>(map_node_size_y) - 1;
  std::vector<int> node_zone;
  //开始分块
  node_zone.push_back(top_left_start_x);
  node_zone.push_back(top_left_start_y);
  node_zone.push_back(top_left_end_x);
  node_zone.push_back(top_left_end_y);
  map_nodes_zones.push_back(node_zone); //取值范围

  int top_center_start_x = 0;
  int top_center_start_y = top_left_start_y;
  int top_center_end_x = static_cast<int>(map_coord_x) - 1 +
                         2 * static_cast<int>(filter_size_x / 2);
  top_center_end_x = top_center_end_x > static_cast<int>(map_node_size_x) - 1
                         ? static_cast<int>(map_node_size_x) - 1
                         : top_center_end_x;
  int top_center_end_y = static_cast<int>(map_node_size_y) - 1;
  node_zone.clear();
  node_zone.push_back(top_center_start_x);
  node_zone.push_back(top_center_start_y);
  node_zone.push_back(top_center_end_x);
  node_zone.push_back(top_center_end_y);
  map_nodes_zones.push_back(node_zone);

  int top_right_start_x = 0;
  int top_right_start_y = top_left_start_y;
  int top_right_end_x = 2 * static_cast<int>(filter_size_x / 2) -
                        (top_left_end_x - top_left_start_x + 1) - 1;
  int top_right_end_y = static_cast<int>(map_node_size_y) - 1;
  node_zone.clear();
  node_zone.push_back(top_right_start_x);
  node_zone.push_back(top_right_start_y);
  node_zone.push_back(top_right_end_x);
  node_zone.push_back(top_right_end_y);
  map_nodes_zones.push_back(node_zone);

  int middle_left_start_x = top_left_start_x;
  int middle_left_start_y = 0;
  int middle_left_end_x = top_left_end_x;
  int middle_left_end_y = static_cast<int>(map_coord_y) - 1 +
                          2 * static_cast<int>(filter_size_y / 2);
  middle_left_end_y = middle_left_end_y > static_cast<int>(map_node_size_y) - 1
                          ? static_cast<int>(map_node_size_y) - 1
                          : middle_left_end_y;
  node_zone.clear();
  node_zone.push_back(middle_left_start_x);
  node_zone.push_back(middle_left_start_y);
  node_zone.push_back(middle_left_end_x);
  node_zone.push_back(middle_left_end_y);
  map_nodes_zones.push_back(node_zone);

  int middle_center_start_x = 0;
  int middle_center_start_y = 0;
  int middle_center_end_x = top_center_end_x;
  int middle_center_end_y = middle_left_end_y;
  node_zone.clear();
  node_zone.push_back(middle_center_start_x);
  node_zone.push_back(middle_center_start_y);
  node_zone.push_back(middle_center_end_x);
  node_zone.push_back(middle_center_end_y);
  map_nodes_zones.push_back(node_zone);

  int middle_right_start_x = 0;
  int middle_right_start_y = 0;
  int middle_right_end_x = top_right_end_x;
  int middle_right_end_y = middle_center_end_y;
  node_zone.clear();
  node_zone.push_back(middle_right_start_x);
  node_zone.push_back(middle_right_start_y);
  node_zone.push_back(middle_right_end_x);
  node_zone.push_back(middle_right_end_y);
  map_nodes_zones.push_back(node_zone);

  int bottom_left_start_x = top_left_start_x;
  int bottom_left_start_y = 0;
  int bottom_left_end_x = top_left_end_x;
  int bottom_left_end_y = 2 * static_cast<int>(filter_size_y / 2) -
                          (top_left_end_y - top_left_start_y + 1) - 1;
  node_zone.clear();
  node_zone.push_back(bottom_left_start_x);
  node_zone.push_back(bottom_left_start_y);
  node_zone.push_back(bottom_left_end_x);
  node_zone.push_back(bottom_left_end_y);
  map_nodes_zones.push_back(node_zone);

  int bottom_center_start_x = middle_center_start_x;
  int bottom_center_start_y = bottom_left_start_y;
  int bottom_center_end_x = middle_center_end_x;
  int bottom_center_end_y = bottom_left_end_y;
  node_zone.clear();
  node_zone.push_back(bottom_center_start_x);
  node_zone.push_back(bottom_center_start_y);
  node_zone.push_back(bottom_center_end_x);
  node_zone.push_back(bottom_center_end_y);
  map_nodes_zones.push_back(node_zone);

  int bottom_right_start_x = middle_right_start_x;
  int bottom_right_start_y = bottom_left_start_y;
  int bottom_right_end_x = middle_right_end_x;
  int bottom_right_end_y = bottom_left_end_y;
  node_zone.clear();
  node_zone.push_back(bottom_right_start_x);
  node_zone.push_back(bottom_right_start_y);
  node_zone.push_back(bottom_right_end_x);
  node_zone.push_back(bottom_right_end_y);
  map_nodes_zones.push_back(node_zone);
//分块完成
//不知道为什么要分块？关键分块之后有很多重复的，为什么要重复,因为是3维立体的原因，转化到2维？

  std::vector<int> start_x_indices(3, 0);
  std::vector<int> start_y_indices(3, 0);
  start_x_indices[0] = 0;
  start_x_indices[1] = top_left_end_x - top_left_start_x + 1;
  start_x_indices[2] =
      start_x_indices[1] + top_center_end_x - top_center_start_x + 1;
  start_y_indices[0] = 0;
  start_y_indices[1] = top_left_end_y - top_left_start_y + 1;
  start_y_indices[2] =
      start_y_indices[1] + middle_center_end_y - middle_center_start_y + 1;

  std::vector<std::pair<int, int>> composed_map_indices;
  for (int y = 0; y < 3; ++y) {
    for (int x = 0; x < 3; ++x) {
      composed_map_indices.push_back(
          std::make_pair(start_x_indices[x], start_y_indices[y]));
    }
  }

  Eigen::Vector2d coord2d_center =
      map_node_lt->GetCoordinate(map_node_size_x / 2, map_node_size_y / 2);

  cell_map_.clear();
  Eigen::Affine3d transform = inverse_transform.inverse();
  Eigen::Vector3d R_inv_t = -transform.translation();
  // Calculate left top corner
  map_left_top_corner_ = Eigen::Vector3d::Zero();
  map_left_top_corner_.block<2, 1>(0, 0) = map_node_lt->GetLeftTopCorner();
  map_left_top_corner_ += R_inv_t;

  for (int y = 0; y < 3; ++y) {
    for (int x = 0; x < 3; ++x) {
      if (map_nodes_zones[y * 3 + x][2] - map_nodes_zones[y * 3 + x][0] >= 0 &&
          map_nodes_zones[y * 3 + x][3] - map_nodes_zones[y * 3 + x][1] >= 0) {
        // get map node
        NdtMapNode* map_node_ptr = nullptr;
        if (x == 0 && y == 0) {
          map_node_ptr = map_node_lt;
        } else {
          Eigen::Vector2d coord2d_xy;
          coord2d_xy[0] =
              coord2d_center[0] + static_cast<double>(x * map_node_size_x) *
                                      static_cast<double>(map_pixel_resolution);
          coord2d_xy[1] =
              coord2d_center[1] + static_cast<double>(y * map_node_size_y) *
                                      static_cast<double>(map_pixel_resolution);

          MapNodeIndex map_id = MapNodeIndex::GetMapNodeIndex(
              map_.GetMapConfig(), coord2d_xy, resolution_id, zone_id);
          NdtMapNode* map_node_xy =
              dynamic_cast<NdtMapNode*>(map_.GetMapNodeSafe(map_id));
          assert(map_.IsMapNodeExist(map_id));
          map_node_ptr = map_node_xy;
        }

        // get map matrix
        NdtMapMatrix& map_cells =
            dynamic_cast<NdtMapMatrix&>(map_node_ptr->GetMapCellMatrix());

        // start obtain cells in MapNdtMatrix
        const Eigen::Vector2d& left_top_corner =
            map_node_ptr->GetLeftTopCorner();
        double resolution = map_node_ptr->GetMapResolution();
        double resolution_z = map_node_ptr->GetMapResolutionZ();
        //获取对应网格的质心坐标，以及网格内的点云数，均值，方差
        for (int map_y = map_nodes_zones[y * 3 + x][1];
             map_y <= map_nodes_zones[y * 3 + x][3]; ++map_y) {
          for (int map_x = map_nodes_zones[y * 3 + x][0];
               map_x <= map_nodes_zones[y * 3 + x][2]; ++map_x) {
            const NdtMapCells& cell_ndt = map_cells.GetMapCell(map_y, map_x);
            if (cell_ndt.cells_.size() > 0) {
              for (auto it = cell_ndt.cells_.begin();
                   it != cell_ndt.cells_.end(); ++it) {
                unsigned int cell_count = it->second.count_;
                if (cell_count >= 6) {
                  Leaf leaf;
                  leaf.nr_points_ = static_cast<int>(cell_count);

                  Eigen::Vector3d eigen_point(Eigen::Vector3d::Zero());
                  eigen_point(0) = left_top_corner[0] + map_x * resolution +
                                   it->second.centroid_[0];
                  eigen_point(1) = left_top_corner[1] + map_y * resolution +
                                   it->second.centroid_[1];
                  eigen_point(2) =
                      resolution_z * it->first + it->second.centroid_[2];
                  leaf.mean_ = (eigen_point + R_inv_t);
                  if (it->second.is_icov_available_ == 1) {
                    leaf.icov_ = it->second.centroid_icov_.cast<double>();
                  } else {
                    leaf.nr_points_ = -1;
                  }

                  cell_map_.push_back(leaf);
                }
              }
            }
          }
        }
      }
    }
  }

  timer.End("Compose map cells.");
}

总结

版权申明：转载请注明出处，严禁用于商业用途。