HKUST-Aerial-Robotics / GVINS

Tightly coupled GNSS-Visual-Inertial system for locally smooth and globally consistent state estimation in complex environment.
GNU General Public License v3.0
865 stars 230 forks source link

Gnss does not initialize #32

Open maghauke opened 2 years ago

maghauke commented 2 years ago

Hi, I have collected a dataset where the "/ublox_driver/range_meas" publish at 1 Hz, from your published dataset I see that the same topic publishes at 10 Hz. The visual-inertial system initializes well for mye dataset, but the GNSS estimate does not. Is there a minimum requirement for the frequency of the "/ublox_driver/range_meas" topic for the gnss estimate to initialize? The GNSSVIAlign() function seems to be stopped by the following line:

for (uint32_t i = 0; i < (WINDOW_SIZE+1); ++i) { if (gnss_meas_buf[i].empty() || gnss_meas_buf[i].size() < 10){ return false; } } So I guess my question is if this seems to be a data-set issue or something that can be fixed by changing some thresholds in the code?

Mil1ium commented 2 years ago

I have had this problem. I guess your image fps is higher than 20Hz, for example 30Hz.

In my case, I use Realsense D435i for this project. Firstly, the image fps is set to 30Hz and GNSS output rate is set to 10Hz in u-center, then the GNSS-VI init problem occured. After reading the code, I guess the cause is that each image in slide windows did not get enough GNSS measurements due to fast camera fps(30Hz).

So, the solution is to slow down your camera fps , or increase your GNSS output rate. In my case, I change the GNSS output rate to 16.67Hz(period 60ms) ,and change the condition "10" in the code to a smaller number, for example 6, and the GNSS-VI init problem solved.

Moreland-cas commented 1 year ago

I have had this problem. I guess your image fps is higher than 20Hz, for example 30Hz.

In my case, I use Realsense D435i for this project. Firstly, the image fps is set to 30Hz and GNSS output rate is set to 10Hz in u-center, then the GNSS-VI init problem occured. After reading the code, I guess the cause is that each image in slide windows did not get enough GNSS measurements due to fast camera fps(30Hz).

So, the solution is to slow down your camera fps , or increase your GNSS output rate. In my case, I change the GNSS output rate to 16.67Hz(period 60ms) ,and change the condition "10" in the code to a smaller number, for example 6, and the GNSS-VI init problem solved.

Hi do you knoe how to publish the /external_trigger topic? I guess initialization failed has somthing to do with the lack of this topic?

Mil1ium commented 1 year ago

Hi~ @ZBY-creator /external_trigger is generated by visual-inertial sensor. If not supported, local-GNSS time diff may not be calibrate online I think, because time diff needs both GNSS time pulse and local sensor time pulse. As for me, I set gnss_local_online_sync and gnss_local_time_diff to 0 in configure yaml file, which means sensors information is considered to be already synchronized. It might be more inaccurate because of unsync time.

Moreland-cas commented 1 year ago

Hi~ @ZBY-creator I think PPS info hasn't been parsed by ublox_driver, so there might not be local trigger infomation(not really sure...). As for me, I set gnss_local_online_sync and gnss_local_time_diff to 0 in configure yaml file, which means sensors information is considered to be already synchronized. It would introduce some errors caused by unsync time.

thanks! It worked by running ublox sync and settting the gnss_local_online_syn to 0 as you said. Currently I'm working on the GNSSVIAlign() failed problem, gnss_meas_buf[i] turn out to be empty for me.

Abner0907 commented 1 year ago

@maghauke When align gnss, it need a movements, it need above 0.3m/s velocity for aligning the direction(yaw).

SSZ1 commented 1 year ago

Hi~ @ZBY-creator /external_trigger is generated by visual-inertial sensor. If not supported, local-GNSS time diff may not be calibrate online I think, because time diff needs both GNSS time pulse and local sensor time pulse. As for me, I set gnss_local_online_sync and gnss_local_time_diff to 0 in configure yaml file, which means sensors information is considered to be already synchronized. It might be more inaccurate because of unsync time.

Hi~ do you mean the topic 'external_trigger' is not needed if the sensors information is synchronized? And how should I get the topic 'external_trigger' ?

Mil1ium commented 1 year ago

Hi~ @ZBY-creator /external_trigger is generated by visual-inertial sensor. If not supported, local-GNSS time diff may not be calibrate online I think, because time diff needs both GNSS time pulse and local sensor time pulse. As for me, I set gnss_local_online_sync and gnss_local_time_diff to 0 in configure yaml file, which means sensors information is considered to be already synchronized. It might be more inaccurate because of unsync time.

Hi~ do you mean the topic 'external_trigger' is not needed if the sensors information is synchronized? And how should I get the topic 'external_trigger' ?

I think topic is initiated by VI-Sensor and it contains time pulse information of your sensor. The topic features time sync between GNSS and local sensor (time pulse sync). So this function requires the support of your device.

SSZ1 commented 1 year ago

Thanks for your quick response. My device is Realsense D455 and a F9P. I don't think my device has the topic. I set gnss_local_online_sync and gnss_local_time_diff to 0 like you did but it stunk at the initialization like this image And the image bandwide is 30 fps and the GPS is 1 hz. So maybe this also cause the init stunk? Thanks for your patience. And could I have your WeChat or QQ or Email so I can communicate with you more clearly? And My wachat is SSZ97- . Thanks a lot.

SSZ1 commented 1 year ago

