cartographer若干疑问

ancientcc

一、[未解决]pose_graph_2d.cc中抛出异常

执行transform::operator*(...)时，参数lhs值是0xFFFFFFFF，抛出内存访问异常。这异常可能一开始就出现，也可能晃动几次激光雷达后出现。

图1 transform::operator*(...)

查找是谁调用transform::operator*(...)导致导常，查下有两处：PoseGraph2D::InitializeGlobalSubmapPoses、PoseGraph2D::ComputeConstraintsForNode，它们都可能抛出异常。

<cartographer>/mapping/internal/2d/pose_graph_2d.cc
------
std::vector<SubmapId> PoseGraph2D::InitializeGlobalSubmapPoses(
    const int trajectory_id, const common::Time time,
    const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps) {
  ...
  if (data_.submap_data.at(last_submap_id).submap ==
      insertion_submaps.front()) {
    // In this case, 'last_submap_id' is the ID of
    // 'insertions_submaps.front()' and 'insertions_submaps.back()' is new.

    // 原先代码++
    const auto& first_submap_pose = submap_data.at(last_submap_id).global_pose;
    optimization_problem_->AddSubmap(
        trajectory_id,
        first_submap_pose *
            constraints::ComputeSubmapPose(*insertion_submaps[0]).inverse() *
            constraints::ComputeSubmapPose(*insertion_submaps[1]));
    // ++原先代码

    // modify...
    // 如果改为“const transform::Rigid2d& first_submap_pose =”，即first_submap_pose是引用，也会抛出异常。
    const transform::Rigid2d first_submap_pose = submap_data.at(last_submap_id).global_pose;
    const transform::Rigid2d global_submap_pose = first_submap_pose *
            constraints::ComputeSubmapPose(*insertion_submaps[0]).inverse() *
            constraints::ComputeSubmapPose(*insertion_submaps[1]);

    optimization_problem_->AddSubmap(
        trajectory_id,
        global_submap_pose);
    // ...modify

    return {last_submap_id,
            SubmapId{trajectory_id, last_submap_id.submap_index + 1}};
  }
  ...
}

WorkItem::Result PoseGraph2D::ComputeConstraintsForNode(
    const NodeId& node_id,
    std::vector<std::shared_ptr<const Submap2D>> insertion_submaps,
    const bool newly_finished_submap) {
    ...
    // 原先代码++
    const transform::Rigid2d global_pose_2d =
        optimization_problem_->submap_data().at(matching_id).global_pose *
        constraints::ComputeSubmapPose(*insertion_submaps.front()).inverse() *
        local_pose_2d;
    // ++原先代码

    // modify...
    // 如果改为“const transform::Rigid2d& global_pose =”，即global_pose是引用，也会抛出异常。
    const transform::Rigid2d global_pose =
        optimization_problem_->submap_data().at(matching_id).global_pose;

    const transform::Rigid2d global_pose_2d =
        global_pose *
        constraints::ComputeSubmapPose(*insertion_submaps.front()).inverse() *
        local_pose_2d;
    // ...modify

  ...
}

总的来说，pose_graph_2d.cc要从submap_data、optimization_problem_->submap_data()取出位姿，以便计算其它位姿，计算时导致异常。

到现在未找到原因。但发现一个现象，要是从submap_data、optimization_problem_->submap_data()取出位姿后先存到一个本地变量，用这本地变量进行后绪计算，好像就不抛出异常了。由于未找到原因，这种方法不确定是否能解决问题。

 

二、[未解决](grid_2d.cpp)Grid2D::GrowLimits中的“MapLimits new_limits(...)”抛出异常

在Grid2D::GrowLimits()执行“MapLimits new_limits(...)”时，抛出内存访问异常。这异常可能一启动cartographer就出现，也可能运行一段时间后出现。

修改方法是把参数“limits_.max()”单独生成一变量old_max，然后把这变量传给“MapLimits new_limits”。

<cartographer>/cartographer/mapping\2d\grid_2d.cc
------
void Grid2D::GrowLimits(const Eigen::Vector2f& point, ...)
{
    ...
    // const Eigen::Vector2d& old_max = limits_.max();

如果定义old_max是引用，也会抛出这异常

    const Eigen::Vector2d old_max = limits_.max();
    const MapLimits new_limits(
        limits_.resolution(),
        // limits_.max() +
        old_max +
            limits_.resolution() * Eigen::Vector2d(y_offset, x_offset),
        CellLimits(2 * limits_.cell_limits().num_x_cells,
                   2 * limits_.cell_limits().num_y_cells));
    ...
}

如果新增变量old_max不是值，而是引用，也会抛出这异常。

这么改后，看不出和原来代码有啥不同。由于未找到原因，这种方法不确定是否能解决问题。

Microsoft Visual Studio Professional 2022 (64-bit) - Current
Version 17.10.1

这问题不是一开始就有。cartographer至少用了2年，2024年6月，把Visual Studio升级到17.10.1后，突然就冒出来。

 

三、PoseGraph2D::WaitForAllComputations()出现阻塞

详细内容见“Global SLAM”中“PoseGraph2D::WaitForAllComputations()出现阻塞”。

 

四、先销毁cartographer_occupancy_grid_node，再销毁cartographer_node

图3 ServiceManager::call()

