视觉Slam3 评价指标及计算方法

小丁的博客

已于 2023-08-04 10:18:32 修改

阅读量1.8k

点赞数 3

文章标签：算法计算机视觉

于 2023-08-04 10:16:23 首次发布

本文链接：https://blog.csdn.net/m0_64767429/article/details/132074860

版权

提示：文章写完后，目录可以自动生成，如何生成可参考右边的帮助文档

文章目录

RPE: relative pose error 相对位姿误差

平移均方根误差

旋转均方根误差

ATE： absolute trajectory error 绝对轨迹误差

4.鲁棒性(Robustness)

5.稳定性(Stability)

重建误差（Reconstruction Error）

轨迹误差(Trajectory Error):

总结

前言

当我们评价一个slam算法的性能时，我们需要考虑时间(time),帧数(Frames),精度(Accuracy),鲁棒性(Robustness),稳定性(Stability)这几个评价指标，本文就是着重来介绍这几个评价指标及其算法。

1.时间(Time)

在视觉SLAM算法中，评价指标中的时间通常指的是算法的执行时间或处理时间。它表示算法完成一次处理所花费的时间。
计算视觉SLAM算法的执行时间可以使用两个时间戳，即开始时间和结束时间。通过记录算法处理的整个过程所花费的时间，可以计算出算法的执行时间。
具体的计算方法如下：
1.使用一个计时器记录算法的开始时间。
2.运行视觉SLAM算法，处理输入数据（例如图像序列）。
3.使用计时器记录算法的结束时间。
4.计算执行时间，即结束时间减去开始时间。
记开始时间为tstart，结束时间为tend，则执行时间可以计算为：
执行时间 = tend - tstart

请注意，计算执行时间时需要确保开始时间和结束时间的单位相同，例如以秒为单位。此外，为了更准确地评估算法的执行时间，可以进行多次运行并取平均值。

具体的计算步骤如下所示：

import time

# 步骤1：记录起始时间
start_time = time.time()

# 步骤2：在关键时间点记录当前时间
# 执行视觉SLAM算法代码

# 步骤3：计算处理时间
end_time = time.time()
processing_time = end_time - start_time

在上面的示例代码中，我们使用了 Python 的 time 模块来获取当前时间。time.time() 函数返回当前时间的时间戳，以秒为单位。

需要注意的是，除了单次执行时间，还可以考虑算法的整体效率和处理速度，这包括每帧的处理时间、帧率和算法在不同条件下的稳定性等指标。这些指标可以帮助评估算法的实时性和适用性。

2.帧数(Frames)

对于视觉SLAM算法的评价指标中的帧数，主要指的是算法的处理速度或实时性。它表示算法能够在单位时间内处理的图像或帧数。

计算帧率的一种常见方法是使用两个时间戳，即开始时间和结束时间。通过记录算法处理一定数量的帧所花费的时间，可以计算出帧率。

计算公式如下：
帧率 = 处理的帧数 / 处理时间

具体步骤如下：

使用一个计时器，记录开始时间。
通过算法处理一定数量的图像帧。
使用计时器记录结束时间。
计算处理时间，即结束时间减去开始时间。
根据处理时间和处理的帧数计算出帧率。

例如，假设你的SLAM算法在处理过程中处理了100帧图像，开始时间为t1，结束时间为t2，那么帧率可以计算为：
帧率 = 100 / (t2 - t1)

请注意，帧率评估通常需要在与实际应用场景相似的硬件设备上进行。此外，要准确评估算法的实时性，建议在多个不同情况下进行测试，例如使用不同分辨率的图像、通过不同传感器采集的数据以及在不同负载条件下运行算法。

在视觉SLAM中，帧率是衡量算法实时性的重要指标，表示算法每秒能够处理的图像帧数。帧率的计算涉及到记录算法处理的帧数以及经过的时间，以下是一种计算帧率的示例代码：

import time

# 设置要处理的帧数
num_frames = 100

# 初始化计时器
start_time = time.time()
processed_frames = 0

# 模拟算法处理帧数
while processed_frames < num_frames:
    # 执行视觉SLAM算法
    # ...

    # 记录已处理的帧数
    processed_frames += 1

# 完成全部帧数的处理，记录结束时间
end_time = time.time()

