Open source/feature/rviz display

[ToniRV]

• Check on baseline.
• Separate ROS input vs output:
  -- RosDisplay publishes 2D images: it replaces the Display module in Kimera (instead of registering callbacks itself).
  -- RosVisualizer publishes 3D stuff: it replaces the Visualizer3D module in Kimera.
• Send RosDisplay and Viz to pipeline now, so modules are replaced.

The ROS wrapper should go faster now as well, since input and output are now de-coupled.

ToDo:

• There might be some regressions on the rosbag parsing side of things, and at the level of clock and/or images.
• I've noticed some duplicated code: publishTf

[ToniRV]

@ruffsl that might simplify the interface overall?

[ToniRV]

@violetteavi this is gone.

[violetteavi]

@ToniRV It seems like you deleted all of the publishing code. Where did it go? Did you replace it?

[violetteavi]

Note: Relies on this PR in Kimera-VIO

[ruffsl]

@ruffsl that might simplify the interface overall?

Does this switch to use Kimera-VIO's internal visualizer, rather than necessarily exporting all visualizations outputs as ROS topics for rviz? I didn't spot much of any references to rviz.

[violetteavi]

@ruffsl It still publishes all of the topics necessary for visualizing in RViz, minus the mesh texture image. I'm not sure how-- perhaps something Toni can clarify.
Example of it working on some RealSense data:

[ToniRV]

Does this switch to use Kimera-VIO's internal visualizer, rather than necessarily exporting all visualizations outputs as ROS topics for rviz? I didn't spot much of any references to rviz.

So the idea is that both RosVisualizer and OpenCvVisualizer inherit from Visualizer3D which is used by the VisualizerModule.

Since the VisualizerModule is connecting to the backend/frontend/mesher modules, it then becomes very easy to swap any of the visualizer (and even others, think of Pangolin for example), to output/visualize the synchronized packets from backend/frontend/mesher, such as the per-frame 3D mesh.

This avoids the ROS wrapper having to sync the backend/frontend/mesher output with threadsafe queues as that was already done inside Kimera-VIO via the VisualizerModule class.

Effectively this means that the ROS wrapper gets leaner, as before it had to register callbacks for the output using the VIO::Pipeline public API, while now it just passes its own implementation of Visualizer3D.

Brought to an extreme, one could even swap the whole frontend, backend, mesher, or any other module.
I actually swap as well the Display class inside the DisplayModule for a RosDisplay one instead of the default OpenCvDisplay.
Although it may sound confusing at first, the DisplayModule was meant to spin the opencv visualizer (which needs to run in the main thread, so that creation of the visualization objects were done in the Visualizer class while the display was done in the Display class).

Indeed, at the end of the day, neither RosDisplay nor RosVisualizer spin a window for displaying but rather publish those things to Ros so Rviz can do the actual displaying of topics.

[violetteavi]

Neat! This also fixes the hanging issue, as the pipeline keeps spinning even if sim_time stops. Good work!

[ToniRV]

Let's merge this thing asap if there is no more comments?
The rest of VIO PRs depend on this one.

[violetteavi]

Was about to merge with the unit test PR and run those. Doing that now

[violetteavi]

Pass! There isn't much there, but glad it works ahaha. Going to update the testing branch.