开源激光SLAM项目BLAM－－－－－2

接上一章节提到的**ProcessPointCloudMessage(m->msg)**函数，它传入一个const PointCloud::ConstPtr& 类型，即点云常指针的引用，程序源代码如下

void BlamSlam::ProcessPointCloudMessage(const PointCloud::ConstPtr& msg) {

   
  static int T=0;
  ROS_INFO("The times is:%d",T++);  
  // 进行一些基本的过滤  提高后面运算速度  降低数据存储量
  PointCloud::Ptr msg_filtered(new PointCloud);
  filter_.Filter(msg, msg_filtered);


  // 激光里程计 得到粗略的 变换矩阵 ， 第一次执行时候进入if语句初始化地图
  if (!odometry_.UpdateEstimate(*msg_filtered)) {
    // First update ever.
    PointCloud::Ptr unused(new PointCloud);
    mapper_.InsertPoints(msg_filtered, unused.get());
    loop_closure_.AddKeyScanPair(0, msg);
    return;
  }

  PointCloud::Ptr msg_transformed(new PointCloud);
  PointCloud::Ptr msg_neighbors(new PointCloud);
  PointCloud::Ptr msg_base(new PointCloud);
  PointCloud::Ptr msg_fixed(new PointCloud);

  // 将当前帧数据通过前面的变换矩阵 转换到 地图坐标系下
  localization_.MotionUpdate(odometry_.GetIncrementalEstimate());
  localization_.TransformPointsToFixedFrame(*msg_filtered,
                                            msg_transformed.get());

  // 在地图坐标系下得到当前帧数据对应的最近邻点
  mapper_.ApproxNearestNeighbors(*msg_transformed, msg_neighbors.get());

  // 最近邻点转换回传感器的坐标系 与当前帧再进行一次匹配 得到精确的位姿变换矩阵
  localization_.TransformPointsToSensorFrame(*msg_neighbors, msg_neighbors.get());
  localization_.MeasurementUpdate(msg_filtered, msg_neighbors, msg_base.get());

  // 回环检测
  bool new_keyframe;
  if (HandleLoopClosures(msg, &new_keyframe)) {
    T=0;
    
    // 找到了一个回环，对地图进行调整
    PointCloud::Ptr regenerated_map(new PointCloud);
    loop_closure_.GetMaximumLikelihoodPoints(regenerated_map.get());

    mapper_.Reset();
    PointCloud::Ptr unused(new PointCloud);
    mapper_.InsertPoints(regenerated_map, unused.get());

    // 对储存的机器人位姿也重新进行调整
    localization_.SetIntegratedEstimate(loop_closure_.GetLastPose());
  } else {
    if (new_keyframe) { // 不是回环检测，但是是一个**关键帧**添加点云数据到地图中
      localization_.MotionUpdate(gu::Transform3::Identity());
      //当前帧数据使用上述的精确位姿变换矩阵 转换到地图坐标系下 
      localization_.TransformPointsToFixedFrame(*msg, msg_fixed.get()); 
      PointCloud::Ptr unused(new PointCloud);
      loop_closure_.GetMapPoints(msg_fixed.get());
      ROS_INFO("The size of get::%d",msg_fixed->points.size());
      //加入到地图中
      mapper_.InsertPoints(msg_fixed, unused.get());
    }
  }

  // 发布位姿节点，里程计数据等
  loop_closure_.PublishPoseGraph();

  // 发布当前帧点云数据
  if (base_frame_pcld_pub_.getNumSubscribers() != 0) {
    PointCloud base_frame_pcld = *msg;
    base_frame_pcld.header.frame_id = base_frame_id_;
    base_frame_pcld_pub_.publish(base_frame_pcld);
  }
}

