博客

记轨迹系 为 L 系，     当前帧系 为 c 系， 重力系为 W 系

我们想要做的是：

1. 估计的 3D pose Tlc 的投影，直接取 x y —– 在 L系下进行投影

2. 当前帧的点云投影，直接取 x y  — 必须满足：当前帧系 与L系的 Z 轴重合

如果机器人肯定是在平面上运动，那么很简单，直接取 x y 即可。

如果机器人是一个3D空间的运动呢，如何达到上述要求呢？难点是第2点，地图系是固定的，但是当前帧系是变化的啊，如何保证两者的 Z 轴重合呢？

借助重力系。

1. L 系 也必须跟重力系对齐，这样才能 直接取 x y 。因为点云变换到了重力系，因为姿态也由 Tlc 变换为 Tlc*Twc.inv() 即 Tlw

2. 将当前帧系转移到重力系；即点云变换到重力系表示。

我们要保证轨迹系L 与重力对齐，cartographer中，初始化轨迹时，初始姿态默认单位阵。这是假设机器人初始水平放置。如果想适用多种情况，则 初始姿态 应该为：Tlc，  L 即 w，为重力系，C 为当前帧系。Tlc 通过重力加速度计算得到。

下面跟随代码来进行解释：

//这里面会进行激光数据的匹配．
std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult>
LocalTrajectoryBuilder2D::AddAccumulatedRangeData(
        const common::Time time,
        const sensor::RangeData& gravity_aligned_range_data,
        const transform::Rigid3d& gravity_alignment,
        const absl::optional<common::Duration>& sensor_duration)
{
    if (gravity_aligned_range_data.returns.empty())
    {
        LOG(WARNING) << "Dropped empty horizontal range data.";
        return nullptr;
    }

    // Computes a gravity aligned pose prediction.
    // 预测6自由度的位姿
    const transform::Rigid3d non_gravity_aligned_pose_prediction =
            extrapolator_->ExtrapolatePose(time);

    // 把上面的6自由度位姿投影成3自由度的位姿．
    // i 当前帧系； w 重力系 ；l local系
    // 如下代码的操作：Tlw = Tli * Twi.inv()
    // 这里的 Twi 是 IMU 不间断、连续积分的结果，因此，yaw不准确。但是没关系。
    // 我们本来是要优化 Tli，现在变成了优化Tlw。虽然Twi不准确，但没关系，我们认为Twi固定不变即可。
    // 优化方程是： Pl = Tlw * Pw
    // 当得到优化结果 Tlw时，很自然可以得到优化结果 Tli = Tlw*Twi
    // 首先，在建造3D图时，上述逻辑肯定是成立的。 
    // 如果建立2D图，需要将 pose 、点云 由 3D 投影成 2D。这里我们可以直接取 x y 即可。
    // 因为 当前帧系 和 local系 的 z 轴是统一的，即两者都对齐到了重力系。

    // 投影到2D，损失高度信息Z
    const transform::Rigid2d pose_prediction = transform::Project2D(
                non_gravity_aligned_pose_prediction * gravity_alignment.inverse());

    //进行滤波．
    //激光点表示在w系；
    //TransformToGravityAlignedFrameAndFilter中已经经过体素滤波了，怎么还来一次？
    const sensor::PointCloud& filtered_gravity_aligned_point_cloud =
            sensor::AdaptiveVoxelFilter(options_.adaptive_voxel_filter_options())
            .Filter(gravity_aligned_range_data.returns);

    if (filtered_gravity_aligned_point_cloud.empty())
    {
        return nullptr;
    }

    std::unique_ptr<transform::Rigid2d> pose_estimate_2d =
            ScanMatch(time, pose_prediction, filtered_gravity_aligned_point_cloud);

    if (pose_estimate_2d == nullptr)
    {
        LOG(WARNING) << "Scan matching failed.";
        return nullptr;
    }
    
    // 这个函数体内部进行的所有重力对齐操作，都是基于一个假设：初始姿态水平放置，即local系 与 重力系 统一
    //把位姿重新换回到６自由度．
    // 有必要再转回3D pose，因为预测姿态时用到的IMU、odom 都是提供的3D信息。
    // 如果只用恒速模型进行预测，则没必要转为3D了。
    // pose_estimate 比 non_gravity_aligned_pose_prediction 有如下的改进：
    // 1. xy,yaw 进行了scanmatch校正
    // 2. roll pitch 进行了重力校正
    // 3. 损失了高度信息，z=0了
    const transform::Rigid3d pose_estimate =
            transform::Embed3D(*pose_estimate_2d) * gravity_alignment;

    extrapolator_->AddPose(time, pose_estimate);

    //把点云转换到估计出来的位姿中．即Submap坐标系中．
    // Pl = Tlw * Pw
    sensor::RangeData range_data_in_local =
            TransformRangeData(gravity_aligned_range_data,
                               transform::Embed3D(pose_estimate_2d->cast<float>()));

    //把点云插入到SubMap中．
    // insertion_result保存了 pose_estimate 和 gravity_alignment；
    // 在加入posegraph时，会根据这两者，变换得到 pose_estimate_2d，加入posegraph
    std::unique_ptr<InsertionResult> insertion_result = InsertIntoSubmap(
                time, range_data_in_local, filtered_gravity_aligned_point_cloud,
                pose_estimate, gravity_alignment.rotation());
    // 统计时间
    const auto wall_time = std::chrono::steady_clock::now();
    if (last_wall_time_.has_value())
    {
        const auto wall_time_duration = wall_time - last_wall_time_.value();
        kLocalSlamLatencyMetric->Set(common::ToSeconds(wall_time_duration));
        if (sensor_duration.has_value())
        {
            kLocalSlamRealTimeRatio->Set(common::ToSeconds(sensor_duration.value()) /
                                         common::ToSeconds(wall_time_duration));
        }
    }

    const double thread_cpu_time_seconds = common::GetThreadCpuTimeSeconds();
    if (last_thread_cpu_time_seconds_.has_value())
    {
        const double thread_cpu_duration_seconds =
                thread_cpu_time_seconds - last_thread_cpu_time_seconds_.value();
        if (sensor_duration.has_value())
        {
            kLocalSlamCpuRealTimeRatio->Set(
                        common::ToSeconds(sensor_duration.value()) /
                        thread_cpu_duration_seconds);
        }
    }

    last_wall_time_ = wall_time;
    last_thread_cpu_time_seconds_ = thread_cpu_time_seconds;
    // 返回匹配的结果；
    return absl::make_unique<MatchingResult>(
                MatchingResult{time, pose_estimate, std::move(range_data_in_local), // 点云在局部子图中的表示（没有经过滤波）
                               std::move(insertion_result)});
}

下面跟随代码来进行解释：

有三个要注意：

1）gravity_alignment  yaw不准确，但是不影响结果。

2）有必要将 2D pose再次转回 3D pose

3）通过odom估计速度时，使用的 R 并不是IMU连续积分的结果，而是基于上一个pose + IMU 帧间pose 预测的结果R

— 如果没有IMU就没法做了，除非假设机器人始终在平面运动，可以直接取 x y 。

—  cartographer 应该不只是用于扫地机器人场景，还可能是 人背包场景。

— cartographer中的代码设置是：如果不使用IMU，则 gravity_alignment 是单位阵，相当于没有进行重力对齐，就是直接取 x y 。如果机器人在平面运动OK， 如果机器人爬坡时，3D点云就不能简单的取 x y 来投影到 2D了，因为3D点云是表示在当前帧系的，所以这种方法投影是错误的。