I need to do custom mapping of surroundings with lidar using mobile robot in Webots. What I use for that:
- GPS for getting robot position.
- Compass for getting direction robot.
- Lidar for getting info about surroundings.
I did translation and rotation of relative points from lidar, which worked well when robot is on flat surface.
But no matter how much I tried I can't figure out how to get accurate global coordinates from point cloud relative points, when robot is even a bit tilted.
My guess is that it suppose to use spatial transformation matrices, but I not sure how to use Webots Compass values in rotation matrix.
Maybe someone can show code example or explain the math behind it or there is a method that I missed in Webots?