Autoware 定位之视觉数据关联与预处理（十一）

0. 简介

这一讲按照《Autoware 技术代码解读（三）》梳理的顺序，我们来说一说Autoware中的视觉定位，这部分主要是通过将从图像中提取的道路表面标记与矢量地图进行匹配来估计位置，而无需使用点云地图和激光雷达。 YabLoc让用户能够在没有激光雷达和点云地图的环境中定位未装备有激光雷达的车辆。但是由于篇章过多，所以我们将视觉定位与位姿初始化放到后面来讲。

1. 如何使用纯视觉定位

在启动Autoware时，如果你将pose_source参数设置为yabloc，YabLoc将会被启动而不是NDT。默认情况下，pose_source为ndt。

运行YabLoc的示例命令如下：

ros2 launch autoware_launch logging_simulator.launch.xml \
  map_path:=$HOME/autoware_map/sample-map-rosbag\
  vehicle_model:=sample_vehicle \
  sensor_model:=sample_sensor_kit \
  pose_source:=yabloc

1.1 大致思路

对于YabLoc而言。它通过在基于图的分割得到的道路区域中提取线段来提取道路地面标线。图表顶部中央的红线代表被识别为道路地面标线的线段。YabLoc为每个粒子转换这些线段，并通过将它们与由Lanelet2生成的成本地图进行比较来确定粒子的权重。

1.2 基础界面

index	topic name	description
1	/localization/yabloc/pf/predicted_particle_marker	粒子滤波器的粒子分布。红色粒子是可能的候选者
2	/localization/yabloc/pf/scored_cloud	3D投影线段。颜色表示它们与地图匹配程度的好坏。
3	/localization/yabloc/image_processing/lanelet2_overlay_image	根据估计的姿态，将Lanelet2的叠加层（黄色线条）叠加到图像上。如果它们与实际的道路标线匹配良好，这意味着定位执行良好。

1.3 debug模式

index	topic name	description
1	/localization/yabloc/pf/cost_map_image	使用Lanelet2生成的成本地图
2	/localization/yabloc/pf/match_image	投影线段
3	/localization/yabloc/image_processing/image_with_colored_line_segment	分类线段。绿色线段用于粒子校正
4	/localization/yabloc/image_processing/lanelet2_overlay_image	lanelet2的叠加
5	/localization/yabloc/image_processing/segmented_image	基于图像的分割结果

1.4 限制

1. 同时运行 YabLoc 和 NDT 不受支持。这是因为同时运行两者可能会造成计算成本过高。此外，在大多数情况下，NDT 优于 YabLoc，因此同时运行它们的好处较少。

2. 它不会估计横滚和俯仰，因此一些感知节点可能无法正常工作。它目前不支持多摄像头，但将来会支持。

3. 在交叉口等道路标线较少的地方，估计结果会严重依赖于 GNSS、IMU 和车辆轮子的里程表。如果 Lanelet2 中没有包含道路边界或道路标线，估计可能会失败。

2. yabloc_common节点代码阅读

这个软件包包含了一些与地图相关的可执行节点。此外，它还提供了一些 yabloc 通用库。

2.1 ground_server节点

这个节点从lanelet2估计地面的高度和倾斜度