# 计算帧率
processing_time = end_time - start_time
frame_rate = num_frames / processing_time

# 打印帧率
print("Frame rate: {:.2f} frames per second".format(frame_rate))

在上述示例代码中，我们模拟处理了 num_frames 个图像帧。通过记录开始时间和结束时间，并计算处理时间，可以得到算法的帧率。最后，我们将帧率以每秒处理的帧数（frames per second）打印出来。

请注意，示例代码中的计时器使用了 Python 的 time 模块，并调用 time.time() 函数来获取当前时间戳。计算得到的帧率是平均值，因为我们假设在处理 num_frames 个帧之间的时间是相等的。

要注意的是，实际的帧率可能受到硬件性能、图像分辨率和具体算法实现等因素的影响。因此，在实际应用中，应该在适当的硬件设备上进行测试和评估，以获得更准确的帧率结果

3.精度(Accuracy)

对于精度的评价是最重要的，在评估精度时有两个重要的评价指标：ATE和RPE

先定义一下公式标注：

算法估计位姿： $P_{1},....,P_{n}\in SE\left ( 3 \right )$

真实位姿： $Q_{1},....,Q_{n}\in SE\left ( 3 \right )$

下标代表时间t(或帧),这里我们假设估计位姿和真实位姿各帧时间已对齐，总帧数相同。 Δ 表示相隔时间time interval。

RPE: relative pose error 相对位姿误差

相对位姿误差又分为平移均方根误差与旋转均方根误差两种

相对位姿误差主要描述的是相隔固定时间差 Δ 两帧位姿差的精度（相比真实位姿），相当于直接测量里程计的误差。因此第i帧的RPE定义如下：

$E_{i}:=\left ( Q_{i}^{-1} Q_{i+\bigtriangleup } \right )^{-1}\left ( P_{i}^{-1} P_{i+\bigtriangleup } \right )$

已知总数n与间隔 Δ 的情况下，可以得到m=n −Δ 个RPE，然后我们可以用均方根误差RMSE统计这个误差，得到一个总体值：

$RMSE\left ( E_{1:n ,\bigtriangleup } \right ) :=\left ( \frac{1}{m}\sum_{i=1}^{m} \left | \right |trans\left ( E_{i} \right )\left | \right |^{2}\right )^{\frac{1}{2}}$

其中 $trains\left ( E_{i} \right )$ 代表取相对位姿误差中的平移部分translation。当然也有人不用RMSE，直接使用平均值、甚至中位数来描述相对误差情况。

当然也有人不用RMSE，直接使用平均值、甚至中位数来描述相对误差情况。需要注意的是，除了平移误差，RPE也包含旋转误差，但通常使用平移误差进行评价已经足够，如果需要，旋转角的误差也可以使用相同的方法进行统计。到这一步，我们基本可以从RMSE值的大小来评价算法的表现，然而实际情况中，我们发现对 Δ 的选取有多种选择，为了能综合衡量算法表现，我们可以计算遍历所有 Δ 的RMSE的平均值：

$RMSE\left ( E_{1:n} \right ):=\frac{1}{n}\sum_{\bigtriangleup =1}^{n}RMSE\left ( E_{1:n} ,\bigtriangleup \right )$

但这样新的问题又出现了，这样的计算复杂度非常高，很耗时间，因此TUM在自己给定的工具中，通过计算固定数量的RPE样本计算一个估计值作为最终结果。

1.平移均方根误差

在视觉SLAM中，平移均方根误差（RMSE）是一种常用的评估SLAM算法的精度指标，用于衡量估计的相机平移误差。计算平移均方根误差的步骤如下：

获取SLAM算法估计的相机平移向量和真实的相机平移向量。
计算估计平移向量与真实平移向量之间的欧氏距离误差。
对所有平移误差进行平方求和。
将平方和除以平移向量数量，得到平移均方根误差。
可以选择将平移均方根误差进行开平方，得到平移均方根误差的实际值。

以下是一个示例代码，展示如何计算平移均方根误差：

import numpy as np

# 获取SLAM算法的相机平移向量和真实的相机平移向量
slam_translations = run_slam_algorithm()  # SLAM算法估计的相机平移向量
ground_truth_translations = load_ground_truth_translations()  # 真实的相机平移向量

