PointCloud visualization reference frames

[dakejahl]

I have a drone with a depth camera attached and I want to visualize the point cloud. The PointCloud data is in body frame of the drone and the visualizer displays the data in world frame. Is there a way to know which link the data came from and a transformation published that we can use?

Continuation of the conversation in #346 (comment)

Here's the model I'm using for the drone, you can see how the camera is attached
https://github.com/PX4/PX4-Autopilot/blob/main/Tools/simulation/gz/models/x500_depth/model.sdf

I have some example code where I am downsampling a camera point cloud and republishing it
dakejahl/PX4-Autopilot@02ef528

Here's a pic of a drone with a depth camera looking at a block some distance from the origin

[azeey]

The reference frame of a point cloud message is set in the header (see https://github.com/gazebosim/gz-sensors/blob/ec52913e608ce8f64053650bfcced84a70ca12e2/src/DepthCameraSensor.cc#L557), so it should be easy to retrieve it. The issue is, since we don't have something equivalent to TF in ROS, knowing the transform from the body frame to the world will be difficult. Have you considered using RViz for this?

If you want to implement this for Gazebo, my suggestion would be to make it so that markers can be published with custom reference frames. Then, in gz-sim's MarkerManager, it would be possible to obtain the entity given a frame name
via entitiesFromScopedName. Once you have the entity, you can find its world pose using worldPose.