/// @brief 地面服务器的构造函数
GroundServer::GroundServer()
: Node("ground_server"),
  force_zero_tilt_(declare_parameter<bool>("force_zero_tilt")),
  R(declare_parameter<int>("R")),
  K(declare_parameter<int>("K"))
{
  using std::placeholders::_1;
  using std::placeholders::_2;
  const rclcpp::QoS map_qos = rclcpp::QoS(10).transient_local().reliable();

  auto on_pose = std::bind(&GroundServer::on_pose_stamped, this, _1);
  auto on_map = std::bind(&GroundServer::on_map, this, _1);

  sub_map_ = create_subscription<HADMapBin>("~/input/vector_map", map_qos, on_map);
  sub_pose_stamped_ = create_subscription<PoseStamped>("~/input/pose", 10, on_pose);

  pub_ground_height_ = create_publisher<Float32>("~/output/height", 10);
  pub_ground_plane_ = create_publisher<Float32Array>("~/output/ground", 10);
  pub_marker_ = create_publisher<Marker>("~/debug/ground_markers", 10);
  pub_string_ = create_publisher<String>("~/debug/ground_status", 10);
  pub_near_cloud_ = create_publisher<PointCloud2>("~/debug/near_cloud", 10);

  height_filter_.reset(0);
}
/// @brief 当收到初始位姿的信息时调用的函数, 如果kdtree（kd树，用于最近邻搜索）为空，则输出错误信息并返回。接着通过传感器获得的位置信息估计地面的高度。
/// @param msg 对应的初始位姿信息
void GroundServer::on_initial_pose(const PoseCovStamped & msg)
{
  if (kdtree_ == nullptr) {
    RCLCPP_FATAL_STREAM(get_logger(), "ground height is not initialized because map is empty");
    return;
  }

  const Point point = msg.pose.pose.position;
  height_filter_.reset(estimate_height_simply(point));
}
/// @brief 当收到位姿信息时调用的函数。根据传感器信息估计地面的平面，然后发布地面高度、地面平面参数、字符串信息和最近点云等信息
/// @param msg 对应的位姿信息
void GroundServer::on_pose_stamped(const PoseStamped & msg)
{
  if (kdtree_ == nullptr) return;
  GroundPlane ground_plane = estimate_ground(msg.pose.position);
  // Publish value msg
  Float32 data;
  data.data = ground_plane.height();
  pub_ground_height_->publish(data);
  pub_ground_plane_->publish(ground_plane.msg());

  // Publish string msg
  {
    std::stringstream ss;
    ss << "--- Ground Estimator Status ----" << std::endl;
    ss << std::fixed << std::setprecision(2);
    ss << "height: " << ground_plane.height() << std::endl;
    float cos = ground_plane.normal.dot(Eigen::Vector3f::UnitZ());
    ss << "tilt: " << std::acos(cos) * 180 / 3.14 << " deg" << std::endl;

    String string_msg;
    string_msg.data = ss.str();
    pub_string_->publish(string_msg);
  }

  // Publish nearest point cloud for debug
  {
    pcl::PointCloud<pcl::PointXYZ> near_cloud;
    for (int index : last_indices_) {
      near_cloud.push_back(cloud_->at(index));
    }
    if (!near_cloud.empty()) common::publish_cloud(*pub_near_cloud_, near_cloud, msg.header.stamp);
  }
}

/// @brief 从地图信息中提取地面信息，并对相关地面进行采样、滤波等处理。最后构建了kd树用于后续的最近邻搜索。
/// @param msg 对应的地图信息
void GroundServer::on_map(const HADMapBin & msg)
{
  lanelet::LaneletMapPtr lanelet_map(new lanelet::LaneletMap);
  lanelet::utils::conversion::fromBinMsg(msg, lanelet_map);

  // These should be loaded from rosparam
  const std::set<std::string> ground_labels = {
    "zebra_marking",      "virtual",   "line_thin", "line_thick",
    "pedestrian_marking", "stop_line", "curbstone"};

  pcl::PointCloud<pcl::PointXYZ>::Ptr upsampled_cloud =
    pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();

  for (lanelet::LineString3d & line : lanelet_map->lineStringLayer) {
    if (!line.hasAttribute(lanelet::AttributeName::Type)) continue;

    lanelet::Attribute attr = line.attribute(lanelet::AttributeName::Type);
    if (ground_labels.count(attr.value()) == 0) continue;

    lanelet::ConstPoint3d const * from = nullptr;
    for (const lanelet::ConstPoint3d & p : line) {
      if (from != nullptr) upsample_line_string(*from, p, upsampled_cloud);
      from = &p;
    }
  }

  // NOTE: Under construction
  // This function is used to generate a uniform point cloud from within the polygons surrounding
  // the area when there is no lane information, such as at an intersection.
  // if(lanelet_map->polygonLayer.size() > 0)
  //   *upsampled_cloud += sample_from_polygons(lanelet_map->polygonLayer);

  cloud_ = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
  // Voxel
  pcl::VoxelGrid<pcl::PointXYZ> filter;
  filter.setInputCloud(upsampled_cloud);
  filter.setLeafSize(1.0f, 1.0f, 1.0f);
  filter.filter(*cloud_);

  kdtree_ = pcl::make_shared<pcl::KdTreeFLANN<pcl::PointXYZ>>();
  kdtree_->setInputCloud(cloud_);
}

