search

rpng / open_vins

An open source platform for visual-inertial navigation research.
https://docs.openvins.com
GNU General Public License v3.0
2.13k stars 630 forks source link

Running open_vins with gazebo and px4 #240

Closed stevedanomodolor closed 2 years ago

stevedanomodolor commented 2 years ago

``System ros2 foxy Problem: I am trying to run the open_vis to estimate VIO for a simulation I made with the iris drone with a front camera. Given that the message type is not sensor_msg::msg but px4_msgs::sensorCombined, I modified the code in the ros2visualiser, specifically the imu subscirber. This is the modification I made. 

void ROS2Visualizer::callback_inertial(const px4_msgs::msg::SensorCombined::UniquePtr msg) {

  // convert into correct format
  ov_core::ImuData message;
  // message.timestamp = msg->header.stamp.sec + msg->header.stamp.nanosec * 1e-9;
  // message.wm << msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z;
  // message.am << msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z;
  //
  message.timestamp = msg->timestamp/1000000.0;
  message.wm << msg->gyro_rad[0], msg->gyro_rad[1], msg->gyro_rad[2];
  message.am << msg->accelerometer_m_s2[0], msg->accelerometer_m_s2[1], msg->accelerometer_m_s2[2];

Eveytime i run the simulation I get this error 

1649928195.6166582 [INFO] [launch]: All log files can be found below /home/stevedan/.ros/log/2022-04-14-11-23-15-615892-stevedan-OMEN-Laptop-15-ek0xxx-209021
1649928195.6167920 [INFO] [launch]: Default logging verbosity is set to INFO
1649928195.7044764 [INFO] [run_subscribe_msckf-1]: process started with pid [209023]
1649928195.7046418 [INFO] [rviz2-2]: process started with pid [209025]
1649928195.8140919 [run_subscribe_msckf-1] initialized imu conv node
1649928195.8167100 [run_subscribe_msckf-1] overriding node verbosity with value from ROS!
1649928195.8168349 [run_subscribe_msckf-1] Setting printing level to: INFO
1649928195.8334203 [run_subscribe_msckf-1] overriding node max_cameras with value from ROS!
1649928195.8335309 [run_subscribe_msckf-1] overriding node max_cameras with value from ROS!
1649928195.8335917 [run_subscribe_msckf-1] overriding node use_stereo with value from ROS!
1649928195.8336411 [run_subscribe_msckf-1] overriding node max_cameras with value from ROS!
1649928195.8336775 [run_subscribe_msckf-1] overriding node use_stereo with value from ROS!
1649928195.8337104 [run_subscribe_msckf-1] Logging tets
1649928198.3971076 [run_subscribe_msckf-1] [TIME]: 0.0030 seconds total (337.8 hz)
1649928198.3972430 [run_subscribe_msckf-1] [TIME]: 0.0040 seconds total (249.9 hz)
1649928198.3973083 [run_subscribe_msckf-1] [TIME]: 0.0024 seconds total (414.1 hz)
1649928198.3973594 [run_subscribe_msckf-1] [TIME]: 0.0015 seconds total (678.0 hz)
1649928198.3974059 [run_subscribe_msckf-1] [TIME]: 0.0015 seconds total (684.0 hz)
1649928198.3974519 [run_subscribe_msckf-1] [TIME]: 0.0020 seconds total (494.1 hz)
1649928198.3974969 [run_subscribe_msckf-1] [TIME]: 0.0016 seconds total (613.5 hz)
1649928198.3975430 [run_subscribe_msckf-1] [TIME]: 0.0014 seconds total (730.5 hz)
1649928198.3975887 [run_subscribe_msckf-1] [TIME]: 0.0015 seconds total (662.7 hz)
1649928198.3976338 [run_subscribe_msckf-1] [TIME]: 0.0018 seconds total (559.9 hz)
1649928198.3976791 [run_subscribe_msckf-1] [TIME]: 0.0039 seconds total (259.1 hz)
1649928198.3977242 [run_subscribe_msckf-1] [TIME]: 0.0038 seconds total (265.6 hz)
1649928198.3977687 [run_subscribe_msckf-1] [TIME]: 0.0031 seconds total (318.2 hz)
1649928198.3978133 [run_subscribe_msckf-1] [TIME]: 0.0026 seconds total (389.7 hz)
1649928198.3978581 [run_subscribe_msckf-1] [TIME]: 0.0057 seconds total (176.8 hz)
1649928198.3979025 [run_subscribe_msckf-1] [TIME]: 0.0042 seconds total (240.7 hz)
1649928198.3979473 [run_subscribe_msckf-1] [TIME]: 0.0044 seconds total (229.0 hz)
1649928198.3979921 [run_subscribe_msckf-1] [TIME]: 0.0026 seconds total (381.7 hz)
1649928198.3980362 [run_subscribe_msckf-1] [TIME]: 0.0025 seconds total (401.8 hz)
1649928198.3980803 [run_subscribe_msckf-1] [TIME]: 0.0026 seconds total (385.5 hz)
1649928198.3981245 [run_subscribe_msckf-1] [TIME]: 0.0029 seconds total (340.7 hz)
1649928198.3981690 [run_subscribe_msckf-1] [TIME]: 0.0026 seconds total (384.2 hz)
1649928198.3982134 [run_subscribe_msckf-1] [TIME]: 0.0024 seconds total (421.4 hz)
1649928198.3982575 [run_subscribe_msckf-1] [TIME]: 0.0024 seconds total (412.4 hz)
1649928198.3983023 [run_subscribe_msckf-1] [TIME]: 0.0026 seconds total (382.8 hz)
1649928198.3983469 [run_subscribe_msckf-1] [TIME]: 0.0026 seconds total (383.7 hz)
1649928198.3983982 [run_subscribe_msckf-1] [TIME]: 0.0026 seconds total (388.2 hz)
1649928198.3984427 [run_subscribe_msckf-1] [TIME]: 0.0025 seconds total (399.4 hz)
1649928198.3984876 [run_subscribe_msckf-1] [TIME]: 0.0037 seconds total (272.7 hz)
1649928198.3985324 [run_subscribe_msckf-1] [TIME]: 0.0026 seconds total (388.7 hz)
1649928198.3985775 [run_subscribe_msckf-1] [init]: successful initialization in 0.0003 seconds
1649928198.3986351 [run_subscribe_msckf-1] [init]: orientation = 1.0000, 0.0000, -0.0009, 0.0057
1649928198.3986821 [run_subscribe_msckf-1] [init]: bias gyro = -0.0009, -0.0054, -0.0001
1649928198.3987265 [run_subscribe_msckf-1] [init]: velocity = 0.0000, 0.0000, 0.0000
1649928198.3987715 [run_subscribe_msckf-1] [init]: bias accel = 0.0001, -0.0006, 0.0559
1649928198.3988166 [run_subscribe_msckf-1] [init]: position = 0.0000, 0.0000, 0.0000
1649928198.3988607 [run_subscribe_msckf-1] [TIME]: 0.0023 seconds total (433.7 hz)
1649928198.3989055 [run_subscribe_msckf-1] image received out of order, unable to do anything (prop dt = -1649928076.969715)
1649928198.3989508 [run_subscribe_msckf-1] [TIME]: 0.0058 seconds total (173.6 hz)
1649928198.3989961 [run_subscribe_msckf-1] Propagator::select_imu_readings(): No IMU measurements to propagate with (0 of 2). IMU-CAMERA are likely messed up!!!
1649928198.3990421 [run_subscribe_msckf-1] [ZUPT]: There are no IMU data to check for zero velocity with!!
1649928198.3990927 [run_subscribe_msckf-1] image received out of order, unable to do anything (prop dt = -1649928076.901715)
1649928198.3991385 [run_subscribe_msckf-1] [TIME]: 0.0022 seconds total (448.0 hz)
1649928198.3991833 [run_subscribe_msckf-1] Propagator::select_imu_readings(): No IMU measurements to propagate with (0 of 2). IMU-CAMERA are likely messed up!!!
1649928198.3992372 [run_subscribe_msckf-1] [ZUPT]: There are no IMU data to check for zero velocity with!!
1649928198.3992825 [run_subscribe_msckf-1] image received out of order, unable to do anything (prop dt = -1649928076.833715)
1649928198.3993273 [run_subscribe_msckf-1] [TIME]: 0.0020 seconds total (489.2 hz)
1649928198.3993723 [run_subscribe_msckf-1] Propagator::select_imu_readings(): No IMU measurements to propagate with (0 of 2). IMU-CAMERA are likely messed up!!!
1649928198.3994172 [run_subscribe_msckf-1] [ZUPT]: There are no IMU data to check for zero velocity with!!
1649928198.3994617 [run_subscribe_msckf-1] image received out of order, unable to do anything (prop dt = -1649928076.769715)
1649928198.3995059 [run_subscribe_msckf-1] [TIME]: 0.0022 seconds total (458.5 hz)
1649928198.3995507 [run_subscribe_msckf-1] Propagator::select_imu_readings(): No IMU measurements to propagate with (0 of 2). IMU-CAMERA are likely messed up!!!
1649928198.3995957 [run_subscribe_msckf-1] [ZUPT]: There are no IMU data to check for zero velocity with!!
1649928198.3996403 [run_subscribe_msckf-1] image received out of order, unable to do anything (prop dt = -1649928076.701715)
1649928198.3996854 [run_subscribe_msckf-1] [TIME]: 0.0021 seconds total (470.8 hz)
1649928198.3997302 [run_subscribe_msckf-1] Propagator::select_imu_readings(): No IMU measurements to propagate with (0 of 2). IMU-CAMERA are likely messed up!!!
1649928198.3997750 [run_subscribe_msckf-1] [ZUPT]: There are no IMU data to check for zero velocity with!!
1649928198.3998199 [run_subscribe_msckf-1] image received out of order, unable to do anything (prop dt = -1649928076.633715)
1649928198.3998649 [run_subscribe_msckf-1] [TIME]: 0.0024 seconds total (412.7 hz)
1649928198.3999088 [run_subscribe_msckf-1] Propagator::select_imu_readings(): No IMU measurements to propagate with (0 of 2). IMU-CAMERA are likely messed up!!!
1649928198.3999529 [run_subscribe_msckf-1] [ZUPT]: There are no IMU data to check for zero velocity with!!
1649928198.3999970 [run_subscribe_msckf-1] image received out of order, unable to do anything (prop dt = -1649928076.569715)
1649928198.4000411 [run_subscribe_msckf-1] [TIME]: 0.0031 seconds total (318.5 hz)
1649928199.2134056 [run_subscribe_msckf-1] Propagator::select_imu_readings(): No IMU measurements to propagate with (0 of 2). IMU-CAMERA are likely messed up!!!
1649928199.2135444 [run_subscribe_msckf-1] [ZUPT]: There are no IMU data to check for zero velocity with!!
1649928199.2136121 [run_subscribe_msckf-1] image received out of order, unable to do anything (prop dt = -1649928076.497715)
1649928199.2136638 [run_subscribe_msckf-1] [TIME]: 0.0022 seconds total (455.8 hz)
1649928199.2137122 [run_subscribe_msckf-1] Propagator::select_imu_readings(): No IMU measurements to propagate with (0 of 2). IMU-CAMERA are likely messed up!!!
1649928199.2137661 [run_subscribe_msckf-1] [ZUPT]: There are no IMU data to check for zero velocity with!!
1649928199.2138128 [run_subscribe_msckf-1] image received out of order, unable to do anything (prop dt = -1649928076.433715)
1649928199.2138591 [run_subscribe_msckf-1] [TIME]: 0.0025 seconds total (403.9 hz)
1649928199.2139041 [run_subscribe_msckf-1] Propagator::select_imu_readings(): No IMU measurements to propagate with (0 of 2). IMU-CAMERA are likely messed up!!!
1649928199.2139497 [run_subscribe_msckf-1] [ZUPT]: There are no IMU data to check for zero velocity with!!
1649928199.2139952 [run_subscribe_msckf-1] image received out of order, unable to do anything (prop dt = -1649928076.369715)
1649928199.2140405 [run_subscribe_msckf-1] [TIME]: 0.0030 seconds total (330.7 hz)
1649928199.2140856 [run_subscribe_msckf-1] Propagator::select_imu_readings(): No IMU measurements to propagate with (0 of 2). IMU-CAMERA are likely messed up!!!
1649928199.2141306 [run_subscribe_msckf-1] [ZUPT]: There are no IMU data to check for zero velocity with!!
1649928199.2141757 [run_subscribe_msckf-1] image received out of order, unable to do anything (prop dt = -1649928076.301715)
1649928199.2142203 [run_subscribe_msckf-1] [TIME]: 0.0037 seconds total (268.0 hz)
1649928199.2142658 [run_subscribe_msckf-1] Propagator::select_imu_readings(): No IMU measurements to propagate with (0 of 2). IMU-CAMERA are likely messed up!!!
1649928199.2143106 [run_subscribe_msckf-1] [ZUPT]: There are no IMU data to check for zero velocity with!!
1649928199.2143557 [run_subscribe_msckf-1] image received out of order, unable to do anything (prop dt = -1649928076.233715)
1649928199.2144003 [run_subscribe_msckf-1] [TIME]: 0.0022 seconds total (455.6 hz)
1649928199.2144454 [run_subscribe_msckf-1] Propagator::select_imu_readings(): No IMU measurements to propagate with (0 of 2). IMU-CAMERA are likely messed up!!!
1649928199.2144904 [run_subscribe_msckf-1] [ZUPT]: There are no IMU data to check for zero velocity with!!
1649928199.2145352 [run_subscribe_msckf-1] image received out of order, unable to do anything (prop dt = -1649928076.169715)
1649928199.2145798 [run_subscribe_msckf-1] [TIME]: 0.0024 seconds total (411.7 hz)
1649928199.2146254 [run_subscribe_msckf-1] Propagator::select_imu_readings(): No IMU measurements to propagate with (0 of 2). IMU-CAMERA are likely messed up!!!
1649928199.2146702 [run_subscribe_msckf-1] [ZUPT]: There are no IMU data to check for zero velocity with!!
1649928199.2147148 [run_subscribe_msckf-1] image received out of order, unable to do anything (prop dt = -1649928076.101715)
1649928199.2147596 [run_subscribe_msckf-1] [TIME]: 0.0023 seconds total (435.9 hz)
1649928199.2148042 [run_subscribe_msckf-1] Propagator::select_imu_readings(): No IMU measurements to propagate with (0 of 2). IMU-CAMERA are likely messed up!!!
1649928199.2148485 [run_subscribe_msckf-1] [ZUPT]: There are no IMU data to check for zero velocity with!!
1649928199.2148931 [run_subscribe_msckf-1] image received out of order, unable to do anything (prop dt = -1649928076.033715)
1649928199.2149386 [run_subscribe_msckf-1] [TIME]: 0.0027 seconds total (376.5 hz)
1649928199.2149837 [run_subscribe_msckf-1] Propagator::select_imu_readings(): No IMU measurements to propagate with (0 of 2). IMU-CAMERA are likely messed up!!!
1649928199.2150345 [run_subscribe_msckf-1] [ZUPT]: There are no IMU data to check for zero velocity with!!
1649928199.2150793 [run_subscribe_msckf-1] image received out of order, unable to do anything (prop dt = -1649928075.969715)
1649928199.2151244 [run_subscribe_msckf-1] [TIME]: 0.0053 seconds total (189.6 hz)
1649928199.2151694 [run_subscribe_msckf-1] Propagator::select_imu_readings(): No IMU measurements to propagate with (0 of 2). IMU-CAMERA are likely messed up!!!
1649928199.2152267 [run_subscribe_msckf-1] [ZUPT]: There are no IMU data to check for zero velocity with!!
1649928199.2152731 [run_subscribe_msckf-1] image received out of order, unable to do anything (prop dt = -1649928075.901715)
1649928199.2153187 [run_subscribe_msckf-1] [TIME]: 0.0023 seconds total (435.0 hz)
1649928199.2153645 [run_subscribe_msckf-1] Propagator::select_imu_readings(): No IMU measurements to propagate with (0 of 2). IMU-CAMERA are likely messed up!!!
1649928199.2154098 [run_subscribe_msckf-1] [ZUPT]: There are no IMU data to check for zero velocity with!!
1649928199.2154551 [run_subscribe_msckf-1] image received out of order, unable to do anything (prop dt = -1649928075.833715)
1649928199.2155004 [run_subscribe_msckf-1] [TIME]: 0.0022 seconds total (449.4 hz)
1649928199.2155459 [run_subscribe_msckf-1] Propagator::select_imu_readings(): No IMU measurements to propagate with (0 of 2). IMU-CAMERA are likely messed up!!!
1649928199.2155914 [run_subscribe_msckf-1] [ZUPT]: There are no IMU data to check for zero velocity with!!
1649928199.2156370 [run_subscribe_msckf-1] image received out of order, unable to do anything (prop dt = -1649928075.769715)
1649928199.2156820 [run_subscribe_msckf-1] [TIME]: 0.0024 seconds total (411.4 hz)
1649928297.7045290 [WARNING] [launch]: user interrupted with ctrl-c (SIGINT)
1649928297.7074146 [run_subscribe_msckf-1] [INFO] [1649928297.704264472] [rclcpp]: signal_handler(signal_value=2)
1649928297.8073688 [INFO] [rviz2-2]: process has finished cleanly [pid 209025]
1649928297.8443716 [ERROR] [run_subscribe_msckf-1]: process has died [pid 209023, exit code -11, cmd '/home/stevedan/drone_ros2_ws/install/ov_msckf/lib/ov_msckf/run_subscribe_msckf --ros-args -r __ns:=/ --params-file /tmp/launch_params_dxy6d30x --params-file /tmp/launch_params_xwk891kx --params-file /tmp/launch_params__kp9k_e8 --params-file /tmp/launch_params_qk9_22dp'].
1649928297.8446238 [run_subscribe_msckf-1] Propagator::select_imu_readings(): No IMU measurements to propagate with

This is the configuaration files: I do not know what the problem is, can someone help me please. 

%YAML:1.0 # need to specify the file type at the top!

verbosity: "INFO" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT

use_fej: true # if first-estimate Jacobians should be used (enable for good consistency)
use_imuavg: true # if using discrete integration, if we should average sequential IMU measurements to "smooth" it
use_rk4int: true # if rk4 integration should be used (overrides imu averaging)

use_stereo: false # if we have more than 1 camera, if we should try to track stereo constraints between pairs
max_cameras: 1 # how many cameras we have 1 = mono, 2 = stereo, >2 = binocular (all mono tracking)

calib_cam_extrinsics: true # if the transform between camera and IMU should be optimized R_ItoC, p_CinI
calib_cam_intrinsics: true # if camera intrinsics should be optimized (focal, center, distortion)
calib_cam_timeoffset: true # if timeoffset between camera and IMU should be optimized

max_clones: 11 # how many clones in the sliding window
max_slam: 50 # number of features in our state vector
max_slam_in_update: 25 # update can be split into sequential updates of batches, how many in a batch
max_msckf_in_update: 40 # how many MSCKF features to use in the update
dt_slam_delay: 1 # delay before initializing (helps with stability from bad initialization...)

gravity_mag: 9.81 # magnitude of gravity in this location

feat_rep_msckf: "GLOBAL_3D"
feat_rep_slam: "ANCHORED_MSCKF_INVERSE_DEPTH"
feat_rep_aruco: "ANCHORED_MSCKF_INVERSE_DEPTH"

# zero velocity update parameters we can use
# we support either IMU-based or disparity detection.
try_zupt: true
zupt_chi2_multipler: 0 # set to 0 for only disp-based
zupt_max_velocity: 0.1
zupt_noise_multiplier: 50
zupt_max_disparity: 1.5 # set to 0 for only imu-based
zupt_only_at_beginning: true

# ==================================================================
# ==================================================================

init_window_time: 2.0 # how many seconds to collect initialization information
init_imu_thresh: 1.5 # threshold for variance of the accelerometer to detect a "jerk" in motion
init_max_disparity: 1.5 # max disparity to consider the platform stationary (dependent on resolution)
init_max_features: 25 # how many features to track during initialization (saves on computation)

init_dyn_mle_opt_calib: false
init_dyn_mle_max_iter: 50
init_dyn_mle_max_time: 0.05
init_dyn_mle_max_threads: 6
init_dyn_num_pose: 6
init_dyn_min_deg: 10.0

init_dyn_inflation_ori: 10
init_dyn_inflation_vel: 100
init_dyn_inflation_bg: 10
init_dyn_inflation_ba: 100
init_dyn_min_rec_cond: 1e-12

init_dyn_bias_g: [0.0, 0.0, 0.0]
init_dyn_bias_a: [0.0, 0.0, 0.0]

# ==================================================================
# ==================================================================

record_timing_information: false # if we want to record timing information of the method
record_timing_filepath: "/tmp/traj_timing.txt" # https://docs.openvins.com/eval-timing.html#eval-ov-timing-flame

# if we want to save the simulation state and its diagional covariance
# use this with rosrun ov_eval error_simulation
save_total_state: false
filepath_est: "/tmp/ov_estimate.txt"
filepath_std: "/tmp/ov_estimate_std.txt"
filepath_gt: "/tmp/ov_groundtruth.txt"

# ==================================================================
# ==================================================================

# our front-end feature tracking parameters
# we have a KLT and descriptor based (KLT is better implemented...)
use_klt: true
num_pts: 200
fast_threshold: 20
grid_x: 20
grid_y: 20
min_px_dist: 15
knn_ratio: 0.70
track_frequency: 21.0
downsample_cameras: false # will downsample image in half if true
multi_threading: true # if should enable opencv multi threading
histogram_method: "HISTOGRAM" # NONE, HISTOGRAM, CLAHE

# aruco tag tracker for the system
# DICT_6X6_1000 from https://chev.me/arucogen/
use_aruco: false
num_aruco: 1024
downsize_aruco: true

# ==================================================================
# ==================================================================

# camera noises and chi-squared threshold multipliers
up_msckf_sigma_px: 1
up_msckf_chi2_multipler: 1
up_slam_sigma_px: 1
up_slam_chi2_multipler: 1
up_aruco_sigma_px: 1
up_aruco_chi2_multipler: 1

# masks for our images
use_mask: false

# imu and camera spacial-temporal
# imu config should also have the correct noise values
relative_config_imu: "kalibr_imu_chain.yaml"
relative_config_imucam: "kalibr_imucam_chain.yaml"

%YAML:1.0

imu0:
  T_i_b:
    - [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]
  accelerometer_noise_density: 0.00186  # [ m / s^2 / sqrt(Hz) ]   ( accel "white noise" )
  accelerometer_random_walk: 0.006    # [ m / s^3 / sqrt(Hz) ].  ( accel bias diffusion )
  gyroscope_noise_density: 0.00018665     # [ rad / s / sqrt(Hz) ]   ( gyro "white noise" )
  gyroscope_random_walk: 3.8785e-05       # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion )
  model: calibrated
  rostopic: /fmu/sensor_combined/out
  time_offset: 0.0
  update_rate: 240.0

%YAML:1.0

cam0:
  T_imu_cam: #rotation from camera to IMU, position of IMU in camera
    - [1.0, 0.0, 0.0, 0.1]
    - [0.0, 1.0, 0.0, 0.0]
    - [0.0, 0.0, 1.0, 0.0]
    - [0.0, 0.0, 0.0, 1.0]
  cam_overlaps: [1]
  camera_model: pinhole
  distortion_coeffs: [0,0,0,0,0]
  distortion_model: plumb_bob
  intrinsics: [554.3827128226441, 554.3827128226441, 320.5, 240.5] #fu, fv, cu, cv
  resolution: [640, 480]
  rostopic: /front_camera/image_raw
# cam1:
#   T_imu_cam: #rotation from camera to IMU, position of IMU in camera
#     - [0.0125552670891, -0.999755099723, 0.0182237714554, -0.0198435579556]
#     - [0.999598781151, 0.0130119051815, 0.0251588363115, 0.0453689425024]
#     - [-0.0253898008918, 0.0179005838253, 0.999517347078, 0.00786212447038]
#     - [0.0, 0.0, 0.0, 1.0]
#   cam_overlaps: [0]
#   camera_model: pinhole
#   distortion_coeffs: [-0.28368365,0.07451284,-0.00010473,-3.55590700e-05]
#   distortion_model: radtan
#   intrinsics: [457.587, 456.134, 379.999, 255.238] #fu, fv, cu, cv
#   resolution: [752, 480]
#   rostopic: /cam1/image_raw
goldbattle commented 2 years ago

From your printout, it looks like there is an issue with the camera and IMU timestamps. I suggest you take a look at those and see if they are reasonable if you plot them (relative to the start time). It looks like the IMU is off by 1649928076.969715 seconds from the camera times. Looking at the PX4 message I think getting the timestamp is a bit more complicated:

uint64 timestamp # time since system start (microseconds)
int32 RELATIVE_TIMESTAMP_INVALID = 2147483647 # (0x7fffffff) If one of the relative timestamps is set to this value, it means the associated sensor values are invalid

You might want to try this VehicleImu which seems to have the sample timestamp which should be the raw time? Not sure, as I have never directly interfaced with PX4 sensor data.

uint64 timestamp # time since system start (microseconds)
uint64 timestamp_sample # the timestamp of the raw data (microseconds)?

Another option you can try is to just use the current ROS timestamp if you are running it on an actual robot. This means that both the camera and IMU are all using ROS time, and OpenVINS would just need to calibrate the delay between PX4 sending and the node receiving the IMU reading. This solution wouldn't work for rosbag playback through...

stevedanomodolor commented 2 years ago

I plot the two timestamp relattve to its starting point and I get this plot. Now I am using the timestamp_sample from the vehicleimu, this plot makes sense right? imu

goldbattle commented 2 years ago

From the axis it looks like there is still a 2 second difference? Is this time_camera_relative what is on the camera image topic headers that you are subscribing to?

stevedanomodolor commented 2 years ago

The relative time is the time from when I subscribed to the topic. Time_xxxx_relative = current timestamp -xxxx_suscribe_start_time

goldbattle commented 2 years ago

You can try writing a small node that just takes the px4 custom message, and then re-published a sensor_msgs/IMU topic. You could then just directly timestamp it with ros::Time::now() (or whatever the ros2 version of this is) in this node, and see how that performs. OpenVINS would then subscribe to this new topic and the existing camera topics you have.

stevedanomodolor commented 2 years ago

I was not able to fix, so I added a new imu to the simulation and used that instead.