cartographer代码学习笔记-Node::HandleLaserScanMessage

Running小学生

已于 2023-12-06 14:08:08 修改

阅读量176

点赞数 3

分类专栏： cartographer 文章标签：学习笔记

于 2023-12-01 10:32:53 首次发布

本文链接：https://blog.csdn.net/weixin_28683491/article/details/134719981

版权

cartographer 专栏收录该内容

19 篇文章 1 订阅

订阅专栏

顺序流程图

**加粗样式** 启动数据处理：Node::LaunchSubscribers中订阅话题

SubscribeWithHandler<sensor_msgs::LaserScan>(
             &Node::HandleLaserScanMessage, trajectory_id, topic, &node_handle_,
             this)

处理数据

1、首先进行锁操作，然后判断

void Node::HandleLaserScanMessage(const int trajectory_id,
                                  const std::string& sensor_id,
                                  const sensor_msgs::LaserScan::ConstPtr& msg) {
  absl::MutexLock lock(&mutex_);
  if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) {
    return;
  }
  map_builder_bridge_.sensor_bridge(trajectory_id)
      ->HandleLaserScanMessage(sensor_id, msg);
}

2、然后判断采样率

std::unordered_map<int, TrajectorySensorSamplers> sensor_samplers_;

TrajectorySensorSamplers类对象构造传入如下参数
参考: https://blog.csdn.net/weixin_43013761/article/details/127638039

  rangefinder_sampling_ratio = 1.,          //范围传感器(如单,多线雷达)采样频率
  odometry_sampling_ratio = 1.,            //里程计采样频率
  fixed_frame_pose_sampling_ratio = 1.,   //GPS采样频率
  imu_sampling_ratio = 1.,               //IMU采样频率
  landmarks_sampling_ratio = 1.,        //landmarks采样频率

参数传入1，代表全采样(如果为0.5则表示每两个样本数据采集一次，设置为0则表示丢弃所有数据)。另外，要注意的是上诉采样频率都被保存于 TrajectorySensorSamplers 之中，如 TrajectorySensorSamplers 的构造函数：

  // 控制各个传感器数据的采样频率
  struct TrajectorySensorSamplers {
    TrajectorySensorSamplers(const double rangefinder_sampling_ratio,
                             const double odometry_sampling_ratio,
                             const double fixed_frame_pose_sampling_ratio,
                             const double imu_sampling_ratio,
                             const double landmark_sampling_ratio)
        : rangefinder_sampler(rangefinder_sampling_ratio),
          odometry_sampler(odometry_sampling_ratio),
          fixed_frame_pose_sampler(fixed_frame_pose_sampling_ratio),
          imu_sampler(imu_sampling_ratio),
          landmark_sampler(landmark_sampling_ratio) {}

    ::cartographer::common::FixedRatioSampler rangefinder_sampler;
    ::cartographer::common::FixedRatioSampler odometry_sampler;
    ::cartographer::common::FixedRatioSampler fixed_frame_pose_sampler;
    ::cartographer::common::FixedRatioSampler imu_sampler;
    ::cartographer::common::FixedRatioSampler landmark_sampler;
  };

在 FixedRatioSampler 的构造函数中，可以看到，如果采样频率设置为0，其会打印警告信息。FixedRatioSampler 中有实现 Pulse() 函数，调用该函数，可以判断当前采集的样本是否使用。其实现的函数比较巧妙，如下：

// 在比例小于ratio_时返回true, ratio_设置为1时都返回true, 也就是说使用所有的数据
bool FixedRatioSampler::Pulse() {
  ++num_pulses_; 
  if (static_cast<double>(num_samples_) / num_pulses_ < ratio_) {
    ++num_samples_;
    return true;
  }
  // 返回false时代表数据可以不用,可以跳过计算
  return false;
}

假设 ratio_ 为0.7，根据下面表格来分析下他的运行过程：
在这里插入图片描述

可以看到10个数据中，有个为true，概率分布刚刚好为0.7

3、调用HandleLaserScanMessage

  map_builder_bridge_.sensor_bridge(trajectory_id)
      ->HandleLaserScanMessage(sensor_id, msg);