程序有一点长，不过相对于其他开源激光SLAM项目还是挺好理解的。下面就一段段的分析

  static int T=0;
  ROS_INFO("The times is:%d",T++);  
  // 进行一些基本的过滤  提高后面运算速度  降低数据存储量
  PointCloud::Ptr msg_filtered(new PointCloud);
  filter_.Filter(msg, msg_filtered);


  // 激光里程计 得到粗略的 变换矩阵 ， 第一次执行时候进入if语句初始化地图
  if (!odometry_.UpdateEstimate(*msg_filtered)) {
    // First update ever.
    PointCloud::Ptr unused(new PointCloud);
    mapper_.InsertPoints(msg_filtered, unused.get());
    loop_closure_.AddKeyScanPair(0, msg);
    return;
  }

filter_.Filter(msg, msg_filtered); 这里就是调用PCL库进行一些基本的数据过滤，其配置在config.yaml文件进行设置，比较见到就不进去看了，不了解的可以去PCL官网教程里去看一下(http://pointclouds.org/documentation/tutorials/)

*odometry_.UpdateEstimate(msg_filtered) odometry_是 odometry类实例化的一个对象，odometry类定义在point_cloud_odometry这个package下。进入这个函数

bool PointCloudOdometry::UpdateEstimate(const PointCloud& points) {
  // 和pcl到ros的时间戳转换有关 转换成合理的形式
  stamp_.fromNSec(points.header.stamp*1e3);

  // 如果这是第一个消息  储存下来等待下一个消息 
  if (!initialized_) {
    copyPointCloud(points, *query_);
    initialized_ = true;
    return false;
  }

  // Move current query points (acquired last iteration) to reference points.
  copyPointCloud(*query_, *reference_);
  // Set the incoming point cloud as the query point cloud.
  copyPointCloud(points, *query_);

  // 有了两帧数据 执行icp
  return UpdateICP();
}

可以看到这里是保存当前帧和上一帧的数据,通过两帧数据做匹配。下面就进入匹配的函数

bool PointCloudOdometry::UpdateICP() {

  // 计算两帧之间的转换---incremental transform
  GeneralizedIterativeClosestPoint<PointXYZ, PointXYZ> icp;  //这里使用的GICP
  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();  //得到粗略的  位姿变换

  //将Eigen库的Matrix4f 转换到 blam自定义的 位姿变换矩阵 transform3
  incremental_estimate_.translation = gu::Vec3(T(0, 3), T(1, 3), T(2, 3));  //4*4的位姿变换矩阵 设置平移量
  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));

  //如果变换矩阵比较小--说明转换是正确的 如果比较大则舍弃
  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 {
    ROS_WARN(
        "%s: Discarding incremental transformation with norm (t: %lf, r: %lf)",
        name_.c_str(), incremental_estimate_.translation.Norm(),
        incremental_estimate_.rotation.ToEulerZYX().Norm());
  }

  // Convert pose estimates to ROS format and publish.
  PublishPose(incremental_estimate_, incremental_estimate_pub_);   //发布两帧之间的位姿变换
  PublishPose(integrated_estimate_, integrated_estimate_pub_);     //发布累计的位姿变换

  // Publish point clouds for visualization.
  PublishPoints(query_, query_pub_);
  PublishPoints(reference_, reference_pub_);

  // Convert transform between fixed frame and odometry frame.
  geometry_msgs::TransformStamped tf;
  tf.transform = gr::ToRosTransform(integrated_estimate_);         //发布tf变换
  tf.header.stamp = stamp_;
  tf.header.frame_id = fixed_frame_id_;
  tf.child_frame_id = odometry_frame_id_;
  tfbr_.sendTransform(tf);

  return true;
}

这段代码的主要功能就是计算两帧之间的位姿变换，注意这里只是一个粗略的计算，后面还有精确计算。另外就是得到累计的位姿估计。如果对上述代码注释中的GICP , ICP , TF变换 , 位姿矩阵等名词不熟悉的话 需要自行百度去了解一下 再回头来看这段代码。

---

                                                                            转载注明出处