Method SyncData

src/SensorSync.cpp:498–640

    }

    void SyncData()
    {
        while (true)
        {
            /* #region Probing the key buffers ----------------------------------------------------------------------*/

            // Case 0: If any buffer is empty, then loop until all have data
            if (merged_cloud_buf.empty() || ImuEmpty())
            {
                std::this_thread::sleep_for(std::chrono::milliseconds(10));
                continue;
            }

            // Case 1: If latest imu data is still earlier than the earliest
            // feature message, then wait for these measurements to catch up:

            // |___________[mfc]___mfc___mfc____________
            // |_imu_[imu]______________________________
            // |----------------------------------------> t+
            if (merged_cloud_buf.front().endTime > ImuEndTime())
            {
                std::this_thread::sleep_for(std::chrono::milliseconds(10));
                continue;
            }

            // Case 2: If earliest imu data is later than the earliest mfc's
            // end time, then discard the earliest cloud message.
            // This should only occur at the beginning

            // |_[mfc]___mfc___mfc__mfc________________
            // |_____________________[imu]_imu_imu_____
            // |---------------------------------------> t+
            if (merged_cloud_buf.front().endTime < ImuStartTime())
            {
                printf(KYEL "Popping mfc. Start: %.3f, End: %.3f. Size: %d\n" RESET,
                             merged_cloud_buf.front().startTime,
                             merged_cloud_buf.front().endTime,
                             merged_cloud_buf.front().cloud->size());

                merged_cloud_buf_mtx.lock();
                merged_cloud_buf.pop_front();
                merged_cloud_buf_mtx.unlock();
                continue;
            }

            // Case 3: A regular scenario, extract all imu measurement in the scan period

            // |________mfc___mfc___mfc________________
            // |_imu_imu_imu_imu_______________________
            // |---------------------------------------> t+

            // Extract the cloud packet
            CloudPacket merged_cloud = merged_cloud_buf.front();
            merged_cloud_buf_mtx.lock();
            merged_cloud_buf.pop_front();
            merged_cloud_buf_mtx.unlock();

            slict::FeatureCloud msg;