/// @brief 根据输入的点估计地面高度。通过简单的距离过滤来估计相应点的地面高度
/// @param point 对应的点
/// @return 
float GroundServer::estimate_height_simply(const geometry_msgs::msg::Point & point) const
{
  // NOTE: Sometimes it might give not-accurate height
  constexpr float sq_radius = 3.0 * 3.0;
  const float x = point.x;
  const float y = point.y;

  float height = std::numeric_limits<float>::infinity();
  for (const auto & p : cloud_->points) {
    const float dx = x - p.x;
    const float dy = y - p.y;
    const float sd = (dx * dx) + (dy * dy);
    if (sd < sq_radius) {
      height = std::min(height, static_cast<float>(p.z));
    }
  }
  return std::isfinite(height) ? height : 0;
}

/// @brief 通过RANSAC算法估计局内点。这里用于估计地面的平面模型
/// @param indices_raw 获取的最近邻点的索引
/// @return 
std::vector<int> GroundServer::estimate_inliers_by_ransac(const std::vector<int> & indices_raw)
{
  pcl::PointIndicesPtr indices(new pcl::PointIndices);
  indices->indices = indices_raw;

  pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
  pcl::PointIndices::Ptr inliers(new pcl::PointIndices);

  pcl::SACSegmentation<pcl::PointXYZ> seg;
  seg.setOptimizeCoefficients(true);
  seg.setModelType(pcl::SACMODEL_PLANE);
  seg.setMethodType(pcl::SAC_RANSAC);
  seg.setDistanceThreshold(1.0);
  seg.setProbability(0.6);

  seg.setInputCloud(cloud_);
  seg.setIndices(indices);
  seg.segment(*inliers, *coefficients);
  return inliers->indices;
}