调用该接口：这个声明表示，在使用这个变量的时候，必须对 mutex_ 进行加锁，如果没有加锁就使用，则会报错: https://blog.csdn.net/weixin_43013761/article/details/127686479

 // c++11: GUARDED_BY 是在Clang Thread Safety Analysis（线程安全分析）中定义的属性
  // GUARDED_BY是数据成员的属性, 该属性声明数据成员受给定功能保护.
  // 对数据的读操作需要共享访问, 而写操作则需要互斥访问.
  // 官方介绍文档: https://clang.llvm.org/docs/ThreadSafetyAnalysis.html

MapBuilderBridge map_builder_bridge_ GUARDED_BY(mutex_);

SensorBridge* MapBuilderBridge::sensor_bridge(const int trajectory_id) {
  return sensor_bridges_.at(trajectory_id).get();
}

在 https://blog.csdn.net/weixin_28683491/article/details/134710850?spm=1001.2014.3001.5502中完成了对sensor_bridges_参数初始化。
调用SensorBridge::HandleLaserScanMessage

void SensorBridge::HandleLaserScanMessage(
    const std::string& sensor_id, const sensor_msgs::LaserScan::ConstPtr& msg) {
  carto::sensor::PointCloudWithIntensities point_cloud;
  carto::common::Time time;
  std::tie(point_cloud, time) = `ToPointCloudWithIntensities`(*msg);
  HandleLaserScan(sensor_id, time, msg->header.frame_id, point_cloud);
}

分为两步，
1、对数据进行预处理，数据格式转换；
2、数据处理

3.1数据格式转换

std::tuple<::cartographer::sensor::PointCloudWithIntensities,
           ::cartographer::common::Time>
ToPointCloudWithIntensities(const sensor_msgs::LaserScan& msg) {
  return LaserScanToPointCloudWithIntensities(msg);
}

调用LaserScanToPointCloudWithIntensities
摘自: https://blog.csdn.net/weixin_43013761/article/details/127841694

// For sensor_msgs::LaserScan and sensor_msgs::MultiEchoLaserScan.
// 将LaserScan与MultiEchoLaserScan转成carto格式的点云数据
template <typename LaserMessageType>
std::tuple<PointCloudWithIntensities, ::cartographer::common::Time>
LaserScanToPointCloudWithIntensities(const LaserMessageType& msg) {
  CHECK_GE(msg.range_min, 0.f); //最小距离大于等于0.f
  CHECK_GE(msg.range_max, msg.range_min); //最大距离大于等于最小距离

  //针对与点云的两种情况
  if (msg.angle_increment > 0.f) { //如果后一个数据点与前一个数据点之间的角度应该大于0.f
    CHECK_GT(msg.angle_max, msg.angle_min);//那么最大角度大于最小角度
  } else {//如果后一个数据点小于前一个数据点之间的角度应该大于0.f
    CHECK_GT(msg.angle_min, msg.angle_max);//那么最小角度应该大于最大于角度
  }

  //用于存储一帧带强度的点云数据
  PointCloudWithIntensities point_cloud;
  //注意angle值随着循环在变化，使用弧度制
  float angle = msg.angle_min; 
  //对所有的数据点进行遍历，每个数据点可能存在多个echoe
  for (size_t i = 0; i < msg.ranges.size(); ++i) {
    // c++11: 使用auto可以适应不同的数据类型
    const auto& echoes = msg.ranges[i];
    //HasEcho其有两个重载函数，会根据输入类型分别进行不同处理
    //如果为单线雷达，直接返回 true；如果为多回声雷达，则需要echoes.echoes不为空才执行
    if (HasEcho(echoes)) {
      //仍然有两个重载分别对单线雷达与多回声雷达进行处理
      const float first_echo = GetFirstEcho(echoes); 
      // 满足范围才进行使用,即第一个echo其数值在合理范围内才进行处理，为inf不会进行处理
      if (msg.range_min <= first_echo && first_echo <= msg.range_max) {
        //first_echo仅仅是一个三维点到传感器的距离，需要根据把其变换成雷达坐标系下的点云数据
        //假设雷达与first_echo连线新坐标的x轴,则在这个新坐标系中first_echo的位置是(x,0,0)
        //绕Z轴旋转angle弧度,即回到了雷达坐标系。
        const Eigen::AngleAxisf rotation(angle, Eigen::Vector3f::UnitZ());
        const cartographer::sensor::TimedRangefinderPoint point{
            rotation * (first_echo * Eigen::Vector3f::UnitX()), // position
            i * msg.time_increment};//根据time_increment参数记录时间戳time          
        // 保存点云坐标与时间信息
        point_cloud.points.push_back(point);
        // 如果存在强度信息
        if (msg.intensities.size() > 0) {
          CHECK_EQ(msg.intensities.size(), msg.ranges.size());
          // 使用auto可以适应不同的数据类型
          const auto& echo_intensities = msg.intensities[i];
          CHECK(HasEcho(echo_intensities));
          //存储真实强度信息
          point_cloud.intensities.push_back(GetFirstEcho(echo_intensities));
        } else {//不过不存在强度信息,则把强度设置为0.f
          point_cloud.intensities.push_back(0.f);
        }
      }
    }
    //每处理完一个数据，增加一个角度分辨率单位
    angle += msg.angle_increment;
  }
  ::cartographer::common::Time timestamp = FromRos(msg.header.stamp);
  if (!point_cloud.points.empty()) {
    const double duration = point_cloud.points.back().time;
    timestamp += cartographer::common::FromSeconds(duration);
    for (auto& point : point_cloud.points) {
      point.time -= duration;
    }
  }
  return std::make_tuple(point_cloud, timestamp);
}