# 计算平移向量之间的欧氏距离误差
translation_errors = np.linalg.norm(slam_translations - ground_truth_translations, axis=1)

# 计算平移均方根误差
rmse_translation = np.sqrt(np.mean(translation_errors ** 2))

print("RMSE Translation: {:.4f}".format(rmse_translation))

在上述代码中，run_slam_algorithm()是运行SLAM算法并获取相机平移向量的函数。load_ground_truth_translations()用于加载真实的相机平移向量。通过计算估计平移向量与真实平移向量之间的欧氏距离误差，然后对平方误差求平均，最后开平方可以得到平移均方根误差（RMSE Translation）的实际值。

需要注意的是，平移均方根误差的值越小，表示SLAM算法的平移估计越准确。

2.旋转均方根误差

在视觉SLAM算法中，旋转均方根误差（RMSE）是用来评估SLAM算法旋转估计的精度指标。计算旋转均方根误差的步骤如下：

获取SLAM算法估计的相机旋转矩阵（或四元数）和真实的相机旋转矩阵（或四元数）。
计算估计旋转矩阵（或四元数）与真实旋转矩阵（或四元数）之间的误差。
将旋转误差转换为角度误差（通常是弧度）。
对所有角度误差进行平方求和。
将平方和除以旋转向量数量，得到旋转均方根误差。
可以选择将旋转均方根误差进行开平方，得到旋转均方根误差的实际值。

以下是一个示例代码，展示如何计算旋转均方根误差：

import numpy as np

# 获取SLAM算法的相机旋转矩阵（或四元数）和真实的相机旋转矩阵（或四元数）
slam_rotations = run_slam_algorithm()  # SLAM算法估计的相机旋转矩阵（或四元数）
ground_truth_rotations = load_ground_truth_rotations()  # 真实的相机旋转矩阵（或四元数）

# 计算旋转矩阵（或四元数）之间的误差
rotation_errors = calculate_rotation_errors(slam_rotations, ground_truth_rotations)

# 将旋转误差转换为角度误差（弧度）
angle_errors = convert_to_angle_errors(rotation_errors)

# 计算旋转均方根误差
rmse_rotation = np.sqrt(np.mean(angle_errors ** 2))

print("RMSE Rotation: {:.4f}".format(rmse_rotation))

在上述代码中，run_slam_algorithm()是运行SLAM算法并获取相机旋转矩阵（或四元数）的函数。load_ground_truth_rotations()是用于加载真实的相机旋转矩阵（或四元数）的函数。calculate_rotation_errors()是一个自定义函数，用于计算估计旋转矩阵（或四元数）与真实旋转矩阵（或四元数）之间的误差。convert_to_angle_errors()是另一个自定义函数，将旋转误差转换为角度误差（通常是弧度）。

需要注意的是，旋转均方根误差的值越小，表示SLAM算法的旋转估计越准确。具体的实现可能会依赖于使用的旋转表示方式（例如旋转矩阵或四元数），因此需要根据具体情况对代码进行适当的调整。

RPE（Relative Pose Error）是一种常用的评估视觉SLAM算法精度的指标。它用于衡量相对位姿估计的准确性，它是估计姿态与真实姿态之间的欧氏距离误差的平均值。。RPE计算的关键是比较估计的相机位姿与真实的相机位姿之间的差异。

获取真实的相机位姿序列和估计的相机位姿序列。
对于每一对相邻的位姿，计算它们之间的相对位姿变换。
计算旋转和平移的误差，可以选择只使用旋转或平移部分。
计算误差的欧氏距离，得到相对位姿的误差。
可以选择对所有相对位姿的误差取平均值，得到RPE的最终结果。

下面是一个示例代码，展示了如何计算RPE：

import numpy as np

