多激光雷达外参标定算法与源码解析(一):基于BLAM的建图模块

前言

原理文字介绍略微简介，但是在代码注释中非常详细。我相信在代码中学习原理才能理解更加通透。
代码参考自livox sdk:
gitcode

一、算法原理

二、源码解析

函数流:main->BlamSlam::ProcessPointCloudMessage
点云处理流程为:滤波->帧间匹配->帧图匹配->插入地图

滤波类PointCloudFilter

函数流:BlamSlam::ProcessPointCloudMessage->PointCloudFilter::Filter

/**
 * 滤波
 * @param points 输入
 * @param points_filtered 输出
 * @return 
 */
bool PointCloudFilter::Filter(const PointCloud::ConstPtr& points,
                              PointCloud::Ptr points_filtered) const 

根据参数选择各种下采样、噪点滤除操作。详情见代码和注释:

  // Apply a random downsampling filter to the incoming point cloud.
  if (params_.random_filter) {
    const int n_points = static_cast((1.0 - params_.decimate_percentage) *
                                          points_filtered->size());
	pcl::RandomSample random_filter;
    random_filter.setSample(n_points);
    random_filter.setInputCloud(points_filtered);
    random_filter.filter(*points_filtered);
  }

  // Apply a voxel grid filter to the incoming point cloud.
  if (params_.grid_filter) {
	  pcl::VoxelGrid grid;
    grid.setLeafSize(params_.grid_res, params_.grid_res, params_.grid_res);
    grid.setInputCloud(points_filtered);
    grid.filter(*points_filtered);
  }

  // Remove statistical outliers in incoming the point cloud.
  if (params_.outlier_filter) {
	  pcl::StatisticalOutlierRemoval sor;
    sor.setInputCloud(points_filtered);
    sor.setMeanK(params_.outlier_knn);
    sor.setStddevMulThresh(params_.outlier_std);
    sor.filter(*points_filtered);
  }

  // Remove points without a threshold number of neighbors within a specified
  // radius.
  if (params_.radius_filter) {
	  pcl::RadiusOutlierRemoval rad;
    rad.setInputCloud(points_filtered);
    rad.setRadiusSearch(params_.radius);
    rad.setMinNeighborsInRadius(params_.radius_knn);
    rad.filter(*points_filtered);
  }

前端里程计类PointCloudOdometry

第一帧不需要icp匹配，直接插入地图

if (!odometry_.UpdateEstimate(*msg_filtered)) {
            PointCloud::Ptr unused(new PointCloud);
            mapper_.InsertPoints(msg_filtered, unused.get());
            //loop_closure_.AddKeyScanPair(0, msg);
            return;
        }

函数流:BlamSlam::ProcessPointCloudMessage->PointCloudOdometry::UpdateEstimate->PointCloudOdometry::UpdateICP
如果非第一帧，则开始gicp匹配