/// @brief 给定一个点，对该点周围的点进行最近邻搜索，并用RANSAC估计局内点来估计地面的法向量和高度
/// @param point 对应的位置
/// @return 
GroundServer::GroundPlane GroundServer::estimate_ground(const Point & point)
{
  // Because height_filter_ is always initialized, getValue does not return nullopt
  const float predicted_z = height_filter_.getValue().value();
  const pcl::PointXYZ xyz(point.x, point.y, predicted_z);

  std::vector<int> raw_indices;
  std::vector<float> distances;
  kdtree_->nearestKSearch(xyz, K, raw_indices, distances);

  std::vector<int> indices = estimate_inliers_by_ransac(raw_indices);

  if (indices.empty()) indices = raw_indices;
  last_indices_ = indices;

  // Estimate normal vector using covariance matrix around the target point
  Eigen::Matrix3f covariance;
  Eigen::Vector4f centroid;
  pcl::compute3DCentroid(*cloud_, indices, centroid);
  pcl::computeCovarianceMatrix(*cloud_, indices, centroid, covariance);

  // NOTE: I forgot why I don't use coefficients computed by SACSegmentation
  Eigen::Vector4f plane_parameter;
  float curvature;
  pcl::solvePlaneParameters(covariance, centroid, plane_parameter, curvature);
  Eigen::Vector3f normal = plane_parameter.topRows(3).normalized();

  {
    // Reverse if it is upside down
    if (normal.z() < 0) normal = -normal;

    // Remove NaN
    if (!normal.allFinite()) {
      normal = Eigen::Vector3f::UnitZ();
      RCLCPP_WARN_STREAM(get_logger(), "Reject NaN tilt");
    }
    // Remove too large tilt (0.707 = cos(45deg))
    if ((normal.dot(Eigen::Vector3f::UnitZ())) < 0.707) {
      normal = Eigen::Vector3f::UnitZ();
      RCLCPP_WARN_STREAM(get_logger(), "Reject too large tilt of ground");
    }
  }

  const Eigen::Vector3f filt_normal = normal_filter_.update(normal);

  GroundPlane plane;
  plane.xyz = Eigen::Vector3f(point.x, point.y, predicted_z);
  plane.normal = filt_normal;

  // Compute z value by intersection of estimated plane and orthogonal line
  {
    Eigen::Vector3f center = centroid.topRows(3);
    float inner = center.dot(plane.normal);
    float px_nx = point.x * plane.normal.x();
    float py_ny = point.y * plane.normal.y();
    plane.xyz.z() = (inner - px_nx - py_ny) / plane.normal.z();
  }

  height_filter_.filter(plane.xyz.z());

  if (force_zero_tilt_) plane.normal = Eigen::Vector3f::UnitZ();
  return plane;
}

2.2 ll2_decomposer节点

该节点从lanelet2中提取与路面标线和yabloc相关的元素

/// @brief ll2分解器的构造函数
Ll2Decomposer::Ll2Decomposer() : Node("ll2_to_image")
{
  using std::placeholders::_1;
  const rclcpp::QoS latch_qos = rclcpp::QoS(10).transient_local();
  const rclcpp::QoS map_qos = rclcpp::QoS(10).transient_local().reliable();

  // Publisher
  pub_road_marking_ = create_publisher<Cloud2>("~/output/ll2_road_marking", latch_qos);
  pub_sign_board_ = create_publisher<Cloud2>("~/output/ll2_sign_board", latch_qos);
  pub_bounding_box_ = create_publisher<Cloud2>("~/output/ll2_bounding_box", latch_qos);
  pub_marker_ = create_publisher<MarkerArray>("~/debug/sign_board_marker", latch_qos);

  // Subscriber
  auto cb_map = std::bind(&Ll2Decomposer::on_map, this, _1);
  sub_map_ = create_subscription<HADMapBin>("~/input/vector_map", map_qos, cb_map);

  auto load_lanelet2_labels =
    [this](const std::string & param_name, std::set<std::string> & labels) -> void {
    this->template declare_parameter<std::vector<std::string>>(param_name);
    auto label_array = get_parameter(param_name).as_string_array();
    for (auto l : label_array) labels.insert(l);
  };

  load_lanelet2_labels("road_marking_labels", road_marking_labels_);
  load_lanelet2_labels("sign_board_labels", sign_board_labels_);
  load_lanelet2_labels("bounding_box_labels", bounding_box_labels_);

  if (road_marking_labels_.empty()) {
    RCLCPP_FATAL_STREAM(
      get_logger(), "There are no road marking labels. No LL2 elements will publish");
  }
}

/// @brief 打印 Lanelet2 地图中的所有类型标签
/// @param lanelet_map lanelet的地图
/// @param logger logger信息
void print_attr(const lanelet::LaneletMapPtr & lanelet_map, const rclcpp::Logger & logger)
{
  std::set<std::string> types;
  for (const lanelet::ConstLineString3d & line : lanelet_map->lineStringLayer) {
    if (!line.hasAttribute(lanelet::AttributeName::Type)) continue;
    lanelet::Attribute attr = line.attribute(lanelet::AttributeName::Type);
    types.insert(attr.value());
  }
  for (const lanelet::ConstPolygon3d & polygon : lanelet_map->polygonLayer) {
    if (!polygon.hasAttribute(lanelet::AttributeName::Type)) {
      continue;
    }
    lanelet::Attribute attr = polygon.attribute(lanelet::AttributeName::Type);
    types.insert(attr.value());
  }

  for (const auto & type : types) {
    RCLCPP_INFO_STREAM(logger, "lanelet type: " << type);
  }
}

