Problem in getting it work

[CocoMCC]

Hi, thanks for the great and helpful work! I converted INSANE dataset into rosbag, and tried to fuse 6 DoF VIO with IMU data using mars_ros. However, the orientation of the fused pose was diverged (it kept rotating very fast). The result is the same for other datasets I've tested. But if I set "vision1_att_meas_noise" to very large values like [10.0, 10.0, 10.0], the fused pose's orientation seems getting stable. I don't think this is right. I was wondering whether you could provide some minimal examples of mars_ros for VIO/IMU/GNSS fusion using public datasets like Euroc or INSANE. Then it would be really helpful for us to try mars_ros on more scenarios.

Looking forward to your reply. Thanks.

[Chris-Bee]

@CocoMCC Thank you for the comment. This scenario is covered as an example in the mars_lib. That example was specifically made for running sensor setups from the insane dataset here.

Can you tell me a few more things about the setup:

• Which launch file are you using?
• Which IMU are you using?
• Are you converting the insane cab data to bag with the given script?
• What pose are you using, are you using images and generate a pose your self?

The attitude noise is expected in radiants, setting it to 10 is very unrealistic. I assume it's related to the input data or a calibration which is not in the beacon of convergence.

[CocoMCC]

@Chris-Bee Hi Chris, thanks for your prompt reply. Below are some details of my test:

• Which launch file are you using?

mars_gps_vision.launch

• Which IMU are you using?

For the INSANE Transition 3 dataset, I set the following:

imu_in_topic:=/mavros/imu/data_raw gps1_pos_in:=/mavros/global_position/raw/fix vision1_in_topic:=/realsense/odom/sample

• Are you converting the insane cab data to bag with the given script?

Yes

• What pose are you using, are you using images and generate a pose your self?

/realsense/odom/sample I tried generate pose myself for other datasets, but the divergence is the same.

Part of my config file was like:

# Core state covariance p,v,q,bw,ba
core_init_cov_p: [0.5, 0.5, 0.5]
core_init_cov_v: [0.3, 0.3, 0.3]
core_init_cov_q: [0.1218, 0.1218, 0.1218] # 20degree
core_init_cov_bw: [0.0076, 0.0076, 0.0076]
core_init_cov_ba: [0.01, 0.01, 0.01]

# Pose1 Settings
vision1_fixed_scale: true
vision1_pos_meas_noise: [0.01, 0.01, 0.01]
vision1_att_meas_noise: [0.01, 0.01, 0.01]
vision1_cal_p_ip: [-0.239, -0.05, -0.019]
vision1_cal_q_ip: [0.0106393146811171, 0.0109080578822580, -0.999880660312949, 0.00254644624828031] #w,x,y,z
vision1_cal_ip_init_cov: [0.00000000001, 0.00000000001, 0.00000000001, 0.00000000001, 0.00000000001, 0.00000000001] #position[3], rot[3]

# GPS Settings
gps1_pos_meas_noise: [1.0, 1.0, 1.0]
gps1_vel_meas_noise: [2, 2, 2]
gps1_cal_ig: [0, 0, 0.21]
gps1_state_init_cov: [0.001, 0.001, 0.001]

It would be really helpful for the community if you could generate an ROS example to run INSANE or other public datasets. Really hope to make MARS work. Thanks anyway :)

[CocoMCC]

Hi @Chris-Bee , wondering whether you could help on this? or will you plan to provide an ROS example for any public dataset fusing VIO/IMU/GNSS? Thanks.

[Chris-Bee]

@CocoMCC Regarding the issue that you are observing: I see that you are useing an almost identity quaternion for the RealSense orientation calibration. This makes sense because this is the calibration of the stereo camera which is given by the INSANE dataset. However, I remember that the pose calibration of the RealSense T265 is different from the calibration of the cameras. This information is not present in the INSANE data set and I will check and add this.

That being said, since your initial covariance for the RealSense calibration is very small (1e-10), the framework can't converge to the correct calibration. This explains the behavior of the filter.

Regarding the examples: Can you elaborate a bit further on what you mean with 'examples for public datasets'? MaRS is intended to be generic in the sence that a combination of sensors can be used with in a node. Should a specific configuration not exist, then this can be added with a few lines. In your case, this node/launchfile should cover all VIO/IMU/GNSS configurations for any data set - modulo their specific sensor calibrations.

