Kimera-Multi - simulation in the gazebo

[LauGasp]

Hi, I'm new to ROS and need a little help. I'm trying to put Kimera-Multi working with a simulation in gazebo, instead of rosbag, and i was hoping for some guidance, does anyone have a suggestion for what should i change?
For now, I'm just using a pioneer 3dx robot and a realsense d435i camera. I've already got the gazebo open with the robot and sensors, the topics are beeing published, but I think I need some tips for what changes i need to do in kimera-multi in order to “run it”.

[yunzc]

Hi thank you very much for your interest in our work!
Let me try my best to outline the steps to set up Kimera-Multi for a general setup.

1. Get the single robot odometry working (in our case we run Kimera-VIO. That is what these lines in the example file does. At a high level, you just have to make sure the topics are mapped correctly and that you are pointing to the right folder that has your robot's and sensor's configurations (see our example for VIO here. The outcome of this step is that you should be able to launch VIO on each robot, input stereo images and IMU, and get a reasonable single robot odometry estimate.
2. Set up the multi-robot frontend properly. This is encompassed by this launch in our example. The main thing to do here is to get the robot ids and names correct (like this example) and other parameters like the number of robots (these lines here).
3. Set up the backend. This is encapsulated by this launch. The main key here is similar to the frontend: to get the robot names, ids, and configs correct (easiest way is to look out our launch file.
4. With steps 1. ~ 3. you should be able to get the distributed loop closures and individual robot trajectories. To continue with the dense mesh reconstruction and optimization, which is not shown in our simple example in this repo, see below.
5. The module that generates the odometric dense mesh representation is Kimera-Semantics. To get this to work you have to input the correct sensor frame and the correct topics. There is a bit of intricacy to get this to work with the correct semantic-color configurations.
6. Lastly, the module that performs the deformation of the mesh and maintains the global consistency of the mesh is Kimera-PGMO. Which takes as input the Kimera-Semantics mesh, the unoptimized pose graph from Kimera-VIO, along with this optimized trajectories from the Kimera-Multi backend.

And that's pretty much it! Hope this helps!

Best,
Yun

[LauGasp]

Hello, thank you very much for your help and attention! As I haven't managed to get it working yet (1~3), I'll see if I'm missing something...

First I launch the file in the terminal that opens the gazebo with a robot and the d435i (e.g. roslaunch p3dx_gazebo p3dx_gazebo.launch).

When I run rostopic list:

/Pionner/D435i_camera/color/camera_info
/Pionner/D435i_camera/color/image_raw
/Pionner/D435i_camera/color/image_raw/compressed
/Pionner/D435i_camera/color/image_raw/compressed/parameter_descriptions
/Pionner/D435i_camera/color/image_raw/compressed/parameter_updates
/Pionner/D435i_camera/color/image_raw/compressedDepth
/Pionner/D435i_camera/color/image_raw/compressedDepth/parameter_descriptions
/Pionner/D435i_camera/color/image_raw/compressedDepth/parameter_updates
/Pionner/D435i_camera/color/image_raw/theora
/Pionner/D435i_camera/color/image_raw/theora/parameter_descriptions
/Pionner/D435i_camera/color/image_raw/theora/parameter_updates
/Pionner/D435i_camera/depth/camera_info
/Pionner/D435i_camera/depth/image_rect_raw
/Pionner/D435i_camera/depth/image_rect_raw/compressed
/Pionner/D435i_camera/depth/image_rect_raw/compressed/parameter_descriptions
/Pionner/D435i_camera/depth/image_rect_raw/compressed/parameter_updates
/Pionner/D435i_camera/depth/image_rect_raw/compressedDepth
/Pionner/D435i_camera/depth/image_rect_raw/compressedDepth/parameter_descriptions
/Pionner/D435i_camera/depth/image_rect_raw/compressedDepth/parameter_updates
/Pionner/D435i_camera/depth/image_rect_raw/theora
/Pionner/D435i_camera/depth/image_rect_raw/theora/parameter_descriptions
/Pionner/D435i_camera/depth/image_rect_raw/theora/parameter_updates
/Pionner/D435i_camera/imu/sample
/Pionner/D435i_camera/infra1/camera_info
/Pionner/D435i_camera/infra1/image_rect_raw
/Pionner/D435i_camera/infra1/image_rect_raw/compressed
/Pionner/D435i_camera/infra1/image_rect_raw/compressed/parameter_descriptions
/Pionner/D435i_camera/infra1/image_rect_raw/compressed/parameter_updates
/Pionner/D435i_camera/infra1/image_rect_raw/compressedDepth
/Pionner/D435i_camera/infra1/image_rect_raw/compressedDepth/parameter_descriptions
/Pionner/D435i_camera/infra1/image_rect_raw/compressedDepth/parameter_updates
/Pionner/D435i_camera/infra1/image_rect_raw/theora
/Pionner/D435i_camera/infra1/image_rect_raw/theora/parameter_descriptions
/Pionner/D435i_camera/infra1/image_rect_raw/theora/parameter_updates
/Pionner/D435i_camera/infra2/camera_info
/Pionner/D435i_camera/infra2/image_rect_raw
/Pionner/D435i_camera/infra2/image_rect_raw/compressed
/Pionner/D435i_camera/infra2/image_rect_raw/compressed/parameter_descriptions
/Pionner/D435i_camera/infra2/image_rect_raw/compressed/parameter_updates
/Pionner/D435i_camera/infra2/image_rect_raw/compressedDepth
/Pionner/D435i_camera/infra2/image_rect_raw/compressedDepth/parameter_descriptions
/Pionner/D435i_camera/infra2/image_rect_raw/compressedDepth/parameter_updates
/Pionner/D435i_camera/infra2/image_rect_raw/theora
/Pionner/D435i_camera/infra2/image_rect_raw/theora/parameter_descriptions
/Pionner/D435i_camera/infra2/image_rect_raw/theora/parameter_updates
/Pionner/cmd_vel
/Pionner/odom
/Pionner/parameter_descriptions
/Pionner/parameter_updates
/base_pose_ground_truth
/clock
/gazebo/link_states
/gazebo/model_states
/gazebo/parameter_descriptions
/gazebo/parameter_updates
/gazebo/performance_metrics
/gazebo/set_link_state
/gazebo/set_model_state
/joint_states
/rosout
/rosout_agg
/tf
/tf_static

Then I launch CATKIN_WS=~/catkin_kimera DATA_PATH=~/catkin_kimera/src/datasets LOG_DIR=~/catkin_kimera/kimera_multi/logs tmuxp load ~/catkin_kimera/src/kimera_multi/examples/1014-example.yaml.

Rostopic list now:

/Pionner/D435i_camera/color/camera_info
/Pionner/D435i_camera/color/image_raw
/Pionner/D435i_camera/color/image_raw/compressed
/Pionner/D435i_camera/color/image_raw/compressed/parameter_descriptions
/Pionner/D435i_camera/color/image_raw/compressed/parameter_updates
/Pionner/D435i_camera/color/image_raw/compressedDepth
/Pionner/D435i_camera/color/image_raw/compressedDepth/parameter_descriptions
/Pionner/D435i_camera/color/image_raw/compressedDepth/parameter_updates
/Pionner/D435i_camera/color/image_raw/theora
/Pionner/D435i_camera/color/image_raw/theora/parameter_descriptions
/Pionner/D435i_camera/color/image_raw/theora/parameter_updates
/Pionner/D435i_camera/depth/camera_info
/Pionner/D435i_camera/depth/image_rect_raw
/Pionner/D435i_camera/depth/image_rect_raw/compressed
/Pionner/D435i_camera/depth/image_rect_raw/compressed/parameter_descriptions
/Pionner/D435i_camera/depth/image_rect_raw/compressed/parameter_updates
/Pionner/D435i_camera/depth/image_rect_raw/compressedDepth
/Pionner/D435i_camera/depth/image_rect_raw/compressedDepth/parameter_descriptions
/Pionner/D435i_camera/depth/image_rect_raw/compressedDepth/parameter_updates
/Pionner/D435i_camera/depth/image_rect_raw/theora
/Pionner/D435i_camera/depth/image_rect_raw/theora/parameter_descriptions
/Pionner/D435i_camera/depth/image_rect_raw/theora/parameter_updates
/Pionner/D435i_camera/imu/sample
/Pionner/D435i_camera/infra1/camera_info
/Pionner/D435i_camera/infra1/image_rect_raw
/Pionner/D435i_camera/infra1/image_rect_raw/compressed
/Pionner/D435i_camera/infra1/image_rect_raw/compressed/parameter_descriptions
/Pionner/D435i_camera/infra1/image_rect_raw/compressed/parameter_updates
/Pionner/D435i_camera/infra1/image_rect_raw/compressedDepth
/Pionner/D435i_camera/infra1/image_rect_raw/compressedDepth/parameter_descriptions
/Pionner/D435i_camera/infra1/image_rect_raw/compressedDepth/parameter_updates
/Pionner/D435i_camera/infra1/image_rect_raw/theora
/Pionner/D435i_camera/infra1/image_rect_raw/theora/parameter_descriptions
/Pionner/D435i_camera/infra1/image_rect_raw/theora/parameter_updates
/Pionner/D435i_camera/infra2/camera_info
/Pionner/D435i_camera/infra2/image_rect_raw
/Pionner/D435i_camera/infra2/image_rect_raw/compressed
/Pionner/D435i_camera/infra2/image_rect_raw/compressed/parameter_descriptions
/Pionner/D435i_camera/infra2/image_rect_raw/compressed/parameter_updates
/Pionner/D435i_camera/infra2/image_rect_raw/compressedDepth
/Pionner/D435i_camera/infra2/image_rect_raw/compressedDepth/parameter_descriptions
/Pionner/D435i_camera/infra2/image_rect_raw/compressedDepth/parameter_updates
/Pionner/D435i_camera/infra2/image_rect_raw/theora
/Pionner/D435i_camera/infra2/image_rect_raw/theora/parameter_descriptions
/Pionner/D435i_camera/infra2/image_rect_raw/theora/parameter_updates
/Pionner/cmd_vel
/Pionner/dpgo_ros_node/anchor
/Pionner/dpgo_ros_node/command
/Pionner/dpgo_ros_node/lifting_matrix
/Pionner/dpgo_ros_node/loop_closures
/Pionner/dpgo_ros_node/measurement_weights
/Pionner/dpgo_ros_node/optimized_pose_graph
/Pionner/dpgo_ros_node/path
/Pionner/dpgo_ros_node/public_measurements
/Pionner/dpgo_ros_node/public_poses
/Pionner/dpgo_ros_node/status
/Pionner/dpgo_ros_node/trajectory
/Pionner/imu/sample
/Pionner/kimera_distributed/interactive_node/feedback
/Pionner/kimera_distributed/interactive_node/update
/Pionner/kimera_distributed/interactive_node/update_full
/Pionner/kimera_distributed/pose_graph_incremental
/Pionner/kimera_distributed/posegraph_viewer/graph_nodes
/Pionner/kimera_distributed/posegraph_viewer/graph_nodes_ids
/Pionner/kimera_distributed/posegraph_viewer/loop_edges
/Pionner/kimera_distributed/posegraph_viewer/odometry_edges
/Pionner/kimera_distributed/posegraph_viewer/rejected_loop_edges
/Pionner/kimera_distributed/posegraph_viewer/rejected_loop_edges_array
/Pionner/kimera_vio_ros/bow_query
/Pionner/kimera_vio_ros/frontend_stats
/Pionner/kimera_vio_ros/gt_odom
/Pionner/kimera_vio_ros/imu_bias
/Pionner/kimera_vio_ros/mesh
/Pionner/kimera_vio_ros/odometry
/Pionner/kimera_vio_ros/optimized_odometry
/Pionner/kimera_vio_ros/optimized_trajectory
/Pionner/kimera_vio_ros/pose_graph
/Pionner/kimera_vio_ros/pose_graph_incremental
/Pionner/kimera_vio_ros/reinit_flag
/Pionner/kimera_vio_ros/reinit_pose
/Pionner/kimera_vio_ros/resiliency
/Pionner/kimera_vio_ros/time_horizon_pointcloud
/Pionner/kimera_vio_ros/vlc_frames
/Pionner/left_img_decompress/compressed/parameter_descriptions
/Pionner/left_img_decompress/compressed/parameter_updates
/Pionner/odom
/Pionner/parameter_descriptions
/Pionner/parameter_updates
/Pionner/right_img_decompress/compressed/parameter_descriptions
/Pionner/right_img_decompress/compressed/parameter_updates
/acl_jackal/connected_peer_ids
/acl_jackal/dpgo_ros_node/anchor
/acl_jackal/dpgo_ros_node/command
/acl_jackal/dpgo_ros_node/lifting_matrix
/acl_jackal/dpgo_ros_node/path
/acl_jackal/dpgo_ros_node/public_measurements
/acl_jackal/dpgo_ros_node/public_poses
/acl_jackal/dpgo_ros_node/status
/acl_jackal/kimera_distributed/bow_requests
/acl_jackal/kimera_distributed/loop_ack
/acl_jackal/kimera_distributed/loop_closures
/acl_jackal/kimera_distributed/pose_graph_incremental
/acl_jackal/kimera_distributed/vlc_requests
/acl_jackal/kimera_distributed/vlc_responses
/acl_jackal/kimera_pgmo/optimized_mesh
/acl_jackal/kimera_vio_ros/bow_query
/acl_jackal/kimera_vio_ros/pose_graph_incremental
/acl_jackal/kimera_vio_ros/vlc_frames
/acl_jackal2/dpgo_ros_node/path
/acl_jackal2/kimera_pgmo/optimized_mesh
/apis/kimera_pgmo/optimized_mesh
/apis/kimera_vio_ros/optimized_trajectory
/base_pose_ground_truth
/clicked_point
/clock
/gazebo/link_states
/gazebo/model_states
/gazebo/parameter_descriptions
/gazebo/parameter_updates
/gazebo/performance_metrics
/gazebo/set_link_state
/gazebo/set_model_state
/hathor/dpgo_ros_node/path
/hathor/kimera_pgmo/optimized_mesh
/initialpose
/joint_states
/move_base_simple/goal
/rosout
/rosout_agg
/rviz_1710859123223662282/compressed/parameter_descriptions
/rviz_1710859123223662282/compressed/parameter_updates
/sobek/dpgo_ros_node/path
/sobek/kimera_pgmo/optimized_mesh
/sparkal1/dpgo_ros_node/path
/sparkal1/kimera_pgmo/optimized_mesh
/sparkal2/dpgo_ros_node/path
/sparkal2/kimera_pgmo/optimized_mesh
/tf
/tf_static
/thoth/dpgo_ros_node/path
/thoth/kimera_pgmo/optimized_mesh

I removed mit_rosbag.launch from the 1014-example.yaml file and made some small changes as shown below:

session_name: kimera-distributed-example 

environment:
  ROBOT0: "Pionner"
  RATE: "1.0"
  NUM_ROBOTS: "1"
  BOW_SKIP_NUM: "3"

options:
  default-command: /bin/bash

windows:
- window_name: frontend
  layout: tiled  
  shell_command_before:
    - source $CATKIN_WS/devel/setup.bash
    - mkdir -p $LOG_DIR/$ROBOT0/distributed
    - sleep 5;

  panes:
    - roslaunch kimera_distributed kimera_distributed.launch robot_id:=0 robot_name:=$ROBOT0 num_robots:=$NUM_ROBOTS dataset_name:=Jackal log_output_path:=$LOG_DIR/$ROBOT0/distributed

- window_name: dpgo
  layout: tiled
  shell_command_before:
    - source $CATKIN_WS/devel/setup.bash
    - sleep 5;

  panes:
    - roslaunch kimera_distributed dpgo.launch robot_id:=0 robot_name:=$ROBOT0 num_robots:=$NUM_ROBOTS dataset_name:=Jackal multi_master:=false log_output_path:=$LOG_DIR/$ROBOT0/distributed

- window_name: vio
  layout: tiled  
  shell_command_before:
    - source $CATKIN_WS/devel/setup.bash
    - sleep 5;

  panes:
    - roslaunch kimera_multi kimera_vio_jackal.launch robot_name:=$ROBOT0 robot_id:=0 use_d455:=true multirobot:=true lcd_no_optimize:=true use_external_odom:=true replay:=true should_use_sim_time:=true

- window_name: data
  focus: true  
  layout: tiled  
  shell_command_before:
    - source $CATKIN_WS/devel/setup.bash

  panes:
    - sleep 15; rviz -d $(rospack find kimera_distributed)/rviz/single_machine.rviz
    - rosnode kill -a \

In kimera_vio_jackal.launch I've put in the following and the is also changed? I've changed it but I'm not sure it's right...

<arg name="right_cam_topic"         default="/$(arg robot_name)/D435i_camera/infra2/image_rect_raw"/>
<arg name="imu_topic"               default="/$(arg robot_name)/imu/sample"/>
<arg name="external_odom_topic"     default="/$(arg robot_name)/odom"/>

Another question, in gazebo simulation doesn't it make sense to use: arg name="dataset_name" default="Euroc" ?

[yunzc]

Hi! Seems like you are on the right track! Yeah my recommendation is to create a new launch file (kimera_vio_gazebo.launch) and a new dataset_name, which comes with a set of its own configurations.

[LauGasp]

Hello, thanks @yunzc!

I'm having trouble completing this task. I don't know what specific parameter settings of such my datasets I should make, not even params for the RealSense camera (d435i). I'm just using it anyway with the d455 params you provided...

I have replaced the camera, odom and imu topics in kimera_vio_gazebo.launch.
First, if the robot is stationary in the Gazebo, the trajectory is reproduced (wrong random paths), which makes no sense. If I force it to move with the keyboard, the behavior is the same.

Another problem is the delay between image0 and image1 of the capture.
Do you have any idea what this might be?

Do I have to change any frames? For example, these from kimera_vio_jackal:

<arg name="map_frame_id"      value="$(arg robot_name)/map"/>
  <arg name="base_link_frame_id"  value="$(arg robot_name)/realsense_base"/>
  <arg name="odom_frame_id"        value="$(arg robot_name)/odom"/>
  <arg name="left_cam_frame_id"   value="$(arg robot_name)/forward_infra1_optical_frame"/>
  <arg name="right_cam_frame_id"  value="$(arg robot_name)/forward_infra2_optical_frame"/>
 
 <!-- Launch actual pipeline -->
  <group ns="$(arg robot_name)" >
    <include file="$(find kimera_vio_ros)/launch/kimera_vio_ros.launch"  pass_all_args="true"/>

    <!-- static transform for realsense to base frame -->
    <node pkg="tf2_ros" type="static_transform_publisher" name="base_to_realsense" args="0 0 -0.2 0 0 0 $(arg robot_name)/realsense_base $(arg robot_name)/base" />
    <!-- for arl jackals -->
    <!-- <node pkg="tf2_ros" type="static_transform_publisher" name="base_to_realsense" args="0 0.5 -0.2 1.59 -1.59 0 $(arg robot_name)/realsense_base $(arg robot_name)/base" /> -->

I have another question (not about gazebo simulation):
I also wanted to visualize the rosbag thoth mesh. When browsing through the kimera_semantics postings, I don't know what exactly I should modify and run. I'm feeling quite lost.

[yunzc]

Hi @LauGasp:

1. So most likely the d435i won't work with the d455 camera params that I have, as the VIO depends heavily on having a good calibration. Your screenshot and the fact that most of the feature tracks are red (rejected) seems to indicate this as well. I would check your camera parameters (such as your intrinsics and the extrinsic relative to the IMU. Realsense should provide at least some of these info, otherwise you will have to perform some calibration to obtain these values. If these parameters are not correct, the trajectory would not make sense.
2. The static transforms that we have in the example (i.e. base_to_realsense) is unfortunately specific to our setup and our datasets, so you will have to update those accordingly.
3. Primarily, kimera-semantics take in transforms and a point cloud. I would suggest starting with that and this launch file as an example.