/// @brief 从 Lanelet2 地图中的多边形层加载边界框数据
/// @param polygons 多边形层
/// @return 
pcl::PointCloud<pcl::PointXYZL> Ll2Decomposer::load_bounding_boxes(
  const lanelet::PolygonLayer & polygons) const
{
  pcl::PointCloud<pcl::PointXYZL> cloud;
  int index = 0;

  for (const lanelet::ConstPolygon3d & polygon : polygons) {
    if (!polygon.hasAttribute(lanelet::AttributeName::Type)) continue;
    lanelet::Attribute attr = polygon.attribute(lanelet::AttributeName::Type);
    if (bounding_box_labels_.count(attr.value()) == 0) continue;

    for (const lanelet::ConstPoint3d & p : polygon) {//多边形的坐标转换为点云数据
      pcl::PointXYZL xyzl;
      xyzl.x = p.x();
      xyzl.y = p.y();
      xyzl.z = p.z();
      xyzl.label = index;
      cloud.push_back(xyzl);
    }
    index++;
  }
  return cloud;
}
/// @brief 当接收到 Lanelet2 地图消息时的回调函数
/// @param msg Lanelet2 地图消息
void Ll2Decomposer::on_map(const HADMapBin & msg)
{
  RCLCPP_INFO_STREAM(get_logger(), "subscribed binary vector map");
  lanelet::LaneletMapPtr lanelet_map(new lanelet::LaneletMap);//解析 Lanelet2 地图数据
  lanelet::utils::conversion::fromBinMsg(msg, lanelet_map);
  print_attr(lanelet_map, get_logger());

  const rclcpp::Time stamp = msg.header.stamp;

  const auto & ls_layer = lanelet_map->lineStringLayer;
  const auto & po_layer = lanelet_map->polygonLayer;
  //提取特定的线条和多边形数据
  auto tmp1 = extract_specified_line_string(ls_layer, sign_board_labels_);
  auto tmp2 = extract_specified_line_string(ls_layer, road_marking_labels_);
  pcl::PointCloud<pcl::PointNormal> ll2_sign_board = split_line_strings(tmp1);
  pcl::PointCloud<pcl::PointNormal> ll2_road_marking = split_line_strings(tmp2);
  pcl::PointCloud<pcl::PointXYZL> ll2_bounding_box = load_bounding_boxes(po_layer);

//将提取的数据转换为点云格式并发布
  publish_additional_marker(lanelet_map);

  common::publish_cloud(*pub_road_marking_, ll2_road_marking, stamp);
  common::publish_cloud(*pub_sign_board_, ll2_sign_board, stamp);
  common::publish_cloud(*pub_bounding_box_, ll2_bounding_box, stamp);

  RCLCPP_INFO_STREAM(get_logger(), "succeeded map decomposing");
}

/// @brief 将 Lanelet2 地图中的线条字符串分割为点云数据
/// @param line_strings 线条字符串
/// @return 
pcl::PointCloud<pcl::PointNormal> Ll2Decomposer::split_line_strings(
  const lanelet::ConstLineStrings3d & line_strings)
{
  pcl::PointCloud<pcl::PointNormal> extracted;
  for (const lanelet::ConstLineString3d & line : line_strings) {
    lanelet::ConstPoint3d const * from = nullptr;
    for (const lanelet::ConstPoint3d & to : line) {//遍历线条字符串，将每条线段的起点和终点转换为点云中的点
      if (from != nullptr) {
        pcl::PointNormal pn = to_point_normal(*from, to);
        extracted.push_back(pn);
      }
      from = &to;
    }
  }
  return extracted;
}