Thanks for your quick response. My device is Realsense D455 and a F9P. I don't think my device has the topic. I set gnss_local_online_sync and gnss_local_time_diff to 0 like you did but it stunk at the initialization like this image And the image bandwide is 30 fps and the GPS is 1 hz. So maybe this also cause the init stunk? Thanks for your patience. And could I have your WeChat or QQ or Email so I can communicate with you more clearly? And My wachat is SSZ97- . Thanks a lot.

Hi~ @ZBY-creator /external_trigger is generated by visual-inertial sensor. If not supported, local-GNSS time diff may not be calibrate online I think, because time diff needs both GNSS time pulse and local sensor time pulse. As for me, I set gnss_local_online_sync and gnss_local_time_diff to 0 in configure yaml file, which means sensors information is considered to be already synchronized. It might be more inaccurate because of unsync time.

Hi~ do you mean the topic 'external_trigger' is not needed if the sensors information is synchronized? And how should I get the topic 'external_trigger' ?

I think topic is initiated by VI-Sensor and it contains time pulse information of your sensor. The topic features time sync between GNSS and local sensor (time pulse sync). So this function requires the support of your device.

Thanks for your quick response. My device is Realsense D455 and a F9P. I don't think my device has the topic. I set gnss_local_online_sync and gnss_local_time_diff to 0 like you did but it stunk at the initialization like this image And the image bandwide is 30 fps and the GPS is 1 hz. So maybe this also cause the init stunk? Thanks for your patience. And could I have your WeChat or QQ or Email so I can communicate with you more clearly? And My wachat is SSZ97- . Thanks a lot.

Mil1ium commented 1 year ago

It is impossible to achieve VI-GNSS init at 1Hz of GNSS rate. Check my reply above on 2nd floor.

SSZ1 commented 1 year ago

It is impossible to achieve VI-GNSS init at 1Hz of GNSS rate. Check my reply above on 2nd floor.

All right. Thanks a lot.

SSZ1 commented 1 year ago

I have had this problem. I guess your image fps is higher than 20Hz, for example 30Hz.

In my case, I use Realsense D435i for this project. Firstly, the image fps is set to 30Hz and GNSS output rate is set to 10Hz in u-center, then the GNSS-VI init problem occured. After reading the code, I guess the cause is that each image in slide windows did not get enough GNSS measurements due to fast camera fps(30Hz).

So, the solution is to slow down your camera fps , or increase your GNSS output rate. In my case, I change the GNSS output rate to 16.67Hz(period 60ms) ,and change the condition "10" in the code to a smaller number, for example 6, and the GNSS-VI init problem solved.

And I have one more question. Here it is. How to set the GNSS output rate in the u-center? Any advice ? Thanks.

Mil1ium commented 1 year ago

I have had this problem. I guess your image fps is higher than 20Hz, for example 30Hz. In my case, I use Realsense D435i for this project. Firstly, the image fps is set to 30Hz and GNSS output rate is set to 10Hz in u-center, then the GNSS-VI init problem occured. After reading the code, I guess the cause is that each image in slide windows did not get enough GNSS measurements due to fast camera fps(30Hz). So, the solution is to slow down your camera fps , or increase your GNSS output rate. In my case, I change the GNSS output rate to 16.67Hz(period 60ms) ,and change the condition "10" in the code to a smaller number, for example 6, and the GNSS-VI init problem solved.

And I have one more question. Here it is. How to set the GNSS output rate in the u-center? Any advice ? Thanks.

Check my repository NTRIP_ROS.

SSZ1 commented 1 year ago

I have had this problem. I guess your image fps is higher than 20Hz, for example 30Hz. In my case, I use Realsense D435i for this project. Firstly, the image fps is set to 30Hz and GNSS output rate is set to 10Hz in u-center, then the GNSS-VI init problem occured. After reading the code, I guess the cause is that each image in slide windows did not get enough GNSS measurements due to fast camera fps(30Hz). So, the solution is to slow down your camera fps , or increase your GNSS output rate. In my case, I change the GNSS output rate to 16.67Hz(period 60ms) ,and change the condition "10" in the code to a smaller number, for example 6, and the GNSS-VI init problem solved.

And I have one more question. Here it is. How to set the GNSS output rate in the u-center? Any advice ? Thanks.

Check my repository NTRIP_ROS.

Thanks a lot!

Mil1ium commented 1 year ago

I have had this problem. I guess your image fps is higher than 20Hz, for example 30Hz. In my case, I use Realsense D435i for this project. Firstly, the image fps is set to 30Hz and GNSS output rate is set to 10Hz in u-center, then the GNSS-VI init problem occured. After reading the code, I guess the cause is that each image in slide windows did not get enough GNSS measurements due to fast camera fps(30Hz). So, the solution is to slow down your camera fps , or increase your GNSS output rate. In my case, I change the GNSS output rate to 16.67Hz(period 60ms) ,and change the condition "10" in the code to a smaller number, for example 6, and the GNSS-VI init problem solved.

And I have one more question. Here it is. How to set the GNSS output rate in the u-center? Any advice ? Thanks.

Check my repository NTRIP_ROS.

Thanks a lot!

I think there is one more thing to notice: When your camera frame rate is not 20fps, the value MAX_GNSS_CAMERA_DELAY in estimator_node.cpp should be modified. For example, 0.05 for 20Hz and 0.033 for 30Hz. That is because GNSS factor connects to two key frames k and k+1, so GNSS measurements needs to be between two key frames k and k+1.

See my derivation GnssPsrDoppFactor.