def calculate_rpe(estimated_poses, ground_truth_poses):
    rpe_errors = []

    for i in range(1, len(estimated_poses)):
        estimated_pose_i_prev = estimated_poses[i-1]
        estimated_pose_i = estimated_poses[i]
        ground_truth_pose_i_prev = ground_truth_poses[i-1]
        ground_truth_pose_i = ground_truth_poses[i]

        # 计算估计位姿与真实位姿之间的相对位姿变换
        relative_pose_estimation = np.linalg.inv(estimated_pose_i_prev) @ estimated_pose_i
        relative_pose_ground_truth = np.linalg.inv(ground_truth_pose_i_prev) @ ground_truth_pose_i

        # 计算旋转和平移的误差，可以根据需求选择只使用旋转或平移部分
        rotation_error = calculate_rotation_error(relative_pose_estimation, relative_pose_ground_truth)
        translation_error = calculate_translation_error(relative_pose_estimation, relative_pose_ground_truth)

        # 计算误差的欧氏距离
        pose_error = np.sqrt(rotation_error**2 + translation_error**2)
        rpe_errors.append(pose_error)

    mean_rpe_error = np.mean(rpe_errors)

    return mean_rpe_error

def calculate_rotation_error(estimated_pose, ground_truth_pose):
    estimated_rotation = estimated_pose[:3, :3]
    ground_truth_rotation = ground_truth_pose[:3, :3]

    # 计算旋转矩阵之间的角度差
    rotation_error = np.arccos((np.trace(ground_truth_rotation.T @ estimated_rotation) - 1) / 2)

    return rotation_error

def calculate_translation_error(estimated_pose, ground_truth_pose):
    estimated_translation = estimated_pose[:3, 3]
    ground_truth_translation = ground_truth_pose[:3, 3]

    # 计算平移向量之间的欧氏距离
    translation_error = np.linalg.norm(ground_truth_translation - estimated_translation)

    return translation_error

上述代码中的estimated_poses是估计的相机位姿的集合，ground_truth_poses是真实的相机位姿的集合。首先，通过遍历每个位姿对，计算估计位姿和真实位姿之间的相对位姿变换。然后，使用calculate_rotation_error()函数计算旋转误差，使用calculate_translation_error()函数计算平移误差。最后，根据旋转和平移误差的欧氏距离计算相对位姿的误差，并计算平均RPE误差。

请注意，上述代码只是一个示例，具体的实现方式可能因应用需求和数据格式的不同而有所变化。你可以根据具体情况对代码进行修改和扩展，以满足你的需求。

ATE： absolute trajectory error 绝对轨迹误差

绝对轨迹误差是估计位姿和真实位姿的直接差值，可以非常直观地反应算法精度和轨迹全局一致性。需要注意的是，估计位姿和groundtruth通常不在同一坐标系中，因此我们需要先将两者对其：对于双目SLAM和RGB-D SLAM,尺度统一，因此我们需要通过最小二乘法计算一个从估计位姿到真实位姿的转换矩阵 S∈SE(3) ；对于单目相机，具有尺度不确定性，我们需要计算一个从估计位姿到真实位姿的相似转换矩阵S∈Sim(3) 。因此第i帧的ATE定义如下：

$F_{i}:=Q_{i}^{-1}SP_{i}$

与RPE相似，建议使用RMSE统计ATE

$RMSE\left ( F_{1:n} ,\bigtriangleup \right ):=\left ( \frac{1}{m}\sum_{i=1}^{m}\left | \right | trans\left ( F_{i} \right )\left | \right |^{2}\right )^{\frac{1}{2}}$

当然，使用平均值、中位数等来反应ATE亦可，现在很多evaluation工具会将RMSE、Mean、Median都给出。旋转误差可以通过相同的方式计算，目前的一些开源评测工具都提供了对应的选项。

ATE（Absolute Trajectory Error）：ATE用于评估整体轨迹的精度，它是估计轨迹与真实轨迹之间的欧氏距离误差的平均值。ATE越小，那么位姿估计的性能越好。

ATE计算方式如下：

首先获取SLAM算法估计的相机轨迹和真实的相机轨迹。
对齐估计的相机轨迹和真实的相机轨迹，通常使用最小二乘法来计算刚体变换（尺度、旋转和平移），使得估计的相机轨迹和真实的相机轨迹最优对齐。
计算对齐后的估计相机轨迹与真实相机轨迹之间的欧氏距离误差。

import numpy as np
from sklearn.neighbors import NearestNeighbors

# 获取SLAM算法的相机轨迹和真实的相机轨迹
slam_trajectory = run_slam_algorithm()  # SLAM算法估计的相机轨迹
ground_truth_trajectory = load_ground_truth_trajectory()  # 真实的相机轨迹