bool PointCloudOdometry::UpdateICP() {
    {
        TicToc step("odicp");
        // Compute the incremental transformation.
        GeneralizedIterativeClosestPoint icp;
        icp.setTransformationEpsilon(params_.icp_tf_epsilon);
        icp.setMaxCorrespondenceDistance(params_.icp_corr_dist);
        icp.setMaximumIterations(params_.icp_iterations);
        icp.setRANSACIterations(0);

        icp.setInputSource(query_);
        icp.setInputTarget(reference_);

        PointCloud unused_result;
        icp.align(unused_result);

        const Eigen::Matrix4f T = icp.getFinalTransformation();

        // Update pose estimates.
        incremental_estimate_.translation = gu::Vec3(T(0, 3), T(1, 3), T(2, 3));
        incremental_estimate_.rotation = gu::Rot3(T(0, 0), T(0, 1), T(0, 2),
                                                  T(1, 0), T(1, 1), T(1, 2),
                                                  T(2, 0), T(2, 1), T(2, 2));
    }
  // Only update if the incremental transform is small enough. 目测位姿增量太大就不要
  if (!transform_thresholding_ ||
      (incremental_estimate_.translation.Norm() <= max_translation_ &&
       incremental_estimate_.rotation.ToEulerZYX().Norm() <= max_rotation_)) {
    integrated_estimate_ =
        gu::PoseUpdate(integrated_estimate_, incremental_estimate_);//将本次两帧间位姿集成到全局位姿上
  } else {
      std::cout<<"这一帧不靠谱！！！"<

其中query_为当前帧，reference_为前一帧，如果位姿增量在合理范围，就认为匹配有效。

地图类PointCloudMapper

函数流:BlamSlam::ProcessPointCloudMessage->PointCloudMapper::InsertPoints
把点云插入地图

bool PointCloudMapper::InsertPoints(const PointCloud::ConstPtr& points,
                                    PointCloud* incremental_points) {
  if (!initialized_) {
    return false;
  }
  if (incremental_points == NULL) {
    return false;
  }
  incremental_points->clear();
  if (map_mutex_.try_lock()) {
    for (size_t ii = 0; ii < points->points.size(); ++ii) {
		const pcl::POINT_TYPE p = points->points[ii];
//      if (!map_octree_->isVoxelOccupiedAtPoint(p)) {//会报错，百度修改后如下
      double min_x, min_y, min_z, max_x, max_y, max_z;
      map_octree_->getBoundingBox(min_x, min_y, min_z, max_x, max_y, max_z);
      bool isInBox = (p.x >= min_x && p.x <= max_x) && (p.y >= min_y && p.y <= max_y) && (p.z >= min_z && p.z <= max_z);
      if (!isInBox || !map_octree_->isVoxelOccupiedAtPoint(p)) //判断点所处的空间是否存在与八叉树体素中
      {
        map_octree_->addPointToCloud(p, map_data_);
        incremental_points->push_back(p);
      }
    }
    map_mutex_.unlock();
  } else {}
  incremental_points->header = points->header;
  incremental_points->header.frame_id = fixed_frame_id_;
  map_updated_ = true;
  return true;
}

函数流:BlamSlam::ProcessPointCloudMessage->PointCloudMapper::ApproxNearestNeighbors
在地图里面寻找当前点云(map坐标系)的邻近点云

bool PointCloudMapper::ApproxNearestNeighbors(const PointCloud& points,
                                              PointCloud* neighbors) {
  if (!initialized_) {}
  if (neighbors == NULL) {}
  neighbors->points.clear();
  for (size_t ii = 0; ii < points.points.size(); ++ii) {
    float unused = 0.f;
    int result_index = -1; 
    map_octree_->approxNearestSearch(points.points[ii], result_index, unused);
    if (result_index >= 0)
      neighbors->push_back(map_data_->points[result_index]);
  }   
  return neighbors->points.size() > 0;
}

帧图匹配类PointCloudLocalization

函数流:BlamSlam::ProcessPointCloudMessage->PointCloudLocalization::MotionUpdate
先保存前端位姿增量

bool PointCloudLocalization::MotionUpdate(
    const gu::Transform3& incremental_odom) {
  // Store the incremental transform from odometry.
  incremental_estimate_ = incremental_odom;
  return true;
}

函数流:BlamSlam::ProcessPointCloudMessage->PointCloudLocalization::TransformPointsToFixedFrame
将当前帧滤波后点云转换到map坐标系

bool PointCloudLocalization::TransformPointsToFixedFrame(
    const PointCloud& points, PointCloud* points_transformed) const {
  if (points_transformed == NULL) {
    return false;
  }

  // Compose the current incremental estimate (from odometry) with the
  // integrated estimate, and transform the incoming point cloud.
  const gu::Transform3 estimate =
      gu::PoseUpdate(integrated_estimate_, incremental_estimate_);
  const Eigen::Matrix R = estimate.rotation.Eigen();
  const Eigen::Matrix T = estimate.translation.Eigen();

  Eigen::Matrix4d tf;
  tf.block(0, 0, 3, 3) = R;
  tf.block(0, 3, 3, 1) = T;

  pcl::transformPointCloud(points, *points_transformed, tf);

  return true;
}

在地图里面寻找当前点云(map坐标系)的邻近点云
mapper_.ApproxNearestNeighbors(*msg_transformed, msg_neighbors.get());//当前点云(map坐标系)寻找邻近点云

函数流:BlamSlam::ProcessPointCloudMessage->PointCloudLocalization::TransformPointsToSensorFrame
将邻近点点云msg_neighbors反转换到雷达坐标系

bool PointCloudLocalization::TransformPointsToSensorFrame(
    const PointCloud& points, PointCloud* points_transformed) const {
  if (points_transformed == NULL) {
    return false;
  }

  // Compose the current incremental estimate (from odometry) with the
  // integrated estimate, then invert to go from world to sensor frame.
  const gu::Transform3 estimate = gu::PoseInverse(
      gu::PoseUpdate(integrated_estimate_, incremental_estimate_));//integrated_estimate from config parameters
  const Eigen::Matrix R = estimate.rotation.Eigen();
  const Eigen::Matrix T = estimate.translation.Eigen();

  Eigen::Matrix4d tf;
  tf.block(0, 0, 3, 3) = R;
  tf.block(0, 3, 3, 1) = T;

  pcl::transformPointCloud(points, *points_transformed, tf);

  return true;
}

函数流:BlamSlam::ProcessPointCloudMessage->PointCloudLocalization::MeasurementUpdate
帧图匹配就是在当前帧点云与地图中的临近点之间进行的，更新当前帧到地图位姿integrated_estimate_。其中query为当前帧，reference为地图，如果位姿增量在合理范围，就认为匹配有效。

bool PointCloudLocalization::MeasurementUpdate(const PointCloud::Ptr& query,
                                               const PointCloud::Ptr& reference,
                                               PointCloud* aligned_query) {
  if (aligned_query == NULL) {
    return false;
  }
  // ICP-based alignment. Generalized ICP does (roughly) plane-to-plane
  // matching, and is much more robust than standard ICP.
  GeneralizedIterativeClosestPoint icp;
  icp.setTransformationEpsilon(params_.tf_epsilon);
  icp.setMaxCorrespondenceDistance(params_.corr_dist);
  icp.setMaximumIterations(params_.iterations);
  icp.setRANSACIterations(0);
  icp.setMaximumOptimizerIterations(20); // default 20
  icp.setInputSource(query);
  icp.setInputTarget(reference);
  PointCloud unused;
  icp.align(unused);
  // Retrieve transformation and estimate and update.
  const Eigen::Matrix4f T = icp.getFinalTransformation();
  pcl::transformPointCloud(*query, *aligned_query, T);
  gu::Transform3 pose_update;//本帧到地图位姿增量
  pose_update.translation = gu::Vec3(T(0, 3), T(1, 3), T(2, 3));
  pose_update.rotation = gu::Rot3(T(0, 0), T(0, 1), T(0, 2),
                                  T(1, 0), T(1, 1), T(1, 2),
                                  T(2, 0), T(2, 1), T(2, 2));
  // Only update if the transform is small enough.
  if (!transform_thresholding_ ||
      (pose_update.translation.Norm() <= max_translation_ &&
       pose_update.rotation.ToEulerZYX().Norm() <= max_rotation_)) {
    incremental_estimate_ = gu::PoseUpdate(incremental_estimate_, pose_update);
  } else { }
  integrated_estimate_ =
      gu::PoseUpdate(integrated_estimate_, incremental_estimate_);
  return true;
}

下一步把增量清零 localization_.MotionUpdate(gu::Transform3::Identity())，再当前帧转到地图
函数流:BlamSlam::ProcessPointCloudMessage->PointCloudLocalization::TransformPointsToFixedFrame

三、实验结果

mkdir build
cd build
cmake …
make
./mapping

四、注意事项

未完待续