计算距 TangoPoseData 的距离

Question

我的 Project Tango 设备记录了两个 Java TangoPoseData 观察结果。如何计算这两个观测值之间的距离（以米为单位）？

Answer 1

来自Coordinate Pair Frames Documentation：

对于所有坐标系对，测量单位都是米。

还有……

Project Tango 对 START_OF_SERVICE 和 AREA_DESCRIPTION 坐标系使用右手的局部级框架。此约定将 Z 轴设置为与重力对齐，Z+ 指向上方，并且 X-Y 平面垂直于与地平面局部水平的重力。

因此，坐标可以与JTS Topology Suite 等标准库一起使用，用于使用START_OF_SERVICE 和AREA_DESCRIPTION 坐标框架 的坐标。

public static Coordinate coordinate(TangoPoseData pose){

    return new Coordinate(pose.translation[TangoPoseData.INDEX_TRANSLATION_X],
                pose.translation[TangoPoseData.INDEX_TRANSLATION_Y],
                pose.translation[TangoPoseData.INDEX_TRANSLATION_Z]);
}

坐标提供distance method：

double distanceInMeters = coordinate(pose1).distance(coordinate(pose2));