Because of that, I assume you are asking for dedicated configuration files for common public data sets?

[Chris-Bee]

@CocoMCC according to the RealSense T265 Documentation the pose information is rotated +180 Degree in the Y-Axis w.r.t. the IMU of the RealSense.

Given the calibration of the RealSense IMU w.r.t. the PX4 IMU from the INSANE calibration data here

T_pximu_rsimu = [[-0.0122    0.5677    0.8231    0.0422],
                 [ 0.9994    0.0335   -0.0083    0.0319],
                 [-0.0323    0.8226   -0.5678   -0.0509],
                 [      0         0         0    1.0000]]

and the Realsense Pose calibration w.r.t. to its IMU:

T_rsimu_rspose = [[ -1    0    0    0],
                  [  0    1    0    0],
                  [ 0    0   -1    0],
                  [ 0    0    0    1]]

I'm getting the following RealSense Pose calibration w.r.t. the PX4 IMU:

T_pximu_rsimu * T_rsimu_rspose =   
     [[  0.0122    0.5677   -0.8231    0.0422]
      [ -0.9994    0.0335    0.0083    0.0319]
      [  0.0323    0.8226    0.5678   -0.0509]
      [      0         0         0     1.0000]]

Please try the following quaternion for the RealSense calibration and let me know if the issue persists:

% Quaternion (w,x,y,z)
vision1_cal_q_ip = [0.6351    0.3205   -0.3367   -0.6169]

[CocoMCC]

Hi @Chris-Bee thanks so much for your help. I've tried with the new "vision1_cal_q_ip" you provided, but the same divergence behavior keeps.

---

The complete config file I used to run INSANE is:

# NOTE: Do not use 1e-6 format because this is reqognized as a string

# Framework Settings
pub_on_prop: true
use_ros_time_now: false
gps_ros_time_now: false

verbose: false
verbose_out_of_order: true
discard_ooo_prop_meas: true
pub_cov: false
pub_path: true
publish_gps_enu: true
buffer_size: 2000

# Ros Settings
use_tcpnodelay: true

pub_cb_buffer_size: 1
sub_imu_cb_buffer_size: 200
sub_sensor_cb_buffer_size: 1

# Sensor Settings
# gyro_rate_noise: 0.0023089
# gyro_bias_noise: 0.0012655
# acc_noise: 0.038932
# acc_bias_noise: 0.057518
gyro_rate_noise: 0.01
gyro_bias_noise: 0.0001
acc_noise: 0.1
acc_bias_noise: 0.001

# Core state covariance p,v,q,bw,ba
core_init_cov_p: [0.5, 0.5, 0.5]
core_init_cov_v: [0.3, 0.3, 0.3]
core_init_cov_q: [0.1218, 0.1218, 0.1218] # 20degree
core_init_cov_bw: [0.0076, 0.0076, 0.0076]
core_init_cov_ba: [0.01, 0.01, 0.01]

# Pose1 Settings
vision1_fixed_scale: true
vision1_pos_meas_noise: [0.01, 0.01, 0.01]
vision1_att_meas_noise: [0.01, 0.01, 0.01]
vision1_cal_p_ip: [-0.239, -0.05, -0.019]
vision1_cal_q_ip: [0.6351 , 0.3205, -0.3367, -0.6169] #w,x,y,z
vision1_cal_ip_init_cov: [0.00000000001, 0.00000000001, 0.00000000001, 0.00000000001, 0.00000000001, 0.00000000001] #position[3], rot[3]

# GPS Settings
gps1_pos_meas_noise: [1.0, 1.0, 1.0]
gps1_vel_meas_noise: [2, 2, 2]
gps1_cal_ig: [0, 0, 0.21]
gps1_state_init_cov: [0.001, 0.001, 0.001]

# Baro Settings
pressure1_meas_noise: [1.0]
pressure1_cal_ip: [0, 0, 0]
pressure1_state_init_cov: [0.00001, 0.00001, 0.00001]