/// @brief 提取地图中具有指定标签的线条字符串
/// @param line_string_layer 线条字符串层
/// @param visible_labels 指定的标签
/// @return 
lanelet::ConstLineStrings3d Ll2Decomposer::extract_specified_line_string(
  const lanelet::LineStringLayer & line_string_layer, const std::set<std::string> & visible_labels)
{
  lanelet::ConstLineStrings3d line_strings;
  for (const lanelet::ConstLineString3d & line : line_string_layer) {
    if (!line.hasAttribute(lanelet::AttributeName::Type)) continue;
    lanelet::Attribute attr = line.attribute(lanelet::AttributeName::Type);
    if (visible_labels.count(attr.value()) == 0) continue;
    line_strings.push_back(line);
  }
  return line_strings;
}

/// @brief 提取地图中具有指定标签的多边形
/// @param polygon_layer 多边形层
/// @param visible_labels 指定的标签
/// @return 
lanelet::ConstPolygons3d Ll2Decomposer::extract_specified_polygon(
  const lanelet::PolygonLayer & polygon_layer, const std::set<std::string> & visible_labels)
{
  lanelet::ConstPolygons3d polygons;
  for (const lanelet::ConstPolygon3d & polygon : polygon_layer) {
    if (!polygon.hasAttribute(lanelet::AttributeName::Type)) continue;
    lanelet::Attribute attr = polygon.attribute(lanelet::AttributeName::Type);
    if (visible_labels.count(attr.value()) == 0) continue;
    polygons.push_back(polygon);
  }
  return polygons;
}

/// @brief 将两个点转换为具有法线信息的点云点
/// @param from 起始点
/// @param to 终点
/// @return 
pcl::PointNormal Ll2Decomposer::to_point_normal(
  const lanelet::ConstPoint3d & from, const lanelet::ConstPoint3d & to) const
{
  pcl::PointNormal pn;
  pn.x = from.x();
  pn.y = from.y();
  pn.z = from.z();
  pn.normal_x = to.x();
  pn.normal_y = to.y();
  pn.normal_z = to.z();
  return pn;
}

/// @brief 从线条数据创建 Marker 消息
/// @param line_string_layer 线条字符串层
/// @param labels 指定的标签
/// @param ns 命名空间
/// @return 
Ll2Decomposer::MarkerArray Ll2Decomposer::make_sign_marker_msg(
  const lanelet::LineStringLayer & line_string_layer, const std::set<std::string> & labels,
  const std::string & ns)
{
  lanelet::ConstLineStrings3d line_strings =
    extract_specified_line_string(line_string_layer, labels);

  MarkerArray marker_array;
  int id = 0;
  for (const lanelet::ConstLineString3d & line_string : line_strings) {
    Marker marker;
    marker.header.frame_id = "map";
    marker.header.stamp = get_clock()->now();
    marker.type = Marker::LINE_STRIP;
    marker.color = common::Color(0.6f, 0.6f, 0.6f, 0.999f);
    marker.scale.x = 0.1;
    marker.ns = ns;
    marker.id = id++;

    for (const lanelet::ConstPoint3d & p : line_string) {
      geometry_msgs::msg::Point gp;
      gp.x = p.x();
      gp.y = p.y();
      gp.z = p.z();
      marker.points.push_back(gp);
    }
    marker_array.markers.push_back(marker);
  }
  return marker_array;
}