# 对齐估计相机轨迹和真实相机轨迹
def align_trajectories(slam_trajectory, ground_truth_trajectory):
    # 使用最近邻法匹配对应的帧
    nn = NearestNeighbors(n_neighbors=1)
    nn.fit(ground_truth_trajectory)

    aligned_slam_trajectory = []
    for pose in slam_trajectory:
        # 找到最近的真实帧
        _, nearest_idx = nn.kneighbors([pose])
        aligned_slam_trajectory.append(ground_truth_trajectory[nearest_idx[0][0]])

    return np.array(aligned_slam_trajectory)

aligned_slam_trajectory = align_trajectories(slam_trajectory, ground_truth_trajectory)

# 计算ATE
ate = np.linalg.norm(aligned_slam_trajectory - ground_truth_trajectory)
mean_ate = ate.mean()
print("ATE: {:.4f}".format(mean_ate))

综上，RPE与ATE具有强烈的相关性，但含义不尽相同，需结合实际，选择合适的指标进行算法评价。

4.鲁棒性(Robustness)

SLAM算法的鲁棒性指其对不同场景、光照条件、动态物体和传感器噪声等方面的适应能力。常用的鲁棒性指标包括算法的失败率（Failure Rate）和关键帧跟踪准确率（Keyframe Tracking Accuracy）以及覆盖率（Coverage）。

其中覆盖率（Coverage）是衡量一个slam3算法鲁棒性好坏的关键指标。覆盖率（Coverage）用来衡量算法在建立地图时覆盖环境的能力，。

覆盖率=成功追踪的帧数或关键点占总帧数或关键点数的比例

具体的计算过程如下：

首先定义一个地图区域的边界框或者在环境中选择一个特定的区域。
对于每个关键帧，检查它在地图区域内的投影点是否与该区域重叠。
统计落在地图区域内的关键帧的数量。
计算覆盖率，即落在地图区域内的关键帧数量除以总关键帧数量。

在ORB-SLAM或其他SLAM系统中，通常可以编写代码来计算覆盖率。下面是一个简单的示例代码片段，演示了如何计算ORB-SLAM的覆盖率：

def compute_coverage(keyframes, map_region):
    covered_keyframes = 0
    total_keyframes = len(keyframes)

    for keyframe in keyframes:
        keyframe_region = keyframe.get_region()  # 获取关键帧的投影区域
        if keyframe_region.overlaps(map_region):  # 判断关键帧区域与地图区域是否有重叠
            covered_keyframes += 1

    coverage = covered_keyframes / total_keyframes

    return coverage

在上述代码中，keyframes 是所有关键帧的集合，每个关键帧有一个方法 get_region() 用于获取其在地图上的投影区域。map_region 是用于计算覆盖率的地图区域。通过遍历每个关键帧，检查其投影区域与地图区域是否有重叠，并统计落在地图区域内的关键帧数量。最后计算覆盖率并返回结果。

请注意，上述代码只是一个示例，具体的实现可能因应用环境和数据结构的不同而有所变化。因此，在实际使用中，您可能需要根据具体需求对代码进行修改和调整。

5.稳定性(Stability)

评估视觉SLAM算法的稳定性可以使用多种指标

重建误差（Reconstruction Error）

重建误差是评估SLAM算法在重建3D场景时的准确性和稳定性的指标。可以使用重建点与地面真实点云之间的差距来衡量。以下是一个示例代码：

import numpy as np

# 获取重建点云
reconstructed_points = run_slam_algorithm()

# 计算重建误差
error = np.linalg.norm(reconstructed_points - ground_truth_points, axis=1)  # 计算欧氏距离误差

# 打印平均误差和标准差
mean_error = np.mean(error)
std_dev_error = np.std(error)
print("Mean Error: {:.4f}".format(mean_error))
print("Standard Deviation Error: {:.4f}".format(std_dev_error))

在上述代码中，run_slam_algorithm()是运行SLAM算法并获取重建点云的函数。ground_truth_points表示地面真实点云。通过计算重建点云与真实点云之间的欧氏距离误差，可以得到平均误差和标准差。