# Magnetometer Settings
mag1_normalize: true
mag1_declination: 0.0 #degree
mag1_meas_noise: [0.5, 0.5, 0.5]
mag1_cal_q_im: [1, 0, 0, 0] # w,x,y,z
mag1_state_init_cov: [0.25, 0.25, 0.25, 0.0305, 0.0305, 0.0305] #v_wm q_im

---

The launch file and command I used is :

roslaunch mars_ros mars_gps_vision.launch \
  use_sim_time:=true rviz:=true config_file_name:=gps_vision_config_insane \
  imu_in_topic:=/mavros/imu/data_raw \
  gps1_pos_in:=/mavros/global_position/raw/fix \
  vision1_in_topic:=/realsense/odom/sample \
  mag1_in:=/mavros/imu/mag \
  pressure1_in:=/mavros/imu/static_pressure

---

If you could provide a dedicated config file to run mars_ros on INSANE dataset fusing VIO/GNSS/IMU/Mag etc., that would be of great help. Looking forward to your reply.

Thanks.

[CocoMCC]

BTW, a dedicated configuration to run mars_ros's multi-sensor fusion on any public dataset like Euroc/KITTI would be helpful to. If we fail to get mars_ros work properly on our own dataset, we can look at those configurations to find out reasons.

[Chris-Bee]

Makes sense, appologies for the delay! I will be able to look at this next week. For the time being and to get things running on your side, please try the following values:

vision1_fixed_scale: true
vision1_pos_meas_noise: [0.01, 0.01, 0.01]
vision1_att_meas_noise: [0.01, 0.01, 0.01]
vision1_cal_p_ip: [0.0422, 0.0319, -0.0509]
vision1_cal_q_ip: [0.6351 , 0.3205, -0.3367, -0.6169] #w,x,y,z
vision1_cal_ip_init_cov: [0.0025, 0.0025, 0.0025, 0.0305, 0.0305, 0.0305]

The position calibration was slightly incorrect compared to the transformation I mentioned above. However, more importantly, I set the initial ucertainty of the position calibration to 5cm and 10 Degrees (STD). The previous values did not allow the sensor calibration to converge to any other values as the ones used for initialization. I need to assume that the pose calibration of the RealSense, or much more, the reference frame it generates is slightly incorrect.

You can also use one of the indoor datasets that provide you with 6DoF motion capture data - this should ensure that your setup works in general.

I can investigate this next week to be sure.

[Chris-Bee]

@CocoMCC could you please send me a .bag or rqt plots of your results (core-states and calibration states) via email. That makes it easier to analyse the problem you are observing.

[CocoMCC]

@Chris-Bee Hi Chris, below is the google drive link, which is an rosbag I created for one of the INSANE datasets. But I cannot make MARS work properly (GNSS+IMU+Vision pose by Realsense) on this data. Could you please have a look at it? Thanks.

https://drive.google.com/file/d/1jgcAGF9SFIbEXly50a9XJTZ_LOlFEnZ8/view?usp=sharing

[mascheiber]

Hi @CocoMCC,

Since I did part of the last modifications on the wrapper you are using, I did take a look on your issue.

Given the bagfile, commands, and config that you provided (thanks for sharing) I noticed two things:

The launch file and command I used is :

roslaunch mars_ros mars_gps_vision.launch \
  use_sim_time:=true rviz:=true config_file_name:=gps_vision_config_insane \
  imu_in_topic:=/mavros/imu/data_raw \
  gps1_pos_in:=/mavros/global_position/raw/fix \
  vision1_in_topic:=/realsense/odom/sample \
  mag1_in:=/mavros/imu/mag \
  pressure1_in:=/mavros/imu/static_pressure

1/ This launch command has small mistake as the config_file_name should be config_file instead. Also, on my side I needed to provide the full path to the file. Thus the correct way of launching would be

roslaunch mars_ros mars_gps_vision.launch \
  use_sim_time:=true rviz:=true config_file:=$(pwd)/gps_vision_config_insane.yaml \
  imu_in_topic:=/mavros/imu/data_raw \
  gps1_pos_in:=/mavros/global_position/raw/fix \
  vision1_in_topic:=/realsense/odom/sample \
  mag1_in:=/mavros/imu/mag \
  pressure1_in:=/mavros/imu/static_pressure

