D435i + mesh is strange

[LauGasp]

Description:
Hi, I'm trying to use Kimera-Multi, but I'm facing some difficulties, so I decided to first test the various Kimera modules. In Kimera-VIO-ROS, I am using the launch you provided (https://github.com/MIT-SPARK/Kimera-VIO-ROS/blob/master/launch/kimera_vio_ros_realsense_IR.launch) with a Realsense D435i camera. The topics are being published, the odometry with the real D345i camera seems to be estimated correctly, but the mesh seems strange and I'm getting some errors.

What I'm going to show below is a first error with the real camera, but it also exists in my simulation. I have other problems when I try to simulate in a gazebo, the odometry is totally out of order (problem reported at: IntelRealSense/librealsense#12867).

Can you help me?

Command:

$ roscore
$ roslaunch realsense2_camera rs_d435_camera_with_model.launch 
$ roslaunch kimera_vio_ros kimera_vio_ros_realsense_IR.launch
$ rviz -d $(rospack find kimera_vio_ros)/rviz/kimera_vio_euroc.rviz

Console output:

I0422 16:11:02.457796 27593 KimeraVioRos.cpp:215] Statistics
-----------                                  #	Log Hz	{avg     +- std    }	[min,max]
Display [ms]                            	  687	0.751226	{0.0553130 +- 0.00000}	[0,33]
Mesher [ms]                             	  344	0.376160	{9.59302 +- 4.52597}	[2,38]
Stereo Data Provider [ms]               	    0	
VioBackend [ms]                         	  344	0.376165	{7.74419 +- 2.99589}	[0,20]
VioFrontend Frame Rate [ms]             	27137	29.6031	{2.62472 +- 0.586033}	[1,16]
VioFrontend Keyframe Rate [ms]          	  343	0.375187	{33.0583 +- 6.45371}	[24,55]
VioFrontend [ms]                        	27481	29.9693	{3.02504 +- 3.58724}	[1,55]
Visualizer [ms]                         	  344	0.376160	{0.0755814 +- 1.13458}	[0,11]
backend_input_queue Size [#]            	  344	0.376152	{1.00000 +- 0.00000}	[1,1]
data_provider_left_frame_queue Size [#] 	27486	29.9738	{1.00000 +- 0.00000}	[1,1]
data_provider_right_frame_queue Size [#]	27486	29.9738	{1.00004 +- 0.00000}	[1,2]
display_input_queue Size [#]            	  687	0.751195	{1.00000 +- 0.00000}	[1,1]
frontend_input_queue Size [#]           	27482	29.9683	{1.00011 +- 0.00000}	[1,2]
mesher_backend Size [#]                 	  344	0.376165	{1.00000 +- 0.00000}	[1,1]
mesher_frontend Size [#]                	27481	29.9693	{143.193 +- 19.2324}	[1,302]
visualizer_backend_queue Size [#]       	  344	0.376165	{1.00000 +- 0.00000}	[1,1]
visualizer_frontend_queue Size [#]      	27481	29.9693	{143.362 +- 19.2324}	[1,302]
visualizer_mesher_queue Size [#]        	  344	0.376160	{1.00000 +- 0.00000}	[1,1]
W0422 16:11:02.507817 27593 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!
W0422 16:11:02.507841 27593 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!
W0422 16:11:02.557775 27593 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!
W0422 16:11:02.557806 27593 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!
W0422 16:11:02.607815 27593 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!
W0422 16:11:02.607841 27593 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!
W0422 16:11:02.657779 27593 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!
W0422 16:11:02.707747 27593 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!
W0422 16:11:02.707778 27593 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!
W0422 16:11:02.757757 27593 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!
W0422 16:11:02.757797 27593 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!
W0422 16:11:02.807785 27593 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!
W0422 16:11:02.807890 27593 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!
W0422 16:11:02.857758 27593 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!
W0422 16:11:02.857800 27593 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!
W0422 16:11:02.907778 27593 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!
W0422 16:11:02.907851 27593 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!
W0422 16:11:02.957744 27593 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!
W0422 16:11:03.007741 27593 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!
W0422 16:11:03.007762 27593 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!
W0422 16:11:03.057830 27593 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!
W0422 16:11:03.057901 27593 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!
W0422 16:11:03.107797 27593 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!
W0422 16:11:03.107816 27593 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!
W0422 16:11:03.157739 27593 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!
W0422 16:11:03.207819 27593 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!
W0422 16:11:03.207842 27593 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!
W0422 16:11:03.257776 27593 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!
W0422 16:11:03.257803 27593 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!
W0422 16:11:03.307751 27593 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!
W0422 16:11:03.307777 27593 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!
W0422 16:11:03.357774 27593 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!
W0422 16:11:03.357802 27593 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!
W0422 16:11:03.407771 27593 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!
W0422 16:11:03.407799 27593 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!
W0422 16:11:03.457759 27593 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!
W0422 16:11:03.457787 27593 Visualizer3DModule.cpp:133] Mesher queue is empty, yet Backend or Frontend queue is not!This should not happen since Mesher runs at Backend pace!

Files:

Note: I'm using kimera-vio-ros and kimera-vio from the kimera-multi repository. The installation was done from the following link: https://github.com/MIT-SPARK/Kimera-Multi.

Please give also the following information:

• OpenCV version used: 4.2.0
• Operating system and version: Ubuntu 20.04
• Did you change the source code? no

[yangzhongii]

Hi, I get the similar error, can you show me your kimera_vio_ros_realsense_IR.launch, I think it is related to imu parameters. Thinks

[LauGasp]

Hello @yangzhongii, my kimera_vio_ros_realsense_IR.launch is the one provided. This one:

<launch>
  <arg name="dataset_name" value="RealSenseIR"/>

  <!-- allow for different sensor calibrations -->
  <arg name="sensor_config_path" default="$(find kimera_vio)/params"/>
  <arg name="sensor_config_name" default="$(arg dataset_name)"/>
  <arg name="sensor_folder" value="$(arg sensor_config_path)/$(arg sensor_config_name)"/>

  <arg name="online" default="true" />
  <arg name="do_coarse_temporal_sync" default="false"/>
  <arg name="do_fine_temporal_sync" default="false"/>

  <arg name="log_output" default="false"/>
  <arg name="log_output_path"
       default="$(find kimera_vio_ros)/output_logs/"
       if="$(arg log_output)"/>
  <arg name="log_gt_data" default="false"/>
  <arg name="gt_topic" default=""/>
  <!-- Set use_sim_time to true if you use rosbag with clock argument -->
  <arg name="should_use_sim_time" default="false"/>

  <!-- Only used when parsing a rosbag -->
  <arg name="rosbag_path" default="default.bag"
       unless="$(arg online)"/>

  <!-- Frame IDs. These DO NOT match frame id's on the video streams, as the
	RealSense and Kimera publish conflicting Tf's -->
  <arg name="odom_frame_id"      value="odom"/>
  <arg name="base_link_frame_id" value="base_link"/>
  <arg name="map_frame_id"       value="map"/>
  <arg name="left_cam_frame_id"  value="left_cam"/>
  <arg name="right_cam_frame_id" value="right_cam"/>

  <!-- Subscribed Topics -->
  <arg name="left_cam_topic"  value="/camera/infra1/image_rect_raw"/>
  <arg name="right_cam_topic" value="/camera/infra2/image_rect_raw"/>
  <arg name="imu_topic"       value="/camera/imu"/>

  <!-- Launch actual pipeline -->
  <include file="$(find kimera_vio_ros)/launch/kimera_vio_ros.launch"
           pass_all_args="true"/>

 <!-- Launch static TF node from base_link to velo_link -->
 <node pkg="tf" type="static_transform_publisher" name="velo_link_broadcaster"
   args="0 0 0 0 0 0 1 base_link velo_link 100"/>

</launch>

and the imu parameters:

%YAML:1.0
# Type of IMU preintegration:
# 0: CombinedImuFactor
# 1: ImuFactor
imu_preintegration_type: 1

# Collected by Andrew Violette on 02/19/20
# Sensor extrinsics wrt. the body-frame.
T_BS:
  cols: 4
  rows: 4
  data: [1.0, 0.0, 0.0, 0.0,
         0.0, 1.0, 0.0, 0.0,
         0.0, 0.0, 1.0, 0.0,
         0.0, 0.0, 0.0, 1.0]
rate_hz: 400

# inertial sensor noise model parameters (static)
imu_bias_init_sigma: 1e-3
gyroscope_noise_density: 0.000488     # [ rad / s / sqrt(Hz) ]   ( gyro "white noise" )
gyroscope_random_walk: 4.88e-05       # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion )
accelerometer_noise_density: 0.00147  # [ m / s^2 / sqrt(Hz) ]   ( accel "white noise" )
accelerometer_random_walk: 0.00147    # [ m / s^3 / sqrt(Hz) ].  ( accel bias diffusion )

# Extra parameters
do_imu_rate_time_alignment: 1
time_alignment_window_size_s: 10.0
time_alignment_variance_threshold_scaling: 30.0
imu_integration_sigma: 1.0e-8
imu_time_shift: 0.0
n_gravity: [0.0, 0.0, -9.81]

[ustccxy]

您好，我的 kimera_vio_ros_realsense_IR.launch 就是提供的。这个：

<launch>
  <arg name="dataset_name" value="RealSenseIR"/>

  <!-- allow for different sensor calibrations -->
  <arg name="sensor_config_path" default="$(find kimera_vio)/params"/>
  <arg name="sensor_config_name" default="$(arg dataset_name)"/>
  <arg name="sensor_folder" value="$(arg sensor_config_path)/$(arg sensor_config_name)"/>

  <arg name="online" default="true" />
  <arg name="do_coarse_temporal_sync" default="false"/>
  <arg name="do_fine_temporal_sync" default="false"/>

  <arg name="log_output" default="false"/>
  <arg name="log_output_path"
       default="$(find kimera_vio_ros)/output_logs/"
       if="$(arg log_output)"/>
  <arg name="log_gt_data" default="false"/>
  <arg name="gt_topic" default=""/>
  <!-- Set use_sim_time to true if you use rosbag with clock argument -->
  <arg name="should_use_sim_time" default="false"/>

  <!-- Only used when parsing a rosbag -->
  <arg name="rosbag_path" default="default.bag"
       unless="$(arg online)"/>

  <!-- Frame IDs. These DO NOT match frame id's on the video streams, as the
	RealSense and Kimera publish conflicting Tf's -->
  <arg name="odom_frame_id"      value="odom"/>
  <arg name="base_link_frame_id" value="base_link"/>
  <arg name="map_frame_id"       value="map"/>
  <arg name="left_cam_frame_id"  value="left_cam"/>
  <arg name="right_cam_frame_id" value="right_cam"/>

  <!-- Subscribed Topics -->
  <arg name="left_cam_topic"  value="/camera/infra1/image_rect_raw"/>
  <arg name="right_cam_topic" value="/camera/infra2/image_rect_raw"/>
  <arg name="imu_topic"       value="/camera/imu"/>

  <!-- Launch actual pipeline -->
  <include file="$(find kimera_vio_ros)/launch/kimera_vio_ros.launch"
           pass_all_args="true"/>

 <!-- Launch static TF node from base_link to velo_link -->
 <node pkg="tf" type="static_transform_publisher" name="velo_link_broadcaster"
   args="0 0 0 0 0 0 1 base_link velo_link 100"/>

</launch>

和 imu 参数：

%YAML:1.0
# Type of IMU preintegration:
# 0: CombinedImuFactor
# 1: ImuFactor
imu_preintegration_type: 1

# Collected by Andrew Violette on 02/19/20
# Sensor extrinsics wrt. the body-frame.
T_BS:
  cols: 4
  rows: 4
  data: [1.0, 0.0, 0.0, 0.0,
         0.0, 1.0, 0.0, 0.0,
         0.0, 0.0, 1.0, 0.0,
         0.0, 0.0, 0.0, 1.0]
rate_hz: 400

# inertial sensor noise model parameters (static)
imu_bias_init_sigma: 1e-3
gyroscope_noise_density: 0.000488     # [ rad / s / sqrt(Hz) ]   ( gyro "white noise" )
gyroscope_random_walk: 4.88e-05       # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion )
accelerometer_noise_density: 0.00147  # [ m / s^2 / sqrt(Hz) ]   ( accel "white noise" )
accelerometer_random_walk: 0.00147    # [ m / s^3 / sqrt(Hz) ].  ( accel bias diffusion )

# Extra parameters
do_imu_rate_time_alignment: 1
time_alignment_window_size_s: 10.0
time_alignment_variance_threshold_scaling: 30.0
imu_integration_sigma: 1.0e-8
imu_time_shift: 0.0
n_gravity: [0.0, 0.0, -9.81]

May I ask if you have solved this problem, I have encountered the same problem.

[ustccxy]

嗨，我遇到了类似的错误，你能告诉我你的kimera_vio_ros_realsense_IR.launch吗，我认为这与imu参数有关。认为

May I ask if you have solved this problem, I have encountered the same problem.