Webots中从亲戚处获取激光雷达点云全局坐标的问题

Question

我需要使用 Webots 中的移动机器人使用激光雷达对周围环境进行自定义映射。我用什么：

• 用于获取机器人位置的 GPS。
• 获取方向机器人的指南针。
• 用于获取周围环境信息的激光雷达。

也许熟悉 Webots 的人可以展示基本代码示例或解释其背后的数学原理，或者我在 Webots 中错过了一种方法？

我对激光雷达的相对点进行了平移和旋转，当机器人在平面上时效果很好（2D 旋转）。 但是无论我尝试了多少，当机器人稍微倾斜（3D 旋转）时，我都无法弄清楚如何从点云相对点获得准确的全局坐标。

我的猜测是它应该使用空间变换矩阵，但我不确定如何在旋转矩阵中使用 Webots Compass 值.

Answer 1

在StackExchange 得到一些有用的信息后。 Python 解决方案的基本示例：

from scipy.spatial.transform import Rotation as Rotation

RobotPoint = gps.getValues()
STR = Rotation.from_quat(InertialUnit.getQuaternion())
for RelativeCloudPoint in lidar.getPointCloud():
  Point2 = STR.apply(RelativeCloudPoint)
  GlobalCloudPoint = RelativeCloudPoint + RobotPoint

使用 InternalUnit 获取空间旋转矩阵的四元数。然后将其应用于相对坐标。之后添加来自 GPS 的真实机器人坐标。最后，您将获得所需点的全局坐标。