OMPL 入门Tutorial 5：可视化求解路径(Visualizing solution paths)

以下内容为OMPL官方Tutorial Visualizing solution paths的翻译

Visualizing solution paths 可视化求解路径

OMPL不包含任何内置可视化工具，但一般用外部软件来可视化不会太难。一般的做法是将路径作为一个矩阵打印，保存为一个text文件，然后在外部程序中打开文件。OMPL中有两种路径： ompl::geometric::PathGeometric 和 ompl::control::PathControl。两个类都有一个方法叫printAsMatrix()。对于 ompl::geometric::PathGeometric而言，矩阵包含了路径上的所有状态，每行对应一个状态。对应 ompl::control::PathControl，每一行也包含controls（控制点？）和从上一个状态到达该状态所需要的控制时间（第一行的控制点和时间都为0）。在 ompl::control::PathControl中，你可能会想转换path为一个集合路径。这个转换自动以一定的步长对路径做插值。基本用法如下。

bool solved = ss.solve(20.0);
if (solved)
{
    // ss 为 a ompl::geometric::SimpleSetup object
    ss.getSolutionPath().printAsMatrix(std::cout);
 
    // ss 为 ompl::control::SimpleSetup object
    ss.getSolutionPath().asGeometric().printAsMatrix(std::cout);
}

运行代码并保存printAsMatrix()的输出到一个文件，比如path.txt。假设路径保存的是SE(3)状态。这就意味着前三列表示3D位置，另外四列表示四元数姿态。下面我们将演示如何用一些方法可视化路径。

• OMPL.app GUI：OMPL.app GUI可以被用于回放任何SE(2)或SE(3)路径。首先，加载一个表示机器人的mesh和（可加可不加）环境。然后，加载path文件，你可以令GUI循环加载每一个状态或者直接同时显示所有状态。
• Matplotlib：代码见原文
• Matlab / Octave：代码见原文
• Excel：当你用Excel打开path.txt，它会自动引导你把文件加载到spreadsheet里。你需要选择space-delimited data。不幸的是，在Excel里画一个3D曲线似乎比较困难。然而你可以画2D曲线。要画这样一条曲线，选择前两列数据并选择"Insert>Chart…“。然后，点击"Scatter>Straight Marked Scatter”。（适用于Mac 2011, Microsoft Excel；在Windows中菜单选项名称可能有些区别）
• R语言：代码见原文
• gnuplot：代码见原文

在每个位置用一个坐标系表示4-7列代表的姿态信息是十分简单直观的，因此这里留给读者作为练习。