Closed stevedanomodolor closed 2 years ago
Since I get this error I am not able to update the ekf correctly.
My setup
%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: /imu_pl/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
I just watched your video (thought it is just an image) and not I think there is something wrong with your setup. Your YAML seems to not represent your cam-imu system. Not sure how you are simulating it but features are extracted from z-direction of the system while your camera is looking forward. Try giving proper extrinsic first.
It seems to improve the result. If I have my imu noise as follows, how do convert it to the open_vins format? Thank you again for your previous observation.
<angular_velocity>
<x>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>2e-4</stddev>
<bias_mean>0.0000075</bias_mean>
<bias_stddev>0.0000008</bias_stddev>
</noise>
</x>
<y>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>2e-4</stddev>
<bias_mean>0.0000075</bias_mean>
<bias_stddev>0.0000008</bias_stddev>
</noise>
</y>
<z>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>2e-4</stddev>
<bias_mean>0.0000075</bias_mean>
<bias_stddev>0.0000008</bias_stddev>
</noise>
</z>
</angular_velocity>
<linear_acceleration>
<x>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>1.7e-2</stddev>
<bias_mean>0.1</bias_mean>
<bias_stddev>0.001</bias_stddev>
</noise>
</x>
<y>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>1.7e-2</stddev>
<bias_mean>0.1</bias_mean>
<bias_stddev>0.001</bias_stddev>
</noise>
</y>
<z>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>1.7e-2</stddev>
<bias_mean>0.1</bias_mean>
<bias_stddev>0.001</bias_stddev>
</noise>
</z>
</linear_acceleration>
I think your distortion model needs to be either radtan
or equidistant
https://github.com/rpng/open_vins/blob/master/config/euroc_mav/kalibr_imucam_chain.yaml#L25
You can try specifying the Euroc IMU noises and have gazebo simulate with zero noise. https://github.com/rpng/open_vins/blob/master/config/euroc_mav/kalibr_imu_chain.yaml
From the video, it looks like it might be a bad extrinsic or intrinsics. Additionally, I would verify that the initialization is good, as currently it looks shaky (large jump) and is likely another issue you have.
Correct, but that shouldn't be causing these issues if you have zero distortion. I recommend disabling online calibration to debug your calibration (calibcam* in yaml file).
On Tue, Apr 26, 2022 at 12:09 PM stevedanomodolor @.***> wrote:
distortion model for the camera you mean right? @goldbattle https://github.com/goldbattle
— Reply to this email directly, view it on GitHub https://github.com/rpng/open_vins/issues/244#issuecomment-1109983048, or unsubscribe https://github.com/notifications/unsubscribe-auth/AAQ6TYUKALAAM53PAE3WDFTVHAIKLANCNFSM5ULTPG4Q . You are receiving this because you were mentioned.Message ID: @.***>
I disabled the two first calibration parameters and got the following result. It seems to be somthing to do with the IMU right, because the drift is quite high. I am using the euroc noise parameters as you said and my simulation is set to zero noise.
@goldbattle It seems to work quit well initially. I still get the initial error, I commented the assertion as I mentioned initially. You mentioned this "Additionally, I would verify that the initialization is good, as currently it looks shaky (large jump) and is likely another issue you have." How do It, are there any initialization parameter I can use?
is it related to these parameters
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]
It looks like it is either the camera to IMU transform or camera intrinsics probably. To debug the initialization, I would inspect the console log (not sure how to do this in ROS2 as it should be printing a bunch to console), and make sure that it initializes as soon as it take off. If you enable the zero velocity update, it should initialize when it is still (I recommend this for mavs).
Is your position correct? See the updated config description below (see https://github.com/rpng/open_vins/commit/34a133bc09bf95c445cd8e5debbaf1fda4f4bffa)
T_imu_cam: #rotation from camera to IMU R_CtoI, position of camera in IMU p_CinI
T_imu_cam: #rotation from camera to IMU R_CtoI, position of camera in IMU p_CinI
It also looks to be a very large camera to IMU transform. I would double check this compared to what is shown in the gazebo link panel (left side of gui if I remember right). You can also echo it over TF too I believe.
The initialization seems to be correct
Also T_imu_cam: #rotation from camera to IMU R_CtoI, position of camera in IMU p_CinI, this is the position of the imu wrt the camera right?
1650992097.5913196 [run_subscribe_msckf-1] [32moverriding node [1m[32mverbosity[0m[32m with value from ROS!
1650992097.5914249 [run_subscribe_msckf-1] [0mSetting printing level to: INFO
1650992097.6124985 [run_subscribe_msckf-1] [32moverriding node [1m[32mmax_cameras[0m[32m with value from ROS!
1650992097.6126530 [run_subscribe_msckf-1] [0m[32moverriding node [1m[32mmax_cameras[0m[32m with value from ROS!
1650992097.6127369 [run_subscribe_msckf-1] [0m[32moverriding node [1m[32muse_stereo[0m[32m with value from ROS!
1650992097.6128073 [run_subscribe_msckf-1] [0m[32moverriding node [1m[32mmax_cameras[0m[32m with value from ROS!
1650992097.6128740 [run_subscribe_msckf-1] [0m[32moverriding node [1m[32muse_stereo[0m[32m with value from ROS!
1650992097.6129401 [run_subscribe_msckf-1] [0mLogging tets
1650992104.5673969 [run_subscribe_msckf-1] [34m[TIME]: 0.0062 seconds total (160.3 hz)
1650992104.5675108 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0055 seconds total (181.3 hz)
1650992104.5675597 [run_subscribe_msckf-1] [0m[32m[init]: successful initialization in 0.0002 seconds
1650992104.5676317 [run_subscribe_msckf-1] [0m[32m[init]: orientation = 0.0002, -0.0000, 0.0000, 1.0000
1650992104.5676677 [run_subscribe_msckf-1] [0m[32m[init]: bias gyro = 0.0001, -0.0001, -0.0000
1650992104.5677028 [run_subscribe_msckf-1] [0m[32m[init]: velocity = 0.0000, 0.0000, 0.0000
1650992104.5677369 [run_subscribe_msckf-1] [0m[32m[init]: bias accel = -0.0000, -0.0000, -0.0034
1650992104.5677705 [run_subscribe_msckf-1] [0m[32m[init]: position = 0.0000, 0.0000, 0.0000
1650992104.5678036 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0032 seconds total (310.1 hz)
1650992104.5678372 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0129 seconds total (77.8 hz)
1650992104.5678699 [run_subscribe_msckf-1] [0m[36m[ZUPT]: passed disparity (0.036 < 1.500, 187 features)
1650992104.5679026 [run_subscribe_msckf-1] [0m[36m[ZUPT]: accepted |v_IinG| = 0.002 (chi2 0.000 < 0.000)
1650992104.5679355 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0054 seconds total (185.4 hz)
1650992104.5679691 [run_subscribe_msckf-1] [0m[36m[ZUPT]: passed disparity (0.044 < 1.500, 187 features)
1650992104.5680017 [run_subscribe_msckf-1] [0m[36m[ZUPT]: accepted |v_IinG| = 0.001 (chi2 0.000 < 0.000)
1650992104.5680511 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0063 seconds total (157.7 hz)
1650992104.5681043 [run_subscribe_msckf-1] [0m[36m[ZUPT]: passed disparity (0.042 < 1.500, 187 features)
1650992104.5681460 [run_subscribe_msckf-1] [0m[36m[ZUPT]: accepted |v_IinG| = 0.001 (chi2 0.000 < 0.000)
1650992104.5681794 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0064 seconds total (155.8 hz)
1650992104.5682118 [run_subscribe_msckf-1] [0m[36m[ZUPT]: passed disparity (0.035 < 1.500, 187 features)
1650992104.5682437 [run_subscribe_msckf-1] [0m[36m[ZUPT]: accepted |v_IinG| = 0.001 (chi2 0.000 < 0.000)
1650992104.5682757 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0066 seconds total (150.9 hz)
1650992104.5683079 [run_subscribe_msckf-1] [0m[36m[ZUPT]: passed disparity (0.032 < 1.500, 187 features)
1650992104.5683405 [run_subscribe_msckf-1] [0m[36m[ZUPT]: accepted |v_IinG| = 0.001 (chi2 0.000 < 0.000)
1650992104.5683725 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0053 seconds total (190.0 hz)
1650992104.5684044 [run_subscribe_msckf-1] [0m[36m[ZUPT]: passed disparity (0.037 < 1.500, 187 features)
1650992104.5684366 [run_subscribe_msckf-1] [0m[36m[ZUPT]: accepted |v_IinG| = 0.001 (chi2 0.000 < 0.000)
1650992104.5684690 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0057 seconds total (176.9 hz)
1650992104.5685019 [run_subscribe_msckf-1] [0m[36m[ZUPT]: passed disparity (0.052 < 1.500, 187 features)
1650992104.5685472 [run_subscribe_msckf-1] [0m[36m[ZUPT]: accepted |v_IinG| = 0.001 (chi2 0.000 < 0.000)
1650992104.5685816 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0070 seconds total (143.2 hz)
1650992104.5686145 [run_subscribe_msckf-1] [0m[36m[ZUPT]: passed disparity (0.050 < 1.500, 187 features)
1650992104.5686464 [run_subscribe_msckf-1] [0m[36m[ZUPT]: accepted |v_IinG| = 0.001 (chi2 0.000 < 0.000)
1650992104.5686779 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0088 seconds total (114.3 hz)
1650992104.5687137 [run_subscribe_msckf-1] [0m[36m[ZUPT]: passed disparity (0.047 < 1.500, 187 features)
1650992104.5687549 [run_subscribe_msckf-1] [0m[36m[ZUPT]: accepted |v_IinG| = 0.001 (chi2 0.000 < 0.000)
1650992104.5687883 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0070 seconds total (142.6 hz)
1650992104.5688512 [run_subscribe_msckf-1] [0m[36m[ZUPT]: passed disparity (0.037 < 1.500, 187 features)
1650992104.5689018 [run_subscribe_msckf-1] [0m[36m[ZUPT]: accepted |v_IinG| = 0.001 (chi2 0.000 < 0.000)
1650992104.5689533 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0062 seconds total (160.4 hz)
1650992104.5690022 [run_subscribe_msckf-1] [0m[36m[ZUPT]: passed disparity (0.048 < 1.500, 187 features)
1650992104.5690508 [run_subscribe_msckf-1] [0m[36m[ZUPT]: accepted |v_IinG| = 0.001 (chi2 0.000 < 0.000)
1650992104.5690966 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0059 seconds total (170.6 hz)
1650992104.5691457 [run_subscribe_msckf-1] [0m[36m[ZUPT]: passed disparity (0.025 < 1.500, 187 features)
1650992104.5692089 [run_subscribe_msckf-1] [0m[36m[ZUPT]: accepted |v_IinG| = 0.002 (chi2 0.000 < 0.000)
1650992104.5692606 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0069 seconds total (145.9 hz)
1650992104.5693069 [run_subscribe_msckf-1] [0m[36m[ZUPT]: passed disparity (0.049 < 1.500, 187 features)
1650992104.5693409 [run_subscribe_msckf-1] [0m[36m[ZUPT]: accepted |v_IinG| = 0.002 (chi2 0.000 < 0.000)
1650992104.5693734 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0070 seconds total (142.3 hz)
1650992104.5694058 [run_subscribe_msckf-1] [0m[36m[ZUPT]: passed disparity (0.034 < 1.500, 187 features)
1650992104.5694375 [run_subscribe_msckf-1] [0m[36m[ZUPT]: accepted |v_IinG| = 0.002 (chi2 0.000 < 0.000)
1650992104.5694692 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0074 seconds total (134.4 hz)
1650992104.5695183 [run_subscribe_msckf-1] [0m[36m[ZUPT]: passed disparity (0.035 < 1.500, 187 features)
1650992104.5695505 [run_subscribe_msckf-1] [0m[36m[ZUPT]: accepted |v_IinG| = 0.002 (chi2 0.000 < 0.000)
1650992104.5695822 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0056 seconds total (178.6 hz)
1650992104.5696142 [run_subscribe_msckf-1] [0m[36m[ZUPT]: passed disparity (0.047 < 1.500, 187 features)
1650992104.5696459 [run_subscribe_msckf-1] [0m[36m[ZUPT]: accepted |v_IinG| = 0.002 (chi2 0.000 < 0.000)
1650992104.5696776 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0063 seconds total (158.1 hz)
1650992104.5697165 [run_subscribe_msckf-1] [0m[36m[ZUPT]: passed disparity (0.049 < 1.500, 187 features)
1650992104.5697508 [run_subscribe_msckf-1] [0m[36m[ZUPT]: accepted |v_IinG| = 0.002 (chi2 0.000 < 0.000)
1650992104.5697834 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0053 seconds total (190.3 hz)
1650992104.5698166 [run_subscribe_msckf-1] [0m[36m[ZUPT]: passed disparity (0.022 < 1.500, 187 features)
1650992104.5698493 [run_subscribe_msckf-1] [0m[36m[ZUPT]: accepted |v_IinG| = 0.002 (chi2 0.000 < 0.000)
1650992104.5698872 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0067 seconds total (148.2 hz)
1650992104.5699399 [run_subscribe_msckf-1] [0m[36m[ZUPT]: passed disparity (0.036 < 1.500, 187 features)
1650992104.5699949 [run_subscribe_msckf-1] [0m[36m[ZUPT]: accepted |v_IinG| = 0.002 (chi2 0.000 < 0.000)
1650992104.5700438 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0056 seconds total (180.0 hz)
1650992104.5700958 [run_subscribe_msckf-1] [0m[36m[ZUPT]: passed disparity (0.037 < 1.500, 187 features)
1650992104.5701444 [run_subscribe_msckf-1] [0m[36m[ZUPT]: accepted |v_IinG| = 0.002 (chi2 0.000 < 0.000)
1650992104.5701890 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0063 seconds total (159.3 hz)
1650992106.1075404 [run_subscribe_msckf-1] [0m[36m[ZUPT]: passed disparity (0.053 < 1.500, 187 features)
1650992106.1077087 [run_subscribe_msckf-1] [0m[36m[ZUPT]: accepted |v_IinG| = 0.002 (chi2 0.000 < 0.000)
1650992106.1077876 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0066 seconds total (150.7 hz)
1650992106.1078527 [run_subscribe_msckf-1] [0m[36m[ZUPT]: passed disparity (0.044 < 1.500, 187 features)
1650992106.1079161 [run_subscribe_msckf-1] [0m[36m[ZUPT]: accepted |v_IinG| = 0.002 (chi2 0.000 < 0.000)
1650992106.1079776 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0051 seconds total (194.3 hz)
1650992106.1080384 [run_subscribe_msckf-1] [0m[36m[ZUPT]: passed disparity (0.034 < 1.500, 187 features)
1650992106.1080990 [run_subscribe_msckf-1] [0m[36m[ZUPT]: accepted |v_IinG| = 0.002 (chi2 0.000 < 0.000)
1650992106.1081595 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0051 seconds total (196.0 hz)
1650992106.1082189 [run_subscribe_msckf-1] [0m[36m[ZUPT]: passed disparity (0.039 < 1.500, 187 features)
1650992106.1082788 [run_subscribe_msckf-1] [0m[36m[ZUPT]: accepted |v_IinG| = 0.002 (chi2 0.000 < 0.000)
1650992106.1083381 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0061 seconds total (163.5 hz)
1650992106.1083970 [run_subscribe_msckf-1] [0m[36m[ZUPT]: passed disparity (0.053 < 1.500, 187 features)
1650992106.1084557 [run_subscribe_msckf-1] [0m[36m[ZUPT]: accepted |v_IinG| = 0.002 (chi2 0.000 < 0.000)
1650992106.1085148 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0054 seconds total (184.2 hz)
1650992106.1085742 [run_subscribe_msckf-1] [0m[36m[ZUPT]: passed disparity (0.050 < 1.500, 186 features)
1650992106.1086326 [run_subscribe_msckf-1] [0m[36m[ZUPT]: accepted |v_IinG| = 0.002 (chi2 0.000 < 0.000)
1650992106.1086907 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0066 seconds total (152.1 hz)
1650992106.1087496 [run_subscribe_msckf-1] [0m[36m[ZUPT]: passed disparity (0.055 < 1.500, 186 features)
1650992106.1088085 [run_subscribe_msckf-1] [0m[36m[ZUPT]: accepted |v_IinG| = 0.002 (chi2 0.000 < 0.000)
1650992106.1088676 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0069 seconds total (144.5 hz)
1650992106.1089406 [run_subscribe_msckf-1] [0m[36m[ZUPT]: passed disparity (0.044 < 1.500, 186 features)
1650992106.1089976 [run_subscribe_msckf-1] [0m[36m[ZUPT]: accepted |v_IinG| = 0.002 (chi2 0.000 < 0.000)
1650992106.1090558 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0081 seconds total (122.7 hz)
1650992106.1091130 [run_subscribe_msckf-1] [0m[36m[ZUPT]: passed disparity (0.044 < 1.500, 186 features)
1650992106.1091971 [run_subscribe_msckf-1] [0m[36m[ZUPT]: accepted |v_IinG| = 0.002 (chi2 0.000 < 0.000)
1650992106.1092722 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0054 seconds total (185.0 hz)
1650992106.1093304 [run_subscribe_msckf-1] [0m[36m[ZUPT]: passed disparity (0.042 < 1.500, 186 features)
1650992106.1093886 [run_subscribe_msckf-1] [0m[36m[ZUPT]: accepted |v_IinG| = 0.002 (chi2 0.000 < 0.000)
1650992106.1094463 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0061 seconds total (164.9 hz)
1650992106.1095040 [run_subscribe_msckf-1] [0m[36m[ZUPT]: passed disparity (0.037 < 1.500, 186 features)
1650992106.1095619 [run_subscribe_msckf-1] [0m[36m[ZUPT]: accepted |v_IinG| = 0.002 (chi2 0.000 < 0.000)
1650992106.1096191 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0058 seconds total (172.4 hz)
1650992106.1096768 [run_subscribe_msckf-1] [0m[36m[ZUPT]: passed disparity (0.037 < 1.500, 185 features)
1650992106.1097341 [run_subscribe_msckf-1] [0m[36m[ZUPT]: accepted |v_IinG| = 0.002 (chi2 0.000 < 0.000)
1650992106.1097908 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0059 seconds total (169.6 hz)
1650992106.1098480 [run_subscribe_msckf-1] [0m[36m[ZUPT]: passed disparity (0.053 < 1.500, 185 features)
1650992106.1099052 [run_subscribe_msckf-1] [0m[36m[ZUPT]: accepted |v_IinG| = 0.002 (chi2 0.000 < 0.000)
1650992106.1099629 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0080 seconds total (125.5 hz)
1650992106.1100469 [run_subscribe_msckf-1] [0m[36m[ZUPT]: passed disparity (0.042 < 1.500, 184 features)
1650992106.1101015 [run_subscribe_msckf-1] [0m[36m[ZUPT]: accepted |v_IinG| = 0.002 (chi2 0.000 < 0.000)
1650992106.1101558 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0053 seconds total (189.6 hz)
1650992106.1102092 [run_subscribe_msckf-1] [0m[36m[ZUPT]: passed disparity (0.017 < 1.500, 184 features)
1650992106.1102619 [run_subscribe_msckf-1] [0m[36m[ZUPT]: accepted |v_IinG| = 0.002 (chi2 0.000 < 0.000)
1650992106.1103148 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0050 seconds total (201.7 hz)
1650992106.1103678 [run_subscribe_msckf-1] [0m[36m[ZUPT]: passed disparity (0.042 < 1.500, 184 features)
1650992106.1104207 [run_subscribe_msckf-1] [0m[36m[ZUPT]: accepted |v_IinG| = 0.002 (chi2 0.000 < 0.000)
1650992106.1104739 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0050 seconds total (199.2 hz)
1650992106.1105421 [run_subscribe_msckf-1] [0m[36m[ZUPT]: passed disparity (0.039 < 1.500, 184 features)
1650992106.1106088 [run_subscribe_msckf-1] [0m[36m[ZUPT]: accepted |v_IinG| = 0.002 (chi2 0.000 < 0.000)
1650992106.1106596 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0051 seconds total (195.6 hz)
1650992106.1107106 [run_subscribe_msckf-1] [0m[36m[ZUPT]: passed disparity (0.039 < 1.500, 184 features)
1650992106.1107616 [run_subscribe_msckf-1] [0m[36m[ZUPT]: accepted |v_IinG| = 0.002 (chi2 0.000 < 0.000)
1650992106.1108131 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0061 seconds total (164.8 hz)
1650992106.1108642 [run_subscribe_msckf-1] [0m[36m[ZUPT]: passed disparity (0.035 < 1.500, 184 features)
1650992106.1109161 [run_subscribe_msckf-1] [0m[36m[ZUPT]: accepted |v_IinG| = 0.002 (chi2 0.000 < 0.000)
1650992106.1109667 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0073 seconds total (136.9 hz)
1650992106.1110177 [run_subscribe_msckf-1] [0m[36m[ZUPT]: passed disparity (0.031 < 1.500, 184 features)
1650992106.1110680 [run_subscribe_msckf-1] [0m[36m[ZUPT]: accepted |v_IinG| = 0.002 (chi2 0.000 < 0.000)
1650992106.1111186 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0068 seconds total (147.3 hz)
1650992106.1111703 [run_subscribe_msckf-1] [0m[36m[ZUPT]: passed disparity (0.035 < 1.500, 184 features)
1650992106.1112218 [run_subscribe_msckf-1] [0m[36m[ZUPT]: accepted |v_IinG| = 0.002 (chi2 0.000 < 0.000)
1650992106.1112962 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0050 seconds total (201.1 hz)
1650992106.1113675 [run_subscribe_msckf-1] [0m[36m[ZUPT]: passed disparity (0.039 < 1.500, 184 features)
1650992106.1114383 [run_subscribe_msckf-1] [0m[36m[ZUPT]: accepted |v_IinG| = 0.002 (chi2 0.000 < 0.000)
1650992106.1115117 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0066 seconds total (150.9 hz)
1650992106.1115708 [run_subscribe_msckf-1] [0m[36m[ZUPT]: passed disparity (0.052 < 1.500, 184 features)
1650992106.1116292 [run_subscribe_msckf-1] [0m[36m[ZUPT]: accepted |v_IinG| = 0.002 (chi2 0.000 < 0.000)
1650992106.1116869 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0070 seconds total (142.3 hz)
1650992107.6287270 [run_subscribe_msckf-1] [0m[36m[ZUPT]: passed disparity (0.046 < 1.500, 184 features)
1650992107.6288471 [run_subscribe_msckf-1] [0m[36m[ZUPT]: accepted |v_IinG| = 0.002 (chi2 0.000 < 0.000)
1650992107.6288965 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0058 seconds total (173.0 hz)
1650992107.6289363 [run_subscribe_msckf-1] [0m[36m[ZUPT]: passed disparity (0.048 < 1.500, 184 features)
1650992107.6289723 [run_subscribe_msckf-1] [0m[36m[ZUPT]: accepted |v_IinG| = 0.002 (chi2 0.000 < 0.000)
1650992107.6290069 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0049 seconds total (202.7 hz)
1650992107.6290407 [run_subscribe_msckf-1] [0m[36m[ZUPT]: passed disparity (0.034 < 1.500, 184 features)
1650992107.6290743 [run_subscribe_msckf-1] [0m[36m[ZUPT]: accepted |v_IinG| = 0.002 (chi2 0.000 < 0.000)
1650992107.6291072 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0049 seconds total (205.7 hz)
1650992107.6291399 [run_subscribe_msckf-1] [0m[36m[ZUPT]: passed disparity (0.035 < 1.500, 184 features)
1650992107.6291950 [run_subscribe_msckf-1] [0m[36m[ZUPT]: accepted |v_IinG| = 0.002 (chi2 0.000 < 0.000)
1650992107.6292307 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0050 seconds total (201.8 hz)
1650992107.6292636 [run_subscribe_msckf-1] [0m[36m[ZUPT]: passed disparity (0.044 < 1.500, 184 features)
1650992107.6292958 [run_subscribe_msckf-1] [0m[36m[ZUPT]: accepted |v_IinG| = 0.002 (chi2 0.000 < 0.000)
1650992107.6293287 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0074 seconds total (134.6 hz)
1650992107.6293614 [run_subscribe_msckf-1] [0m[36m[ZUPT]: passed disparity (0.058 < 1.500, 184 features)
1650992107.6293945 [run_subscribe_msckf-1] [0m[36m[ZUPT]: accepted |v_IinG| = 0.002 (chi2 0.000 < 0.000)
1650992107.6294272 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0056 seconds total (177.5 hz)
1650992107.6294594 [run_subscribe_msckf-1] [0m[36m[ZUPT]: passed disparity (0.025 < 1.500, 184 features)
1650992107.6294923 [run_subscribe_msckf-1] [0m[36m[ZUPT]: accepted |v_IinG| = 0.002 (chi2 0.000 < 0.000)
1650992107.6295316 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0081 seconds total (123.1 hz)
1650992107.6295774 [run_subscribe_msckf-1] [0m[36m[ZUPT]: passed disparity (0.026 < 1.500, 184 features)
1650992107.6296215 [run_subscribe_msckf-1] [0m[36m[ZUPT]: accepted |v_IinG| = 0.002 (chi2 0.000 < 0.000)
1650992107.6296606 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0059 seconds total (169.5 hz)
1650992107.6296926 [run_subscribe_msckf-1] [0m[36m[ZUPT]: passed disparity (0.038 < 1.500, 184 features)
1650992107.6297245 [run_subscribe_msckf-1] [0m[36m[ZUPT]: accepted |v_IinG| = 0.002 (chi2 0.000 < 0.000)
1650992107.6297562 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0050 seconds total (199.5 hz)
1650992107.6297994 [run_subscribe_msckf-1] [0m[36m[ZUPT]: passed disparity (0.022 < 1.500, 184 features)
1650992107.6298401 [run_subscribe_msckf-1] [0m[36m[ZUPT]: accepted |v_IinG| = 0.002 (chi2 0.000 < 0.000)
1650992107.6298735 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0103 seconds total (97.5 hz)
1650992107.6299129 [run_subscribe_msckf-1] [0m[36m[ZUPT]: passed disparity (0.029 < 1.500, 184 features)
1650992107.6299448 [run_subscribe_msckf-1] [0m[36m[ZUPT]: accepted |v_IinG| = 0.002 (chi2 0.000 < 0.000)
1650992107.6299767 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0066 seconds total (152.2 hz)
1650992107.6300089 [run_subscribe_msckf-1] [0m[36m[ZUPT]: passed disparity (0.045 < 1.500, 184 features)
1650992107.6300409 [run_subscribe_msckf-1] [0m[36m[ZUPT]: accepted |v_IinG| = 0.002 (chi2 0.000 < 0.000)
1650992107.6300728 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0048 seconds total (208.9 hz)
1650992107.6301048 [run_subscribe_msckf-1] [0m[36m[ZUPT]: passed disparity (0.056 < 1.500, 184 features)
1650992107.6301367 [run_subscribe_msckf-1] [0m[36m[ZUPT]: accepted |v_IinG| = 0.002 (chi2 0.000 < 0.000)
1650992107.6301694 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0058 seconds total (171.2 hz)
1650992107.6302016 [run_subscribe_msckf-1] [0m[36m[ZUPT]: passed disparity (0.053 < 1.500, 184 features)
1650992107.6302333 [run_subscribe_msckf-1] [0m[36m[ZUPT]: accepted |v_IinG| = 0.002 (chi2 0.000 < 0.000)
1650992107.6302657 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0049 seconds total (202.8 hz)
1650992107.6302977 [run_subscribe_msckf-1] [0m[36m[ZUPT]: passed disparity (0.062 < 1.500, 184 features)
1650992107.6303296 [run_subscribe_msckf-1] [0m[36m[ZUPT]: accepted |v_IinG| = 0.002 (chi2 0.000 < 0.000)
1650992107.6303616 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0083 seconds total (121.0 hz)
1650992107.6303933 [run_subscribe_msckf-1] [0m[36m[ZUPT]: passed disparity (0.029 < 1.500, 184 features)
1650992107.6304247 [run_subscribe_msckf-1] [0m[36m[ZUPT]: accepted |v_IinG| = 0.002 (chi2 0.000 < 0.000)
1650992107.6304562 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0055 seconds total (181.8 hz)
1650992107.6305044 [run_subscribe_msckf-1] [0m[36m[ZUPT]: passed disparity (0.028 < 1.500, 184 features)
1650992107.6305356 [run_subscribe_msckf-1] [0m[36m[ZUPT]: accepted |v_IinG| = 0.002 (chi2 0.000 < 0.000)
1650992107.6305897 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0058 seconds total (173.9 hz)
1650992107.6306221 [run_subscribe_msckf-1] [0m[36m[ZUPT]: passed disparity (0.038 < 1.500, 184 features)
1650992107.6306536 [run_subscribe_msckf-1] [0m[36m[ZUPT]: accepted |v_IinG| = 0.002 (chi2 0.000 < 0.000)
1650992107.6306844 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0079 seconds total (127.4 hz)
1650992107.6307154 [run_subscribe_msckf-1] [0m[36m[ZUPT]: passed disparity (0.024 < 1.500, 184 features)
1650992107.6307466 [run_subscribe_msckf-1] [0m[36m[ZUPT]: accepted |v_IinG| = 0.002 (chi2 0.000 < 0.000)
1650992107.6307776 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0048 seconds total (208.6 hz)
1650992107.6308081 [run_subscribe_msckf-1] [0m[36m[ZUPT]: passed disparity (0.030 < 1.500, 184 features)
1650992107.6308391 [run_subscribe_msckf-1] [0m[36m[ZUPT]: accepted |v_IinG| = 0.002 (chi2 0.000 < 0.000)
1650992107.6308875 [run_subscribe_msckf-1] [0m[34m[TIME]: 0.0056 seconds total (179.1 hz)
It should be the position of the camera in respect to the IMU with the rotation from camera to the IMU frame. It needs to be a valid SE(3) transform.
Wouldnt that mean that the following commit is wrong?
YAML:1.0 # need to specify the file type at the top!
cam0:
T_cam_imu: #rotation from IMU to camera R_ItoC, position of IMU in camera p_IinC
- [-0.9995250378696743, 0.029615343885863205, -0.008522328211654736, 0.04727988224914392]
- [0.0075019185074052044, -0.03439736061393144, -0.9993800792498829, -0.047443232143367084]
- [-0.02989013031643309, -0.998969345370175, 0.03415885127385616, -0.0681999605066297]
@@ -16,7 +16,7 @@ cam0:
cam1:
**_T_cam_imu: #rotation from IMU to camera R_ItoC, position of IMU in camera p_IinC_**
- [-0.9995110484978581, 0.030299116376600627, -0.0077218830287333565, -0.053697434688869734]
- [0.008104079263822521, 0.012511643720192351, -0.9998888851620987, -0.046131737923635924]
- [-0.030199136245891378, -0.9994625667418545, -0.012751072573940885, -0.07149261284195751]
I am not sure what you are referring to, can you be more specific? You can refer to here if this helps: https://docs.openvins.com/dev-coding-style.html#coding-style-naming
On Tue, Apr 26, 2022 at 1:20 PM stevedanomodolor @.***> wrote:
Wouldnt that mean that the following commit is wrong?
YAML:1.0 # need to specify the file type at the top!
cam0: T_cam_imu: #rotation from IMU to camera R_ItoC, position of IMU in camera p_IinC
- [-0.9995250378696743, 0.029615343885863205, -0.008522328211654736, 0.04727988224914392]
- [0.0075019185074052044, -0.03439736061393144, -0.9993800792498829, -0.047443232143367084]
- [-0.02989013031643309, -0.998969345370175, 0.03415885127385616, -0.0681999605066297] @@ -16,7 +16,7 @@ cam0:
cam1: _T_cam_imu: #rotation from IMU to camera R_ItoC, position of IMU in camera pIinC
- [-0.9995110484978581, 0.030299116376600627, -0.0077218830287333565, -0.053697434688869734]
- [0.008104079263822521, 0.012511643720192351, -0.9998888851620987, -0.046131737923635924]
- [-0.030199136245891378, -0.9994625667418545, -0.012751072573940885, -0.07149261284195751]
— Reply to this email directly, view it on GitHub https://github.com/rpng/open_vins/issues/244#issuecomment-1110056570, or unsubscribe https://github.com/notifications/unsubscribe-auth/AAQ6TYUZM7PQ2654PTYQT5DVHAQV3ANCNFSM5ULTPG4Q . You are receiving this because you were mentioned.Message ID: @.***>
cam1: T_cam_imu: #rotation from IMU to camera R_ItoC, position of IMU in camera p_IinC
IS there a way to visualize the initial camera features without having to move the robot. this way I can debug it correctly?
This is another result, it seems before it goes awire, it does not detect any visual point @goldbattle
cam1: T_cam_imu: #rotation from IMU to camera R_ItoC, position of IMU in camera p_IinC - [-0.9995110484978581, 0.030299116376600627, -0.0077218830287333565, -0.053697434688869734] - [0.008104079263822521, 0.012511643720192351, -0.9998888851620987, -0.046131737923635924] - [-0.030199136245891378, -0.9994625667418545, -0.012751072573940885, -0.07149261284195751] I am guessing this is just a typo, I am referring to the comment in this fille
There is no typo. The T_cam_imu
is the T_ItoC
SE(3) matrix which has the rotation R_ItoC
and p_IinC
.
The variable T_cam_imu
comes from kalibr which uses this convention: https://github.com/ethz-asl/maplab/wiki/Expressing-frame-transformations-in-code
IS there a way to visualize the initial camera features without having to move the robot. this way I can debug it correctly?
Robot needs to move in order to have disparity to be able to triangulate features. If the platform is stationary, with a single camera it is impossible to recover the depth of the features. If you are using stereo, then one can triangulate using a single frame. In general we require some movement for initialization.
@goldbattle It finally works, the issue was that i used a wrong camera axes. Thank you very much though, Thank you too @WoosikLee2510
@stevedanomodolor Hey, how did you manage to make it work?
@anastaga the issue was that I did not put the transformation matrix correctly
@stevedanomodolor How did you find the correct transformation matrix? Using the two calibcam* functions/parameters? (Because i used those and it didnt fix the issue. )
@anastaga since I used simulation, I knew the exact values of the transformation matrix. If you are using a real robot, you probably need to do calibration,
I am using ros2 foxy with gazebo.