上述主要目的是将激光数据按照std::tuple<PointCloudWithIntensities, ::cartographer::common::Time>类型组合
::cartographer::common::Time为最后一个激光点对应的系统时间

 ::cartographer::common::Time timestamp = FromRos(msg.header.stamp);
 timestamp += cartographer::common::FromSeconds(duration);

PointCloudWithIntensities对应的是每个激光点的位置(x y z)以及其相对于系统时间的时间差。PointCloudWithIntensities的结构：

using TimedPointCloud = std::vector<TimedRangefinderPoint>;

struct PointCloudWithIntensities {
  TimedPointCloud points;
  std::vector<float> intensities;
};

struct TimedRangefinderPoint {
  Eigen::Vector3f position;
  float time;
};

每一个激光点都减去最后一个激光点的时间（激光点time是-duration，-duration+1，-duration+2，…，-2，-1，0）0是最后一个激光点对应的time。

 const double duration = point_cloud.points.back().time;
 for (auto& point : point_cloud.points) {
  point.time -= duration;//保留的是负值，通过元组中的::cartographer::common::Time恢复每个激光点对应的时间
}

坐标变换

const Eigen::AngleAxisf rotation(angle, Eigen::Vector3f::UnitZ());
const cartographer::sensor::TimedRangefinderPoint point{
  rotation * (first_echo * Eigen::Vector3f::UnitX()), // position
  i * msg.time_increment};//根据time_increment参数记录时间戳time

首先需要了解到的是，再ROS中，使用的通常都是右手坐标系，如下所示

在这里插入图片描述
右手坐标系：也就是一般来说，雷达正前方为X+方向，左边为Y+方向，上面为Z+反向，逆时针旋转为正旋转，且通常以弧度为单位，范围在[-π,π]之间。

这里举一个例子，假设 first_echo=19.6152687，angle= -3.0089736，那么该点在雷达坐标系的占位置为 P = (-19.4430275，-2.59373975, 0)。从俯视视角来看，那么如下图所示：

在这里插入图片描述很明显的知道， P 其应该在的位置为第四象限(逆时针方向计算)，可以简单通过 x = − |P| cos ⁡ ( π− θ ) ， y = − |P| sin ⁡ ( π − θ )求得坐标，但是源码的实现是通过对 P′ 进行旋转，从而获得 P 的坐标，

3.2 数据处理

主要作用是将激光数据按照num_subdivisions_per_laser_scan_参数分割，同时更新每段点数据的时间戳。然后调用 HandleRangefinder() 函数。

 HandleLaserScan(sensor_id,             //传感器话题
				  time,                 //最后一个激光点对应的系统时间
				  msg->header.frame_id, //传感器坐标
				  point_cloud);        //转化后的激光点云数据

代码解析参考https://blog.csdn.net/weixin_43013761/article/details/127841694