You should be able to verify the correct loading of the launch file, if the vision_sensor calibration is equivalent to the one provided, which by running your posted command was not the case for me.

2/ MaRS ROS and the MaRS Library both had updates since you first posted this issue. Thus can you please confirm the version (with commit ID) for both repositories?

Further, using the above launch command and below config I was able to run the GPS-Vision Wrapper using the latest main commit (6f7d010).

However, when looking at the pose output of MaRS (image above) I cannot observe the behavior you are describing earlier:

However, the orientation of the fused pose was diverged (it kept rotating very fast)

Thus could you please verify if this issue is still there with the above launch command, and if the issue persists, provide more detailed output (such as an Graph or bagfile containing the MaRS output)?

---

Configuration

```yaml # NOTE: Do not use 1e-6 format because this is reqognized as a string # Framework Settings pub_on_prop: true use_ros_time_now: false gps_ros_time_now: false verbose: false verbose_out_of_order: true discard_ooo_prop_meas: true pub_cov: false pub_path: true publish_gps_enu: true buffer_size: 2000 # Ros Settings use_tcpnodelay: true pub_cb_buffer_size: 1 sub_imu_cb_buffer_size: 200 sub_sensor_cb_buffer_size: 1 # Sensor Settings # gyro_rate_noise: 0.0023089 # gyro_bias_noise: 0.0012655 # acc_noise: 0.038932 # acc_bias_noise: 0.057518 gyro_rate_noise: 0.01 gyro_bias_noise: 0.0001 acc_noise: 0.1 acc_bias_noise: 0.001 # Core state covariance p,v,q,bw,ba core_init_cov_p: [0.5, 0.5, 0.5] core_init_cov_v: [0.3, 0.3, 0.3] core_init_cov_q: [0.1218, 0.1218, 0.1218] # 20degree core_init_cov_bw: [0.0076, 0.0076, 0.0076] core_init_cov_ba: [0.01, 0.01, 0.01] # Pose1 Settings vision1_fixed_scale: true vision1_pos_meas_noise: [0.01, 0.01, 0.01] vision1_att_meas_noise: [0.01, 0.01, 0.01] vision1_cal_p_ip: [0.0422, 0.0319, -0.0509] vision1_cal_q_ip: [0.6351 , 0.3205, -0.3367, -0.6169] #w,x,y,z vision1_cal_ip_init_cov: [0.0025, 0.0025, 0.0025, 0.0305, 0.0305, 0.0305] #position[3], rot[3] # GPS Settings gps1_pos_meas_noise: [1.0, 1.0, 1.0] gps1_vel_meas_noise: [2, 2, 2] gps1_cal_ig: [0, 0, 0.21] gps1_state_init_cov: [0.001, 0.001, 0.001] # Baro Settings pressure1_meas_noise: [1.0] pressure1_cal_ip: [0, 0, 0] pressure1_state_init_cov: [0.00001, 0.00001, 0.00001] # Magnetometer Settings mag1_normalize: true mag1_declination: 0.0 #degree mag1_meas_noise: [0.5, 0.5, 0.5] mag1_cal_q_im: [1, 0, 0, 0] # w,x,y,z mag1_state_init_cov: [0.25, 0.25, 0.25, 0.0305, 0.0305, 0.0305] #v_wm q_im ```

[brick-ai]

Hi @CocoMCC , the mars_lib use ENU as world frame of core states.

What is the world frame of your poses? ENU or NED?

If it is NED, there will be some problems to fuse imu and your poses, caused by different world frames.

The easiest way to make it work is to change the gravity vector to [0, 0, -9.8] in https://github.com/aau-cns/mars_lib/blob/18e9a924733efbb97de76328e1713b13153e9634/source/mars/include/mars/core_state.h#L40

But there still will be some problems if you fuse different sensors with different world frames.

[Chris-Bee]

@brick-ai Thank you for this remark, @mascheiber addressed this within the issue you posted in the MaRS library https://github.com/aau-cns/mars_lib/issues/10 . @CocoMCC I hope the tips from @mascheiber solved your problem. I'll close this issue but we can re-open it, should the problem with your configuration persist.