/// @brief 从多边形数据创建 Marker 消息
/// @param polygon_layer 多边形层
/// @param labels 指定的标签
/// @param ns 命名空间
/// @return 
Ll2Decomposer::MarkerArray Ll2Decomposer::make_polygon_marker_msg(
  const lanelet::PolygonLayer & polygon_layer, const std::set<std::string> & labels,
  const std::string & ns)
{
  lanelet::ConstPolygons3d polygons = extract_specified_polygon(polygon_layer, labels);

  MarkerArray marker_array;
  int id = 0;
  for (const lanelet::ConstPolygon3d & polygon : polygons) {
    Marker marker;
    marker.header.frame_id = "map";
    marker.header.stamp = get_clock()->now();
    marker.type = Marker::LINE_STRIP;
    marker.color = common::Color(0.4f, 0.4f, 0.8f, 0.999f);
    marker.scale.x = 0.2;
    marker.ns = ns;
    marker.id = id++;

    auto gen_point = [](const lanelet::ConstPoint3d & p) -> geometry_msgs::msg::Point {
      geometry_msgs::msg::Point gp;
      gp.x = p.x();
      gp.y = p.y();
      gp.z = p.z();
      return gp;
    };

    for (const lanelet::ConstPoint3d & p : polygon) marker.points.push_back(gen_point(p));
    marker.points.push_back(gen_point(polygon.front()));

    marker_array.markers.push_back(marker);
  }
  return marker_array;
}

/// @brief 发布额外的 Marker 消息
/// @param lanelet_map lanelet的地图
void Ll2Decomposer::publish_additional_marker(const lanelet::LaneletMapPtr & lanelet_map)
{
  auto marker1 =
    make_sign_marker_msg(lanelet_map->lineStringLayer, sign_board_labels_, "sign_board");
  auto marker2 = make_sign_marker_msg(lanelet_map->lineStringLayer, {"virtual"}, "virtual");
  auto marker3 =
    make_polygon_marker_msg(lanelet_map->polygonLayer, {"bounding_box"}, "bounding_box");

  std::copy(marker2.markers.begin(), marker2.markers.end(), std::back_inserter(marker1.markers));
  std::copy(marker3.markers.begin(), marker3.markers.end(), std::back_inserter(marker1.markers));
  pub_marker_->publish(marker1);
}

1.3 extract_line_segments节点

这个节点的目的是从一个包含线段的点云中提取那些距离一个特定变换（或位置）在一定范围内的线段

/// @brief 从点云中提取线段
/// @param line_segments 分割的线段
/// @param transform 位姿变换
/// @param max_range 最大距离
/// @return 
pcl::PointCloud<pcl::PointNormal> extract_near_line_segments(
  const pcl::PointCloud<pcl::PointNormal> & line_segments, const Sophus::SE3f & transform,
  const float max_range)
{
  constexpr double sqrt_two = std::sqrt(2);//计算正方形对角线的长度
  const Eigen::Vector3f pose_vector = transform.translation();

  // All line segments contained in a square with max_range on one side must be taken out,
  // so pick up those that are closer than the **diagonals** of the square.
  //（表示线段的一个点）是否在距离范围内
  auto check_intersection = [max_range, pose_vector](const pcl::PointNormal & pn) -> bool {
    const Eigen::Vector3f from = pn.getVector3fMap() - pose_vector;
    const Eigen::Vector3f to = pn.getNormalVector3fMap() - pose_vector;

    Eigen::Vector3f tangent = to - from;
    if (tangent.squaredNorm() < 1e-3f) {
      return from.norm() < sqrt_two * max_range;
    }

    float inner = from.dot(tangent);
    float mu = std::clamp(inner / tangent.squaredNorm(), -1.0f, 0.0f);
    Eigen::Vector3f nearest = from - tangent * mu;
    return nearest.norm() < sqrt_two * max_range;//计算点到线段的最近点，并检查这个最近点的距离是否在允许的对角线距离内
  };

  pcl::PointCloud<pcl::PointNormal> dst;
  for (const pcl::PointNormal & pn : line_segments) {
    if (check_intersection(pn)) {
      dst.push_back(pn);
    }
  }
  return dst;
}

2. yabloc_image_processing节点代码阅读

这个文件夹中包含了一些与图像处理相关的可执行节点。

2.1 graph_segment

...详情请参照古月居