marginalization_error - Githubissues

ChanCody commented 9 months ago

I'm getting this error randomly when I run ./build/gici_main option/pseudo_real_time_estimation_RTK_RRR.yaml. I'm using the data 4.2 from gici-open-dataset. By randomly, I mean the "id" in the following log changes every time.
The "id" can be 3250486229121942272, 3250486229112914688, 3250486229100626688, ...

I20230918 19:44:23.890939  9368 rtk_imu_camera_rrr_estimator.cpp:449] RTK/IMU/Camera RRR: Iterations: 6, Initial cost: 1.021e+02, Final cost: 1.005e+02, Sensor type: 3, Sat number:  8, GDOP: 4.3, Fix status: 3
I20230918 19:44:23.958727  9366 feature_handler.cpp:208] Select new keyframe by motivation: angle = 0.060711, distance = 0.589349
I20230918 19:44:23.959527  9368 gnss_estimator_base.h:633] Insufficient satellites! We need at least 8 satellites, but we only have 6!
I20230918 19:44:23.979460  9368 gnss_estimator_base.cpp:1584] Rejected pseudorange outlier with ambiguities at E15|7Q&7Q-E27|7Q&7Q: residual = 6.146627
I20230918 19:44:23.979605  9368 rtk_imu_camera_rrr_estimator.cpp:449] RTK/IMU/Camera RRR: Iterations: 6, Initial cost: 4.827e+02, Final cost: 1.352e+02, Sensor type: 1, Sat number:  8, GDOP: 4.3, Fix status: 3
F20230918 19:44:23.982164  9368 marginalization_error.cpp:606] Check failed: it != parameter_block_id_to_parameter_block_info_idx_.end() trying to marginalize out unconnected parameter block id = 3250486229121942272
*** Check failure stack trace: ***
    @     0x7f03c7c66e6e  google::LogMessage::Fail()
    @     0x7f03c7c66db6  google::LogMessage::SendToLog()
    @     0x7f03c7c665de  google::LogMessage::Flush()
    @     0x7f03c7c6a357  google::LogMessageFatal::~LogMessageFatal()
    @     0x7f03c872e748  gici::MarginalizationError::marginalizeOut()
    @     0x7f03c86f754c  gici::EstimatorBase::applyMarginalization()
    @     0x7f03c879685c  gici::RtkImuCameraRrrEstimator::gnssMarginalization()
    @     0x7f03c879bfac  gici::RtkImuCameraRrrEstimator::estimate()
    @     0x7f03c878af7e  gici::MultiSensorEstimating::processEstimator()
    @     0x7f03c878b690  gici::MultiSensorEstimating::runBackend()
    @     0x7f03c7983c80  (unknown)
    @     0x7f03c81156ba  start_thread
    @     0x7f03c73f251d  clone
    @              (nil)  (unknown)
Aborted

Is there something wrong with my configuration? I only changed the data path and start_time. Here is the yaml file I'm using:

# 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/4.2/gnss_rover.bin
    - streamer:
        tag: str_gnss_ref
        output_tags: [fmt_gnss_ref]
        type: file
        path: /home/chan/data/gici_data/4.2/gnss_reference.bin
    - streamer:
        tag: str_gnss_eph
        output_tags: [fmt_gnss_eph]
        type: file
        path: /home/chan/data/gici_data/4.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/4.2/imu.bin
        loop_duration: 0.001
    - streamer:
        tag: str_camera
        output_tags: [fmt_camera]
        type: file
        path: /home/chan/data/gici_data/4.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/4.2.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/4.2.log

Appriciate for any help!

chichengcn commented 9 months ago

Thanks for pointing it out. This is a BUG caused by our latest revision. We have fixed this BUG and uploaded the modified codes.

There are no mistake on your configuration file. Just re-download the latest codes.

ChanCody commented 9 months ago

I've pulled the latest codes, but the error still happens. Is this related to the " Insufficient satellites! We need at least 8 satellites, but we only have 6!" warning?

I20230919 09:24:32.619479 16317 rtk_imu_camera_rrr_estimator.cpp:449] RTK/IMU/Camera RRR: Iterations: 6, Initial cost: 9.936e+01, Final cost: 9.801e+01, Sensor type: 3, Sat number:  8, GDOP: 4.3, Fix status: 3
I20230919 09:24:32.690994 16315 feature_handler.cpp:208] Select new keyframe by motivation: angle = 0.0607363, distance = 0.569074
I20230919 09:24:32.691722 16317 gnss_estimator_base.h:633] Insufficient satellites! We need at least 8 satellites, but we only have 6!
I20230919 09:24:32.709928 16317 gnss_estimator_base.cpp:1584] Rejected pseudorange outlier with ambiguities at G03|5Q&5Q-G06|5Q&5Q: residual = -4.481940
I20230919 09:24:32.710001 16317 gnss_estimator_base.cpp:1584] Rejected pseudorange outlier with ambiguities at E15|7Q&7Q-E27|7Q&7Q: residual = 8.212878
I20230919 09:24:32.710157 16317 rtk_imu_camera_rrr_estimator.cpp:449] RTK/IMU/Camera RRR: Iterations: 6, Initial cost: 3.277e+02, Final cost: 1.285e+02, Sensor type: 1, Sat number:  8, GDOP: 4.3, Fix status: 3
F20230919 09:24:32.712699 16317 marginalization_error.cpp:606] Check failed: it != parameter_block_id_to_parameter_block_info_idx_.end() trying to marginalize out unconnected parameter block id = 3250486229122319104
*** Check failure stack trace: ***
    @     0x7fecfd668e6e  google::LogMessage::Fail()
    @     0x7fecfd668db6  google::LogMessage::SendToLog()
    @     0x7fecfd6685de  google::LogMessage::Flush()
    @     0x7fecfd66c357  google::LogMessageFatal::~LogMessageFatal()
    @     0x7fecfe130748  gici::MarginalizationError::marginalizeOut()
    @     0x7fecfe0f954c  gici::EstimatorBase::applyMarginalization()
    @     0x7fecfe19885c  gici::RtkImuCameraRrrEstimator::gnssMarginalization()
    @     0x7fecfe19dfac  gici::RtkImuCameraRrrEstimator::estimate()
    @     0x7fecfe18cf7e  gici::MultiSensorEstimating::processEstimator()
    @     0x7fecfe18d690  gici::MultiSensorEstimating::runBackend()
    @     0x7fecfd385c80  (unknown)
    @     0x7fecfdb176ba  start_thread
    @     0x7fecfcdf451d  clone
    @              (nil)  (unknown)
Aborted

chichengcn commented 7 months ago

We have tested the new codes on two different computers and the error did not happen. We suggest you clone the codes to a new folder and compile them, in case you did not merge the codes correctly.

chichengcn / gici-open

marginalization_error #11