Trying to implement own OnlineDataProvider

[valar-doharis]

I'm having a similar problem to (Issue #150), but because I'm trying to make my own DataProvider implementation that will provide webcam data and generated IMU data. (In the future I'll replace this with real IMU and camera data).

The frames are being captured, but for some reason the queue size has stopped at 1.

 Launching:

            ██╗  ██╗██╗███╗   ███╗███████╗██████╗  █████╗
            ██║ ██╔╝██║████╗ ████║██╔════╝██╔══██╗██╔══██╗
            █████╔╝ ██║██╔████╔██║█████╗  ██████╔╝███████║
            ██╔═██╗ ██║██║╚██╔╝██║██╔══╝  ██╔══██╗██╔══██║
            ██║  ██╗██║██║ ╚═╝ ██║███████╗██║  ██║██║  ██║
            ╚═╝  ╚═╝╚═╝╚═╝     ╚═╝╚══════╝╚═╝  ╚═╝╚═╝  ╚═╝

 
I1111 16:46:16.271548 40518 RegularVioBackend.cpp:115] Using Regular VIO Backend.
I1111 16:46:16.291844 40518 PipelineModule.h:451] MISO Pipeline Module: Display has no output queue registered.
I1111 16:46:16.292249 40518 Pipeline.cpp:352] Pipeline Modules launched (parallel_run set to 1).
I1111 16:46:16.292402 40545 Pipeline.cpp:102] Spinning Kimera-VIO.
I1111 16:46:16.292451 40546 Pipeline.cpp:167] Shutting down VIO pipeline once processing has finished.
I1111 16:46:16.292560 40546 Pipeline.cpp:180] 
I1111 16:46:16.793032 40546 Pipeline.cpp:180] 
I1111 16:46:16.885535 40544 CustomDataProvider.cpp:45] 0
I1111 16:46:16.980305 40544 CustomDataProvider.cpp:47] 0
I1111 16:46:17.293622 40546 Pipeline.cpp:180] Statistics
-----------	                                       #	log hz	{avg +- std}	[min, max]	last
data_provider_left_frame_queue Size [#]    	         1	1.4	{1.0 +- 0.0}	[1.0, 1.0]	1.0
data_provider_right_frame_queue Size [#]   	         1	1.4	{1.0 +- 0.0}	[1.0, 1.0]	1.0
I1111 16:46:17.794996 40546 Pipeline.cpp:180] Statistics
-----------	                                       #	log hz	{avg +- std}	[min, max]	last
data_provider_left_frame_queue Size [#]    	         1	1.4	{1.0 +- 0.0}	[1.0, 1.0]	1.0
data_provider_right_frame_queue Size [#]   	         1	1.4	{1.0 +- 0.0}	[1.0, 1.0]	1.0
I1111 16:46:18.295699 40546 Pipeline.cpp:180] Statistics

....
and so on

Here is the code of my OnlineDataProvider:

#include "kimera-vio/dataprovider/CustomDataProvider.h"
#include <chrono>
#include <glog/logging.h>

namespace VIO {

CustomDataProvider::CustomDataProvider() {
  if (!initializeWebcam()) {
    LOG(ERROR) << "Failed to initialize webcam.";
    shutdown_ = true;
  }
}

CustomDataProvider::~CustomDataProvider() {
  LOG(INFO) << "CustomDataProvider destructor called.";
  if (webcam_.isOpened()) {
    webcam_.release();
  }
}

bool CustomDataProvider::initializeWebcam() {
  webcam_.open(0, cv::CAP_V4L2); // Open default camera
  if (!webcam_.isOpened()) {
    LOG(ERROR) << "Webcam could not be opened!";
    return false;
  }
  return true;
}

bool CustomDataProvider::spin() {

  
  if (shutdown_) {
    LOG(INFO) << "CustomDataProvider spin stopped due to shutdown request.";
    return false;
  }

  if (!webcam_.isOpened()) {
    LOG(ERROR) << "Webcam not open, cannot spin CustomDataProvider.";
    return false;
  }
    
    cv::Mat left_image, right_image;
    webcam_ >> left_image;
    LOG(INFO) << left_image.empty();
    webcam_ >> right_image;
    LOG(INFO) << right_image.empty();

  if (!left_image.empty() && !right_image.empty()) {
    Timestamp timestamp = std::chrono::duration_cast<std::chrono::microseconds>(
                                      std::chrono::system_clock::now().time_since_epoch())
                                      .count();

    auto left_frame = std::make_unique<Frame>(timestamp, timestamp, CameraParams(), left_image);
    auto right_frame = std::make_unique<Frame>(timestamp, timestamp, CameraParams(), right_image);

    if (left_frame_callback_) left_frame_callback_(std::move(left_frame));
    if (right_frame_callback_) right_frame_callback_(std::move(right_frame));
  } else {
    LOG(ERROR) << "Captured empty frame.";
    return false;
  }

  ImuMeasurement imu_measurement = generateRandomImuData();

  imu_single_callback_(imu_measurement);

  return true;
}

bool CustomDataProvider::hasData() const {
  return !shutdown_;
}

void CustomDataProvider::shutdown() {
  LOG_IF(ERROR, shutdown_) << "Shutdown requested, but CustomDataProvider was already shutdown.";
  LOG(INFO) << "Shutting down CustomDataProvider.";
  shutdown_ = true;
  if (webcam_.isOpened()) {
    webcam_.release();
  }
}

ImuMeasurement CustomDataProvider::generateRandomImuData() {
  static std::default_random_engine generator;
  static std::normal_distribution<double> distribution(0.0, 1.0);

  Vector3 gyro(distribution(generator), distribution(generator), distribution(generator));
  Vector3 accel(distribution(generator), distribution(generator), distribution(generator));
  Timestamp timestamp = std::chrono::duration_cast<std::chrono::microseconds>(
                            std::chrono::system_clock::now().time_since_epoch())
                            .count();

  ImuAccGyr imu_data;  // Create a 6D vector for IMU data
  imu_data << accel, gyro;  // Concatenate accel and gyro into imu_data

  return ImuMeasurement(timestamp, imu_data);
}

}  // namespace VIO

And the code for its use:

#include <gflags/gflags.h>
#include <glog/logging.h>

#include <chrono>
#include <future>
#include <memory>
#include <utility>

#include "kimera-vio/dataprovider/CustomDataProvider.h" // Include custom data provider
#include "kimera-vio/frontend/StereoImuSyncPacket.h"
#include "kimera-vio/logging/Logger.h"
#include "kimera-vio/pipeline/MonoImuPipeline.h"
#include "kimera-vio/pipeline/Pipeline.h"
#include "kimera-vio/pipeline/StereoImuPipeline.h"
#include "kimera-vio/utils/Statistics.h"
#include "kimera-vio/utils/Timer.h"

DEFINE_string(
    params_folder_path,
    "../params/Euroc",
    "Path to the folder containing the yaml files with the VIO parameters.");

int main(int argc, char* argv[]) {
  // Initialize Google's flags library.
  google::ParseCommandLineFlags(&argc, &argv, true);
  // Initialize Google's logging library.
  google::InitGoogleLogging(argv[0]);

  // Parse VIO parameters from gflags.
  VIO::VioParams vio_params(FLAGS_params_folder_path);

  // Instantiate the CustomDataProvider
  VIO::DataProviderInterface::Ptr data_provider = std::make_unique<VIO::CustomDataProvider>();

  VIO::Pipeline::Ptr vio_pipeline;

  // Initialize the VIO pipeline based on the frontend type
  switch (vio_params.frontend_type_) {
    case VIO::FrontendType::kMonoImu: {
      vio_pipeline = std::make_unique<VIO::MonoImuPipeline>(vio_params);
    } break;
    case VIO::FrontendType::kStereoImu: {
      vio_pipeline = std::make_unique<VIO::StereoImuPipeline>(vio_params);
    } break;
    default: {
      LOG(FATAL) << "Unrecognized Frontend type: "
                 << VIO::to_underlying(vio_params.frontend_type_)
                 << ". 0: Mono, 1: Stereo.";
    } break;
  }

  // Register shutdown callback
  vio_pipeline->registerShutdownCallback(
      std::bind(&VIO::DataProviderInterface::shutdown, data_provider));

  // Register data callbacks for IMU and frames
  data_provider->registerImuSingleCallback(
      std::bind(&VIO::Pipeline::fillSingleImuQueue, vio_pipeline, std::placeholders::_1));

  data_provider->registerLeftFrameCallback(
      std::bind(&VIO::Pipeline::fillLeftFrameQueue, vio_pipeline, std::placeholders::_1));


  if (vio_params.frontend_type_ == VIO::FrontendType::kStereoImu) {
    auto stereo_pipeline = std::dynamic_pointer_cast<VIO::StereoImuPipeline>(vio_pipeline);
    CHECK(stereo_pipeline);

    data_provider->registerRightFrameCallback(
        std::bind(&VIO::StereoImuPipeline::fillRightFrameQueue, stereo_pipeline, std::placeholders::_1));
  }


  // Start the data provider and VIO pipeline in parallel or sequentially
  auto tic = VIO::utils::Timer::tic();
  bool is_pipeline_successful = false;
  if (vio_params.parallel_run_) {
    auto handle = std::async(
        std::launch::async, &VIO::DataProviderInterface::spin, data_provider);
    auto handle_pipeline =
        std::async(std::launch::async, &VIO::Pipeline::spin, vio_pipeline);
    auto handle_shutdown = std::async(
        std::launch::async,
        &VIO::Pipeline::waitForShutdown,
        vio_pipeline,
        [&data_provider]() -> bool { return !data_provider->hasData(); },
        500,
        true);
    vio_pipeline->spinViz();
    is_pipeline_successful = !handle.get();
    handle_shutdown.get();
    handle_pipeline.get();
  } else {
    while (data_provider->spin() && vio_pipeline->spin()) {
      continue;
    };
    vio_pipeline->shutdown();
    is_pipeline_successful = true;
  }

  // Output stats
  auto spin_duration = VIO::utils::Timer::toc(tic);
  LOG(WARNING) << "Spin took: " << spin_duration.count() << " ms.";
  LOG(INFO) << "Pipeline successful? "
            << (is_pipeline_successful ? "Yes!" : "No!");

  if (is_pipeline_successful) {
    VIO::PipelineLogger logger;
    logger.logPipelineOverallTiming(spin_duration);
  }

  return is_pipeline_successful ? EXIT_SUCCESS : EXIT_FAILURE;
}