记轨迹系 为 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系 与 重力系 统一
//把位姿重新换回到6自由度.
// 有必要再转回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点云是表示在当前帧系的,所以这种方法投影是错误的。