How to get the pose in RViz to match that of the Pangolin GUI?

[martinakos]

I'm running dm-vio-ros from a rosbag. I visualise both dmvio/unscaled_pose and dmvio/metric_pose topics. It takes a few seconds for dm-vio to initialise at beginning of the sequence. During that time I can see the dmvio/unscaled_pose being updated and only after the initialisation dmvio/metric_pose starts to be updated. As a result of this delay the dmvio/metric_pose has missing the first few seconds of the trajectory. In the Pangolin GUI, however, I see that after the initialisation the GUI shows the camera pose quickly catching up to where is should be after the initial delay and the trajectory seems complete (without the first section missing, like in dmvio/metric_pose rviz).

I wonder if the missing part in dmvio/metric_pose is intended?

If I create another pose that is the same that dmvio/unscaled_pose and then switches to dmvio/metric_pose (composed with the last unscaled_pose) when dmvio/system_status is 2, would I get the same pose as in the GUI?

Also, notice, for the moment I'm interested in the instant pose, even if the scale is more accurate after having traversed the whole sequence.

[martinakos]

Ok, I see that my switching and composing topic is not going to work because the dmvio/unscaled_pose can have a different scale every time and when switching to use the dmvio/metric_pose this would start at different point every time depending on the scale of dmvio/unscaled_pose.

Any suggestions how could I get the same trajectory in Rviz and Pangolin?

[martinakos]

Closing this as I just realise I created the problem myself. I was using a testing framework that I haven't used for a while and it was automatically aligning the first pose of the trajectories to the origin.

[martinakos]

Sorry I'm reopening this issue as I still get different trajectories in Rviz wrt. Pangolin. See the images below: The first image is a screenshot of the section of the trajectory so far in the Pangolin gui. The second is in Rviz. The orange trajectory is the ground truth and green trajectory is the /dmvio/metric_pose topic. Both are being created as I play a rosbag. You can see that the dmvio/metric_pose starts a few seconds after the camera motion starts, that's due to the IMU initialisation I presume. But then it's more noisy and looks different than the trajectory in the Pangolin gui. Why is that?

[lukasvst]

I would generally recommend to use the unscaled_pose, and scale it yourself with the current scale, instead of the metric_pose.

What you see is in Pangolin is the unscaled pose and the entire trajectory is scaled with the newest scale estimate. (Technically it is actually not even scaled, but instead, the groundtruth is scaled with the inverse scale.)

In Rviz you see the metric pose, but each point of the trajectory is scaled with the scale which was available at this point in time. Hence, the trajectory looks less consistent, as differents parts of it are scaled with different scales (and the beginning is not available, as at this time there was no scale estimate at all.)

So if you only need the current pose in metric scale you can use metric_pose, but if you want to perform operations on more than one pose (e.g. the entire trajectory), you should always use unscaled_pose and transform the entire trajectory yourself with the newest scale. I'm not sure if / how this can be visualized in Rviz, maybe a Matrix transformation can somehow be applied to the entire trajectory.

(If you need a fixed scale for you application, you can also set the setting_scaleFixTH (see https://github.com/lukasvst/dm-vio/blob/master/src/IMU/IMUSettings.h#L68). But this is something I would not recommend if it is not absolutely necessary.)

[martinakos]

Understood. Thanks. I'm interested in the online pose estimates so for the moment I'll go with the metric pose for my tests.