// 根据参数配置,将一帧雷达数据分成几段, 再传入trajectory_builder_
void SensorBridge::HandleLaserScan(
    const std::string& sensor_id, const carto::common::Time time,
    const std::string& frame_id,
    const carto::sensor::PointCloudWithIntensities& points) {
  if (points.points.empty()) {
    return;
  }
  // CHECK_LE: 小于等于
  CHECK_LE(points.points.back().time, 0.f);
  // TODO(gaschler): Use per-point time instead of subdivisions.

  // 意为一帧雷达数据被分成几次处理, 一般将这个参数设置为1
  for (int i = 0; i != num_subdivisions_per_laser_scan_; ++i) {
    const size_t start_index =
        points.points.size() * i / num_subdivisions_per_laser_scan_;
    const size_t end_index =
        points.points.size() * (i + 1) / num_subdivisions_per_laser_scan_;
    
    // 生成分段的点云
    carto::sensor::TimedPointCloud subdivision(
        points.points.begin() + start_index, points.points.begin() + end_index);
    if (start_index == end_index) {
      continue;
    }
    const double time_to_subdivision_end = subdivision.back().time;
    // `subdivision_time` is the end of the measurement so sensor::Collator will
    // send all other sensor data first.
    const carto::common::Time subdivision_time =
        time + carto::common::FromSeconds(time_to_subdivision_end);
    
    auto it = sensor_to_previous_subdivision_time_.find(sensor_id);
    if (it != sensor_to_previous_subdivision_time_.end() &&
        // 上一段点云的时间不应该大于等于这一段点云的时间
        it->second >= subdivision_time) {
      LOG(WARNING) << "Ignored subdivision of a LaserScan message from sensor "
                   << sensor_id << " because previous subdivision time "
                   << it->second << " is not before current subdivision time "
                   << subdivision_time;
      continue;
    }
    // 更新对应sensor_id的时间戳
    sensor_to_previous_subdivision_time_[sensor_id] = subdivision_time;
    
    // 检查点云的时间
    for (auto& point : subdivision) {
      point.time -= time_to_subdivision_end;
    }
    CHECK_EQ(subdivision.back().time, 0.f);
    // 将分段后的点云 subdivision 传入 trajectory_builder_
    HandleRangefinder(sensor_id, subdivision_time, frame_id, subdivision);
  } // for 
}

HandleRangefinder() 函数
把基于雷达的传感器的点云数据坐标系变换到 tracking_frame=“imu” 坐标系下，代码如下

/**
 * @brief 
 * 
 * @param[in] sensor_id 数据的话题
 * @param[in] time 点云的时间戳(最后一个点的时间)
 * @param[in] frame_id 点云的frame
 * @param[in] ranges 雷达坐标系下的TimedPointCloud格式的点云
 */
void SensorBridge::HandleRangefinder(
    const std::string& sensor_id, const carto::common::Time time,
    const std::string& frame_id, const carto::sensor::TimedPointCloud& ranges) {
  if (!ranges.empty()) {
    CHECK_LE(ranges.back().time, 0.f);
  }
  //
  const auto sensor_to_tracking =
      tf_bridge_.LookupToTracking(time, CheckNoLeadingSlash(frame_id));//计算最后一个激光点时间对应的激光传感器坐标系转到base或imu坐标下对应的坐标位置。在后续数据同步时使用。

  // 以 tracking 到 sensor_frame 的坐标变换为TimedPointCloudData 的 origin
  // 将点云的坐标转成 tracking 坐标系下的坐标, 再传入trajectory_builder_
   /*  struct TimedPointCloudData {
          common::Time time;      该段点云最后一个点的时间戳
          Eigen::Vector3f origin; 该段点云原点，最后一个点对应时刻下,雷达坐标系laser,在base下的坐标
          TimedPointCloud ranges; 该段点云转换到base下的点云
          // 'intensities' has to be same size as 'ranges', or empty.
          std::vector<float> intensities;  //点云强度
    };
    */
  if (sensor_to_tracking != nullptr) {
    trajectory_builder_->AddSensorData(
        sensor_id, carto::sensor::TimedPointCloudData{
                       time, 
                       sensor_to_tracking->translation().cast<float>(),//最后一个点对应时刻下,雷达坐标系laser,在base下的坐标
                       // 将点云从雷达坐标系下转到tracking_frame坐标系系下
                       carto::sensor::TransformTimedPointCloud(
                           ranges, sensor_to_tracking->cast<float>())} ); 
  }
}