Closed CanCanZeng closed 2 years ago
Hi, I find the causer, there is a small mistake in message sorting.
https://github.com/rpng/open_vins/blob/057534beadca2c8d72571fed180ad289136fcdc4/ov_core/src/utils/sensor_data.h#L72
change to
return *std::min_element(sensor_ids.begin(), sensor_ids.end()) < *std::min_element(other.sensor_ids.begin(), other.sensor_ids.end());
the randomness will be eliminated.
there comes another odd phenomenon, most of the slam points are in cam0, is this reasonable? @goldbattle
Great, thanks for the many PRs. I will try to take some time in the new year to take a look at what you have posted. We don't enforce any distribution of SLAM features across camera frames, but we probably should (tracking a few in each does have benefits if the views do not overlap).
Hi, I was unable to reproduce this with the following command:
roslaunch ov_msckf serial.launch use_stereo:=false
On V1_02, I always get the following after the whole dataset:
q_GtoI = 0.815,-0.038,0.577,0.034 | p_IinG = -0.029,-0.141,-0.026 | dist = 75.03 (meters)
bg = -0.0019,0.0210,0.0760 | ba = -0.0198,0.1734,0.0499
camera-imu timeoffset = -0.00017
cam0 intrinsics = 460.455,459.514,366.991,246.790 | -0.282,0.073,0.000,0.000
cam1 intrinsics = 458.880,457.702,378.376,253.246 | -0.280,0.071,0.000,0.000
cam0 extrinsics = -0.007,0.009,0.702,0.713 | 0.067,-0.006,0.006
cam1 extrinsics = -0.001,0.013,0.702,0.712 | -0.040,-0.003,0.004
If you have any more details that would be great, thanks. Additionally, if you can run with ROS with the included launch files that would work great also.
I made a small change in 68680815176ae946ef6387185647e8ca46af4be1, let me know if this fixes your local issues (I am unable to reproduce on my machine).
Yes, like this issue https://github.com/rpng/open_vins/issues/216, I write an exe without ROS, so you may unable to reproduce my situation. I will check the latest code as soon as possible. Many thanks for your quick fix!
Hi @goldbattle , I checked the code, and tested old version 0da5f339b5fa9ba91af314f2cdb1fefdca6300e2. But I find the result still has some randomness. I did not change anything, and run this coderoslaunch ov_msckf serial.launch use_stereo:=false
, the following is the results of 3 times run:
q_GtoI = 0.816,-0.028,0.577,0.027 | p_IinG = 0.002,-0.129,-0.025 | dist = 75.11 (meters)
bg = -0.0020,0.0211,0.0761 | ba = -0.0193,0.1799,0.0511
camera-imu timeoffset = -0.00013
cam0 intrinsics = 460.571,459.724,367.839,246.430 | -0.281,0.072,0.000,-0.000
cam1 intrinsics = 459.684,458.492,379.450,253.151 | -0.282,0.073,0.000,-0.000
cam0 extrinsics = -0.007,0.010,0.702,0.713 | 0.064,-0.005,0.007
cam1 extrinsics = -0.001,0.014,0.702,0.712 | -0.045,-0.005,0.007
-------------------------------------------------------------------------------------------
q_GtoI = 0.816,-0.028,0.577,0.027 | p_IinG = -0.025,-0.057,-0.016 | dist = 75.11 (meters)
bg = -0.0018,0.0211,0.0760 | ba = -0.0156,0.1758,0.0624
camera-imu timeoffset = -0.00015
cam0 intrinsics = 460.252,459.574,368.954,246.783 | -0.281,0.071,0.000,-0.000
cam1 intrinsics = 459.354,458.457,379.123,253.442 | -0.282,0.072,0.000,-0.000
cam0 extrinsics = -0.008,0.010,0.702,0.713 | 0.064,-0.006,0.007
cam1 extrinsics = -0.001,0.014,0.702,0.712 | -0.042,-0.005,0.006
-------------------------------------------------------------------------------------------
q_GtoI = 0.816,-0.021,0.577,0.023 | p_IinG = -0.021,-0.160,0.031 | dist = 74.91 (meters)
bg = -0.0021,0.0211,0.0763 | ba = -0.0190,0.1582,0.0520
camera-imu timeoffset = -0.00012
cam0 intrinsics = 460.185,459.154,368.039,246.981 | -0.281,0.072,0.000,-0.000
cam1 intrinsics = 459.564,458.556,379.319,253.390 | -0.282,0.073,0.000,0.000
cam0 extrinsics = -0.007,0.010,0.702,0.712 | 0.065,-0.005,0.006
cam1 extrinsics = -0.001,0.014,0.702,0.712 | -0.043,-0.007,0.007
then I checked out to latest master 6d65547d5a2399803af6c50a4e590f1fb7bc1b76, the result is always the same:
q_GtoI = 0.816,-0.030,0.577,0.028 | p_IinG = -0.020,-0.102,-0.025 | dist = 75.22 (meters)
bg = -0.0019,0.0211,0.0761 | ba = -0.0153,0.1771,0.0611
camera-imu timeoffset = -0.00015
cam0 intrinsics = 460.469,459.561,366.901,246.663 | -0.282,0.072,0.000,-0.000
cam1 intrinsics = 459.121,457.895,378.502,253.096 | -0.280,0.071,0.000,0.000
cam0 extrinsics = -0.007,0.009,0.702,0.713 | 0.066,-0.005,0.006
cam1 extrinsics = -0.001,0.013,0.702,0.712 | -0.041,-0.003,0.005
but the SLAM points are all on left camera, which is not shown in 0da5f339b5fa9ba91af314f2cdb1fefdca6300e2
for old version 0da5f339b5fa9ba91af314f2cdb1fefdca6300e2, the results of roslaunch ov_msckf serial.launch max_cameras:=1 are always the same, so I think the randomness must come from the sort stage.
I was wrong, the old version feed two cameras once and use https://github.com/rpng/open_vins/blob/0da5f339b5fa9ba91af314f2cdb1fefdca6300e2/ov_core/src/track/TrackKLT.cpp#L46 to track images in multi-monocular way. But the latest version feed cameras one by one.
This should not have any difference in theory, but then I changed to feed cameras once and let the insider to track images one by one, the result has randomness again! And the SLAM points show on both left and right image. I changed parallel_for_
to for
loop, it has no effect. Also, the sort function is not called, since we feed image in once therefor the size of camera_queue
keeps to 1.
Don't know how this come, I',m still digging into it.
The randomness comes from parallel_for_
, I checked the result once again. I suspect the causer is from here https://github.com/rpng/open_vins/blob/6d65547d5a2399803af6c50a4e590f1fb7bc1b76/ov_core/src/track/TrackKLT.cpp#L146 , you update the global varible in parallel way, so cause the randomness.
Another thing is that https://github.com/rpng/open_vins/blob/6d65547d5a2399803af6c50a4e590f1fb7bc1b76/ov_core/src/track/TrackKLT.cpp#L87 you use [] operator
of unordered_map, this is a write action in first call, and unordered_map is not thread safe, so when I feed 4 cameras, the program may crash in here sometimes. So I add the following code befor parallel_for_
to avoid this problem. It's not very elegent but it works
if(pts_last.size() < num_images) {
for(size_t cam_idx = 0; cam_idx < num_images; cam_idx++) {
int cam_id = message.sensor_ids.at(cam_idx);
pts_last[cam_id];
ids_last[cam_id];
img_last[cam_id];
img_pyramid_last[cam_id];
img_mask_last[cam_id];
}
}
So my conclusion is that, if I use the latest code 6d65547d5a2399803af6c50a4e590f1fb7bc1b76 , and feed all cameras to open_vins at once, and change parallel_for_
to for loop
, the result has no randomness and the SLAM points distributes on all cameras(which is expected).
We use a mutex on each camera id when performing tracking. This means that only one thread at a time can process a given camera id object. More details can be found in the TrackBase class here: https://docs.openvins.com/classov__core_1_1TrackBase.html
I think your suspicion that the parallel_for_
might cause the issue is correct. Can you try enabling single threaded mode? Your presented solution doesn't quite make sense on why it would fix anything (what you presented would be optimized away by the compiler normally). You can try to replace the parallel_for_
with a standard loop to see if that fixes the problems for you.
Also since you are using a custom reader / main file, you might want to check that this is repeatable. Hope this helps.
Hi @goldbattle ,
You can try to replace the parallelfor with a standard loop to see if that fixes the problems for you.
if I change parallel_for_
to for loop
, the randomness gone. As I have made a pull request, you can check the result if you have some time. just uncomment here https://github.com/CanCanZeng/open_vins/blob/204b7c8e352d306874546a0750944e59e79c5a17/ov_core/src/track/TrackKLT.cpp#L56 and comment out here https://github.com/CanCanZeng/open_vins/blob/204b7c8e352d306874546a0750944e59e79c5a17/ov_core/src/track/TrackKLT.cpp#L61, you can see the difference.
Can you try enabling single threaded mode?
Yes, if I using single threaded mode, the randomness gone.
Great. Glad the randomness in the frontend is gone when serial mode is used as desired.
Hi, I find that if I run euroc dataset in stereo mode or using just one camera, the output pose result is alwayz the same. But if I run with two camera in non-stereo mode, the output pose result show same randomness, how this come? You can try it on euroc dataset, I tested euroc v1_01 data, and without ROS(I guess the phenomenon will remain the same for ROS version)