cartographer包需要运行两个节点，一个是cartographer_node，一个是cartographer_occupancy_grid_node。停止时，一定要先销毁cartographer_occupancy_grid_node，确保它已销毁后，再停掉cartographer_node的cbqueue.callAvailable()。当然，做这操作，基本就是销毁cartographer_node。否则会造成销毁cartographer_node时，会因为在ServiceManager::unadvertiseService死等shutting_down_mutex_，导致程序锁死。

让看下先销毁cartographer_node，会发生什么事。

对cartographer_occupancy_grid_node节点，它的功能是画栅格图，画栅格图是通过不断（定时）向cartographer_node的“/submap_query”服务发请求，让获得当前子图集。图1显示了ServiceManager::call()，它处理如何发出请求。首先要申请掉shutting_down_mutex_，在发出“/submap_query”请求后，会死等“e”有信号。“e”啥时有信号？——cartographer_node节点在处理了这个“/submap_query”请求后。可此时cartographer_node已处于正在销毁，它不会再调用cbqueue.callAvailable()，于是cartographer_occupancy_grid_node只能继续等在这里。它造成的另一个恶果：会一直占用shutting_down_mutex_。

再看此时cartographer_node，正处在销毁中，过程中会析构cartographer_ros::Node对象。

<cartographer_ros>/cartographer_ros/cartographer_ros/node.h
------
namespace cartographer_ros {
class Node {
  std::vector<::ros::ServiceServer> service_servers_;
};
}

成员变量service_servers_存储着cartographer_node提供的服务，当中就有“/submap_query”。在析构某个ServiceServer时，会先调用unadvertiseService注销服务。

<libros>/ros_comm/roscpp/src/libros/service_manager.cpp
------
bool ServiceManager::unadvertiseService(const string &serv_name)
{
  boost::recursive_mutex::scoped_lock shutdown_lock(shutting_down_mutex_);
  ...
}

可以看到，unadvertiseService首先要申请shutting_down_mutex_，可这锁此时等被图1的ServiceManager::call()占用着，结果造成程序死锁在unadvertiseService。

解决办法是在销毁cartographer_node节点前，确保没有节点会向它发服务请求，像“/submap_query”。换句话说，先销毁cartographer_occupancy_grid_node，再销毁cartographer_node。

 

五、cartographer开始建图后，停在断点，继续运行后建出的图像会漂移

cartographer开始建图后，设置断点，停在断点一点时间，继续运行后，建出的图像会漂移，是什么原因？

 

六、是否能优化Node::HandleLaserScanMessage的执行时间

分析Node::HandleLaserScanMessage中的各任务，以及它们的执行时间。在android下，找到如何缩短时间的方法。

 

七、调试导航

7.1 验证cartographer发出的点云和ObstacleLayer::pointCloud2Callback收到的点云是否一致

  1. 让map-->odom tf发出一个不是0的固定变换。方法：修改MapBuilderBridge::GetLocalTrajectoryData()，让local_to_map返回一个非0位姿。
  2. 在Node::PublishLocalTrajectoryData，把发出的点云数据sensor_msgs::PointCloud2.data存成文件，须要的话map、odom各一个。
  3. 在ObstacleLayer::pointCloud2Callback，把收到的点云数据sensor_msgs::PointCloud2.data存成文件。
  4. 比较接收、发送端两处的sensor_msgs::PointCloud2.data文件，看是否一致。

 

八、ceres

目前用的ceres是2021-6-28从github下载的版本。曾在2023-6-23从github下载ceres，编译后发现几个问题。

8.1 在windows下用CMake编译2023-6-23版本ceres

-- Failed to find glog - Could not find glog library, set GLOG_LIBRARY to full path to libglog.
CMake Error at CMakeLists.txt:448 (message):
  Can't find Google Log (glog).  Please set either: glog_DIR (newer CMake
  built versions of glog) or GLOG_INCLUDE_DIR & GLOG_LIBRARY or enable
  MINIGLOG option to use minimal glog implementation.

如果报以上错，参考“cmake报错CMake Error at CMakeLists.txt: Can‘t find Google Log (glog). Please set either ... 的解决方法”。方法是修改CMakeLists.txt，让不用glog，改为用MiniGlog。

<ceres-solver>/CMakeLists.txt
------
# MiniGLog.
set(MINIGLOG 1)  # 增加这条语句
if (MINIGLOG)
  message("-- Compiling minimal glog substitute into Ceres.")
...

 

8.2 2023-6-23版本已缺少cartographer需要的一些类型

<cartographer>/cartographer/mapping/internal/optimization/ceres_pose.cc
------
CeresPose::CeresPose(
    const transform::Rigid3d& pose,
    std::unique_ptr<ceres::LocalParameterization> translation_parametrization,
    std::unique_ptr<ceres::LocalParameterization> rotation_parametrization,
    ceres::Problem* problem)
    : data_(std::make_shared<CeresPose::Data>(FromPose(pose))) {
  problem->AddParameterBlock(data_->translation.data(), 3,
                             translation_parametrization.release());
  problem->AddParameterBlock(data_->rotation.data(), 4,
                             rotation_parametrization.release());
}

2023-6-23版本中没有定义上面出现的ceres::LocalParameterization。

 

8.3 2023-6-23需要C++17

namespace ceres::internal {
}

类似以上语法须要C++17，这语法到处在用。除这外，还有其它需要C++17的语法。

目前launcher+kdesktop的是C++14。编译2023-6-23版本似乎并不需要abseil-cpp。因为存在5.2这问题，先不升级ceres。

发布于 2024-11-08 19:53:17

全部评论: 0 条

    写评论: