search

chichengcn / gici-open

GNSS/INS/Camera Integrated Navigation Library
GNU General Public License v3.0
399 stars 112 forks source link

gici_main hangs like forever #10

Closed ChanCody closed 9 months ago

ChanCody commented 9 months ago

Thanks for this awesome project! When I execute gici_main with option/pseudo_real_time_estimation_RTK_RRR.yaml, the program seems to run normally at first, but it now gets stuck at this point:

I20230915 16:04:37.551968 7109 data_integration.cpp:301] Waiting for ephemeris. We still have 24 satellites that do not have ephemeris. Total number of satellite is 28 I20230915 16:04:37.653551 7109 data_integration.cpp:301] Waiting for ephemeris. We still have 24 satellites that do not have ephemeris. Total number of satellite is 28 I20230915 16:04:37.706096 7118 feature_handler.cpp:227] Select new keyframe by time duration: 1.1 vs 1 I20230915 16:04:37.755172 7109 data_integration.cpp:301] Waiting for ephemeris. We still have 24 satellites that do not have ephemeris. Total number of satellite is 28 I20230915 16:04:37.867003 7109 data_integration.cpp:301] Waiting for ephemeris. We still have 26 satellites that do not have ephemeris. Total number of satellite is 30 I20230915 16:04:37.963382 7109 data_integration.cpp:301] Waiting for ephemeris. We still have 26 satellites that do not have ephemeris. Total number of satellite is 30 I20230915 16:04:38.059954 7109 data_integration.cpp:301] Waiting for ephemeris. We still have 26 satellites that do not have ephemeris. Total number of satellite is 30 I20230915 16:04:38.161489 7109 data_integration.cpp:301] Waiting for ephemeris. We still have 26 satellites that do not have ephemeris. Total number of satellite is 30 I20230915 16:04:38.257926 7109 data_integration.cpp:301] Waiting for ephemeris. We still have 26 satellites that do not have ephemeris. Total number of satellite is 30 I20230915 16:04:38.359628 7109 data_integration.cpp:301] Waiting for ephemeris. We still have 26 satellites that do not have ephemeris. Total number of satellite is 30 I20230915 16:04:38.461269 7109 data_integration.cpp:301] Waiting for ephemeris. We still have 26 satellites that do not have ephemeris. Total number of satellite is 30 I20230915 16:04:38.563179 7109 data_integration.cpp:301] Waiting for ephemeris. We still have 26 satellites that do not have ephemeris. Total number of satellite is 30 I20230915 16:04:38.659427 7109 data_integration.cpp:301] Waiting for ephemeris. We still have 26 satellites that do not have ephemeris. Total number of satellite is 30 I20230915 16:04:38.760854 7109 data_integration.cpp:301] Waiting for ephemeris. We still have 26 satellites that do not have ephemeris. Total number of satellite is 30

I've been waiting for almost an hour and nothing changes. The data is 1.2 from gici_dataset, I wonder if this is the expected behavior or something goes wrong here.

Appreciate for any help!

chichengcn commented 9 months ago

You might forget to modify the "start_time" options.

The RTCM message does not record GPS week. Hence, if you do not specify the start time, the timestamps of the decoded data will be mismatched.

ChanCody commented 9 months ago

You might forget to modify the "start_time" options.

The RTCM message does not record GPS week. Hence, if you do not specify the start time, the timestamps of the decoded data will be mismatched.

Thanks for your reply! My bad, I do forget to set the "start_time" options. After I set them, gici_main generates the output file as I expected, but it still gets stuck at some point:

I20230918 09:54:09.403123 22171 gnss_estimator_base.h:633] Insufficient satellites! We need at least 8 satellites, but we only have 0! I20230918 09:54:09.405330 22171 imu_estimator_base.cpp:499] Added ZUPT at 1679893915.600000 I20230918 09:54:09.416489 22171 rtk_imu_camera_rrr_estimator.cpp:449] RTK/IMU/Camera RRR: Iterations: 6, Initial cost: 1.565e+01, Final cost: 1.264e+01, Sensor type: 1, Sat number: 14, GDOP: 1.9, Fix status: 3 I20230918 09:54:09.509326 22171 rtk_imu_camera_rrr_estimator.cpp:449] RTK/IMU/Camera RRR: Iterations: 6, Initial cost: 1.052e+01, Final cost: 9.881e+00, Sensor type: 3, Sat number: 14, GDOP: 1.9, Fix status: 3 I20230918 09:54:09.510105 22171 gnss_estimator_base.h:633] Insufficient satellites! We need at least 8 satellites, but we only have 0! I20230918 09:54:09.513250 22171 imu_estimator_base.cpp:499] Added ZUPT at 1679893915.700000 I20230918 09:54:09.524933 22171 rtk_imu_camera_rrr_estimator.cpp:449] RTK/IMU/Camera RRR: Iterations: 6, Initial cost: 1.585e+01, Final cost: 1.220e+01, Sensor type: 1, Sat number: 14, GDOP: 1.9, Fix status: 3

I'm using 1.2 from gici_open_dataset and the "start_time" is set to 2023.03.27 according to the dataset sheet. Is this the correct setting or where can I find the "start_time" of gici_open_dataset?

chichengcn commented 9 months ago

"Insufficient satellites! We need at least 8 satellites, but we only have 0", this is abnormal in the open-source dataset. You can post your configuration file here so that I can check if it has any inappropriate settings.

You can find the start time at https://github.com/chichengcn/gici-open-dataset

ChanCody commented 9 months ago

My configuration file is modified based on option/pseudo_real_time_estimation_RTK_RRR.yaml. I only changed the path of data and the "start time" option. Here are my changes for clarity:

chan@DESKTOP-HFII7MF:~/develop/gici-open$ git diff option/pseudo_real_time_estimation_RTK_RRR.yaml
diff --git a/option/pseudo_real_time_estimation_RTK_RRR.yaml b/option/pseudo_real_time_estimation_RTK_RRR.yaml
index 72cb792..658db44 100755
--- a/option/pseudo_real_time_estimation_RTK_RRR.yaml
+++ b/option/pseudo_real_time_estimation_RTK_RRR.yaml
@@ -6,40 +6,40 @@ stream:
         tag: str_gnss_rov
         output_tags: [fmt_gnss_rov]
         type: file
-        path: <data-directory>/gnss_rover.bin
+        path: /home/chan/data/gici_data/1.2/gnss_rover.bin
     - streamer:
         tag: str_gnss_ref
         output_tags: [fmt_gnss_ref]
         type: file
-        path: <data-directory>/gnss_reference.bin
+        path: /home/chan/data/gici_data/1.2/gnss_reference.bin
     - streamer:
         tag: str_gnss_eph
         output_tags: [fmt_gnss_eph]
         type: file
-        path: <data-directory>/gnss_ephemeris.bin
+        path: /home/chan/data/gici_data/1.2/gnss_ephemeris.bin
     - streamer:
         tag: str_dcb_file
         output_tags: [fmt_dcb_file]
         type: file
-        path: <gici-root-directory>/option/CAS0MGXRAP_20221580000_01D_01D_DCB.BSX
+        path: /home/chan/develop/gici-open/option/CAS0MGXRAP_20221580000_01D_01D_DCB.BSX
         enable_time_tag: false
     - streamer:
         tag: str_imu
         output_tags: [fmt_imu]
         type: file
-        path: <data-directory>/imu.bin
+        path: /home/chan/data/gici_data/1.2/imu.bin
         loop_duration: 0.001
     - streamer:
         tag: str_camera
         output_tags: [fmt_camera]
         type: file
-        path: <data-directory>/camera.bin
+        path: /home/chan/data/gici_data/1.2/camera.bin
         buffer_length: 361472 # 752 * 480 + 512 
     - streamer:
         tag: str_rrr_solution_file
         input_tags: [fmt_rrr_solution_file]
         type: file
-        path: <output-directory>/rtk_rrr_solution.txt
+        path: /home/chan/data/gici_output/rtk_rrr_solution.txt
         enable_time_tag: false
     - streamer:
         tag: str_solution_stream
@@ -114,12 +114,12 @@ stream:
         io: input
         tag: fmt_gnss_ref
         type: gnss-rtcm-3
-        start_time: 2023.03.20
+        start_time: 2023.03.27
     - formator:
         io: input
         tag: fmt_gnss_eph
         type: gnss-rtcm-3
-        start_time: 2023.03.20
+        start_time: 2023.03.27
     - formator:
         io: input
         tag: fmt_imu
@@ -253,7 +253,7 @@ estimate:
                 cols: 1
                 rows: 4
                 data: [-0.36472323465957157, 0.11530151924890532, 0.0006024586154990503, 0.0020184970860498133]
-            mask: <gici-root-directory>/option/gici-mask.png
+            mask: /home/chan/develop/gici-open/option/gici-mask.png
           T_B_C:
             cols: 4
             rows: 4
@@ -266,4 +266,4 @@ logging:
   enable: true
   min_log_level: 0
   log_to_stderr: true
-  file_directory: <log-directory>
+  file_directory: /home/chan/data/gici_output/rtk_rrr_solution.log

And here is the whole file:

# GICI offline pseudo-real-time RTK/IMU/Camera RRR estimation
# You can uncomment the node configuration to publish related topics and observe estimation results (Build GICI in ROS wrapper).
stream:
  streamers:
    - streamer:
        tag: str_gnss_rov
        output_tags: [fmt_gnss_rov]
        type: file
        path: /home/chan/data/gici_data/1.2/gnss_rover.bin
    - streamer:
        tag: str_gnss_ref
        output_tags: [fmt_gnss_ref]
        type: file
        path: /home/chan/data/gici_data/1.2/gnss_reference.bin
    - streamer:
        tag: str_gnss_eph
        output_tags: [fmt_gnss_eph]
        type: file
        path: /home/chan/data/gici_data/1.2/gnss_ephemeris.bin
    - streamer:
        tag: str_dcb_file
        output_tags: [fmt_dcb_file]
        type: file
        path: /home/chan/develop/gici-open/option/CAS0MGXRAP_20221580000_01D_01D_DCB.BSX
        enable_time_tag: false
    - streamer:
        tag: str_imu
        output_tags: [fmt_imu]
        type: file
        path: /home/chan/data/gici_data/1.2/imu.bin
        loop_duration: 0.001
    - streamer:
        tag: str_camera
        output_tags: [fmt_camera]
        type: file
        path: /home/chan/data/gici_data/1.2/camera.bin
        buffer_length: 361472 # 752 * 480 + 512 
    - streamer:
        tag: str_rrr_solution_file
        input_tags: [fmt_rrr_solution_file]
        type: file
        path: /home/chan/data/gici_output/rtk_rrr_solution.txt
        enable_time_tag: false
    - streamer:
        tag: str_solution_stream
        input_tags: [fmt_solution_stream]
        type: tcp-server
        port: 9001
    # - streamer:
    #     tag: str_ros_imu
    #     input_tags: [fmt_imu]
    #     type: ros
    #     topic_name: imu_raw
    #     queue_size: 10
    #     io: log
    #     format: imu
    # - streamer:
    #     tag: str_ros_camera
    #     input_tags: [fmt_camera]
    #     type: ros
    #     topic_name: image_raw
    #     queue_size: 10
    #     io: log
    #     format: image
    # - streamer:
    #     tag: str_solution
    #     type: ros
    #     topic_name: solution
    #     queue_size: 5
    #     io: output
    #     format: pose_stamped
    # - streamer:
    #     tag: str_solution_odometry
    #     type: ros
    #     topic_name: solution_odometry
    #     queue_size: 10
    #     io: output
    #     format: odometry
    #     subframe_id: Body
    # - streamer:
    #     tag: str_solution_path
    #     type: ros
    #     topic_name: solution_path
    #     queue_size: 100
    #     io: output
    #     format: path
    # - streamer:
    #     tag: str_featured_image
    #     type: ros
    #     topic_name: featured_image
    #     queue_size: 3
    #     io: output
    #     format: image
    # - streamer:
    #     tag: str_landmarks
    #     type: ros
    #     topic_name: landmarks
    #     queue_size: 3
    #     marker_scale: 0.2
    #     io: output
    #     format: marker

  formators:
    - formator:
        io: input
        tag: fmt_dcb_file
        type: dcb-file
    - formator:
        io: input
        tag: fmt_gnss_rov
        type: gnss-raw
        sub_type: tersus
    - formator:
        io: input
        tag: fmt_gnss_ref
        type: gnss-rtcm-3
        start_time: 2023.03.27
    - formator:
        io: input
        tag: fmt_gnss_eph
        type: gnss-rtcm-3
        start_time: 2023.03.27
    - formator:
        io: input
        tag: fmt_imu
        type: imu-pack
    - formator:
        io: input
        tag: fmt_camera
        width: 752
        height: 480
        type: image-pack
    - formator:
        io: output
        tag: fmt_rrr_solution_file
        type: nmea 
        use_esa: true
        talker_id: GP
    - formator:
        io: output
        tag: fmt_solution_stream
        type: nmea 
        use_esa: true
        use_esd: true
        talker_id: GP

  replay:
    enable: true
    speed: 1.0
    start_offset: 0.0

estimate:
- estimator:
    tag: est_rtk_imu_camera_rrr
    type: rtk_imu_camera_rrr
    input_tags: [fmt_gnss_rov, fmt_gnss_ref, fmt_gnss_eph, fmt_dcb_file, fmt_camera, fmt_imu]
    fmt_gnss_rov_roles: [rover]
    fmt_gnss_ref_roles: [reference]
    fmt_gnss_eph_roles: [ephemeris]
    fmt_dcb_file_roles: [code_bias]
    fmt_camera_roles: [mono]
    fmt_imu_roles: [major]
    output_tags: [fmt_rrr_solution_file, fmt_solution_stream]
    # output_tags: [fmt_rrr_solution_file, fmt_solution_stream,　str_solution, str_solution_odometry, str_solution_path, str_featured_image, str_landmarks]
    output_align_tag: fmt_imu
    output_downsample_rate: 40
    compute_covariance: false        
    enable_input_align: true
    input_align_latency: 0.2
    enable_backend_data_sparsify: true
    pending_num_threshold: 5
    rtk_imu_camera_rrr_options:
      max_keyframes: 3
      min_yaw_std_init_visual: 0.5
    rtk_options:
      use_ambiguity_resolution: true
    gnss_estimator_base_options:
      use_outlier_rejection: true
      reject_one_outlier_once: false
      max_pesudorange_error: 4.0          
      max_phaserange_error: 0.06         
      max_doppler_error: 0.5
      gnss_common:
        min_elevation: 12.0
        min_SNR: [35.0, 30.0]
    gnss_loose_estimator_base_options:
    visual_estimator_base_options:
      feature_error_std: 10.0
      stable_feature_error_std: 1.0
      min_observation_stable: 20
      landmark_outlier_rejection_threshold: 2.0
    imu_estimator_base_options:
      imu_parameters:
        sigma_bg: 1.0e-3
        sigma_ba: 1.0e-2
        sigma_g_c: 2.6700e-04
        sigma_a_c: 0.0112
        sigma_gw_c: 2.0881e-06
        sigma_aw_c: 2.7361e-04
      body_to_imu_rotation: [-90.0, -90.0, 0.0]
      body_to_imu_rotation_std: 5.0
      car_motion: true
      car_motion_min_velocity: 3.0
      car_motion_max_anguler_velocity: 5.0
    estimator_base_options:
      max_iteration: 5
      num_threads: 4
      max_solver_time: 0.03
      verbose_output: true
      solver_type: dense_schur
      trust_region_strategy_type: dogleg
    ambiguity_resolution_options:
      system_exclude: [R]
      min_percentage_fixation_uwl: 1.0
      min_percentage_fixation_wl: 0.9
      min_percentage_fixation_nl: 0.9
      ratio: 2.0
    gnss_imu_initializer_options:
      max_iteration: 30
      num_threads: 4
      max_solver_time: 0.5
      gnss_extrinsics: [-0.035, 0.354, -0.042]
      gnss_extrinsics_initial_std: [0.0, 0.0, 0.0]
      time_window_length_slow_motion: 0.05
      time_window_length_dynamic_motion: 0.5
      min_acceleration: 0.5
    feature_handler_options:
      max_features_per_frame: 100
      max_n_kfs: 30
      kfselect_min_numkfs: 30
      kfselect_min_disparity: 10.0
      kfselect_min_dist_metric: 0.5
      kfselect_min_angle: 5.0  
      kfselect_min_dt: 1.0
      detector:
        cell_size: 40
      initialization:
      camera_parameters:
        label: "gici-board"
        cameras:
        - camera:
            label: cam0
            image_width: 752
            image_height: 480
            type: pinhole
            intrinsics:
              cols: 1
              rows: 4
              data: [510.12091726559544, 509.55338413343736, 357.48596105253193, 240.00309579253124]
            distortion:
              type: radial-tangential  
              parameters:
                cols: 1
                rows: 4
                data: [-0.36472323465957157, 0.11530151924890532, 0.0006024586154990503, 0.0020184970860498133]
            mask: /home/chan/develop/gici-open/option/gici-mask.png
          T_B_C:
            cols: 4
            rows: 4
            data: [0.0134381210697455122,   -0.999901594987671172, -0.00402706384669154413,  -0.0229793554058591656,
                  0.999907681540912807,   0.0134460859659659704, -0.00195733688253543802,   0.0110786309679912626,
                  0.00201129251744842832, -0.00400038914436078377,     0.99998997574430859,    0.025008868367930974,
                                      0,                       0,                       0,                       1]

logging:
  enable: true
  min_log_level: 0
  log_to_stderr: true
  file_directory: /home/chan/data/gici_output/rtk_rrr_solution.log
chichengcn commented 9 months ago

The "Insufficient satellites" messages appear only at the end, because the GNSS receiver was shut down before the camera and IMU.

ChanCody commented 9 months ago

The "Insufficient satellites" messages appear only at the end, because the GNSS receiver was shut down before the camera and IMU.

Thanks! So this behavior is as expected, and the output file is ready for evaluation, right? Here is the start and end of my output file:

$GPRMC,050931.639,A,3106.1475302,N,12126.7349972,E,8.60,212.78,270323,0.0,E,R,V*68
$GPGGA,050931.639,3106.1475302,N,12126.7349972,E,5,28,1.0,5.950,M,11.053,M,14745822.0,0000*4A
$GPESA,050931.639,-0.961,+4.317,-0.073,+0.603,+1.196,+11.806*4F
......
$GPRMC,051155.853,A,3106.5181719,N,12126.9740038,E,0.01,283.33,270323,0.0,E,R,V*69
$GPGGA,051155.853,3106.5181719,N,12126.9740038,E,5,14,1.0,5.536,M,11.063,M,14745822.0,0000*43
$GPESA,051155.853,+0.001,-0.005,+0.004,+0.159,+1.322,-81.955*46
$GPRMC,051155.953,A,3106.5181721,N,12126.9740028,E,0.00,283.33,270323,0.0,E,R,V*63
$GPGGA,051155.953,3106.5181721,N,12126.9740028,E,5,14,1.0,5.536,M,11.063,M,14745822.0,0000*48
$GPESA,051155.953,-0.001,-0.001,+0.001,+0.164,+1.323,-81.954*4E

It has 4329 valid lines. Is this the same as yours?

chichengcn commented 9 months ago

Yes.