Add kimera spectacular data provider

WORKSPACE

@@ -522,7 +522,8 @@ http_archive(
   strip_prefix="opengv-91f4b19c73450833a40e463ad3648aae80b3a7f3",
   build_file="//third_party:BUILD.opengv",
   patch_args = ["-p1"],
-  patches = ["//third_party:opengv_0001-prefix-unsupported-eigen-include-paths.patch"],
+  patches = ["//third_party:opengv_0001-prefix-unsupported-eigen-include-paths.patch",
+             "//third_party:opengv_0002-unused-parameter.patch"],
   integrity = "sha256-gIK3IvE6rGDpxvN3BA+EPFKPvZ7Zi7Ix33IIcm2RULA="
 )
 
@@ -539,12 +540,7 @@ http_archive(
   urls = ["https://github.com/ewfuentes/Kimera-VIO/archive/master.zip"],
   strip_prefix = "Kimera-VIO-master",
   build_file = "//third_party:BUILD.kimera_vio",
-  integrity = "sha256-8fLHPn8JPWffnnXEA0gJbZrBhoGpK0Gw80I3oWXpgZg=",
-  patch_args = ["-p1"],
-  patches = [
-    "//third_party:kimera_vio_0001-add-missing-includes.patch",
-    "//third_party:kimera_vio_0002-add-override-markers.patch",
-  ]
+  integrity = "sha256-+hQBAPv4bDRbYnH1iXeuHTNsWnfXgqZ0FoUTv6usQ/k=",
 )
 
 http_archive(
@@ -556,9 +552,9 @@ http_archive(
 
 http_archive(
   name = "spectacular_log_snippet",
-  urls = ["https://www.dropbox.com/scl/fi/2u4ec000kx24980t7ifjz/recording_2024-11-21_13-36-30.zip?rlkey=ojw96yykmw9crtqs15kbxwnvp&st=gjcrawhw&dl=1"],
+  urls = ["https://www.dropbox.com/scl/fi/6y2x4nstw7h3xx6jvbzzg/20241212_150605.zip?rlkey=rwktg7egzki7vqeq9qpfaml1l&dl=1"],
   build_file = "//third_party:BUILD.zip_file",
-  integrity = "sha256-K+KFNzufwccL4vEJLErEtNKNVjnWStClXVF2mKpI6lI="
+  integrity = "sha256-7WD3AyKE+v7fA6ohAqk1UXBU+i2n9uH/6HcovNhORhA="
 )
 
 http_archive(

common/video/image_compare.cc

@@ -1,4 +1,4 @@
-#include "opencv2/opencv.hpp"
+#include "common/video/image_compare.hh"
 
 namespace robot::common::video {
 
@@ -7,8 +7,17 @@ bool images_equal(const cv::Mat& img1, const cv::Mat& img2) {
         return false;
     }
     cv::Mat diff;
-    cv::absdiff(img1, img2, diff);
+    if (img1.type() == CV_32F) {
+        cv::Mat img1_no_nan = img1;
+        cv::Mat img2_no_nan = img2;
+        cv::patchNaNs(img1_no_nan);
+        cv::patchNaNs(img2_no_nan);
+        cv::absdiff(img1_no_nan, img2_no_nan, diff);
+    } else {
+        cv::absdiff(img1, img2, diff);
+    }
     diff = diff.reshape(1);
+
     return cv::countNonZero(diff) == 0;
 }
 

common/video/image_compare.hh

@@ -6,4 +6,4 @@ namespace robot::common::video {
 
 bool images_equal(const cv::Mat& img1, const cv::Mat& img2);
 
-}
+}

experimental/overhead_matching/BUILD

@@ -1,5 +1,3 @@
-
-
 package(features=["warning_compile_flags"])
 
 load("@pybind11_bazel//:build_defs.bzl", "pybind_extension")
@@ -31,6 +29,7 @@ cc_test(
 	":spectacular_log",
 	"@com_google_googletest//:gtest_main",
 	"@fmt",
+    "//common/video:image_compare",
     ]
 )
 
@@ -50,3 +49,27 @@ py_binary(
 	requirement("numpy"),
     ],
 )
+
+
+cc_library(
+    name = "kimera_spectacular_data_provider",
+    hdrs = ["kimera_spectacular_data_provider.hh"],
+    srcs = ["kimera_spectacular_data_provider.cc"],
+    copts = ["-Wno-sign-compare", "-Wno-vla"],
+    deps = [
+        "@kimera_vio",
+        ":spectacular_log",
+    ]
+)
+cc_test(
+    name = "kimera_spectacular_data_provider_test",
+    srcs = ["kimera_spectacular_data_provider_test.cc"],
+    data = ["@spectacular_log_snippet//:files"],
+    copts = ["-Wno-sign-compare", "-Wno-vla"],
+    deps = [
+        ":kimera_spectacular_data_provider",
+        "@com_google_googletest//:gtest_main",
+        "@fmt",
+        "//common/video:image_compare",
+    ]
+)

experimental/overhead_matching/kimera_spectacular_data_provider.cc

@@ -0,0 +1,166 @@
+#include "experimental/overhead_matching/kimera_spectacular_data_provider.hh"
+
+#include <opencv2/core/core.hpp>
+#include <string>
+#include <vector>
+
+#include "kimera-vio/imu-frontend/ImuFrontend-definitions.h"
+#include "kimera-vio/logging/Logger.h"
+
+namespace robot::experimental::overhead_matching {
+
+VIO::Timestamp vio_time_from_robot_time(const time::RobotTimestamp& robot_time) {
+    /// VIO timestamp is int64_t nanoseconds. time::RobotTimestamp is std::nano
+    return robot_time.time_since_epoch().count();
+}
+
+/* -------------------------------------------------------------------------- */
+SpectacularDataProviderInterface::SpectacularDataProviderInterface(const std::string& dataset_path,
+                                                                   const int initial_k,
+                                                                   const int final_k,
+                                                                   const VIO::VioParams& vio_params)
+    : DataProviderInterface(),
+      vio_params_(vio_params),
+      dataset_path_(dataset_path),
+      current_k_(initial_k),
+      final_k_(final_k),
+      spec_log_(dataset_path) {
+    ROBOT_CHECK(!dataset_path_.empty(), "Dataset path for SpectacularDataProvider is empty.");
+
+    // Start processing dataset from frame initial_k.
+    // Useful to skip a bunch of images at the beginning (imu calibration).
+    ROBOT_CHECK(current_k_ >= 0);
+
+    // Finish processing dataset at frame final_k.
+    // Last frame to process (to avoid processing the entire dataset),
+    // skip last frames.
+
+    if (final_k_ > spec_log_.num_frames()) {
+        LOG(WARNING) << "Provided final frame for KimeraSpectacularDataProvider was above the "
+                        "number of frames. Reducing to match size of dataset";
+        final_k_ = spec_log_.num_frames();
+    }
+    ROBOT_CHECK(final_k_ <= spec_log_.num_frames());
+
+    ROBOT_CHECK(final_k_ > current_k_,
+                "Value for final_k is smaller than value for current_k (initial value)", final_k_,
+                current_k_);
+}
+
+SpectacularDataProviderInterface::~SpectacularDataProviderInterface() {
+    LOG(INFO) << "SpectacularDataProviderInterface destructor called.";
+}
+
+bool SpectacularDataProviderInterface::spin() {
+    if (!is_imu_data_sent_) {
+        // First, send all the IMU data. The flag is to avoid sending it several
+        // times if we are running in sequential mode.
+        if (imu_single_callback_) {
+            send_imu_data();
+        } else {
+            LOG(ERROR) << "Imu callback not registered! Not sending IMU data.";
+        }
+        is_imu_data_sent_ = true;
+    }
+
+    // Spin.
+    // We log only the first one, because we may be running in sequential mode.
+    LOG_FIRST_N(INFO, 1) << "Running dataset between frame " << current_k_ << " and frame "
+                         << final_k_;
+    while (!shutdown_ && spin_once()) {
+        if (!vio_params_.parallel_run_) {
+            // Return, instead of blocking, when running in sequential mode.
+            return true;
+        }
+    }
+    LOG_IF(INFO, shutdown_) << "shutdown requested.";
+    return false;
+}
+
+bool SpectacularDataProviderInterface::hasData() const { return current_k_ < final_k_; }
+
+/* -------------------------------------------------------------------------- */
+bool SpectacularDataProviderInterface::spin_once() {
+    if (current_k_ >= final_k_) {
+        LOG(INFO) << "Finished spinning dataset.";
+        return false;
+    }
+    // TODO: How do we want to handle camera parameters?
+    //  const VIO::CameraParams& left_cam_info = vio_params_.camera_params_.at(0);
+    const VIO::CameraParams left_cam_FAKE_PARAMS;
+    const bool equalize_image = false;
+    // vio_params_.frontend_params_.stereo_matching_params_.equalize_image_;
+
+    std::optional<FrameGroup> maybe_frame = spec_log_.get_frame(current_k_);
+
+    const VIO::Timestamp timestamp_frame_k =
+        vio_time_from_robot_time(maybe_frame->time_of_validity);
+    // LOG(INFO) << "Sending left frames k= " << current_k_ << " with timestamp: " <<
+    // timestamp_frame_k;
+
+    cv::Mat& bgr = maybe_frame->bgr_frame;
+    if (bgr.channels() > 1) {
+        //   LOG(INFO) << "Converting img from BGR to GRAY...";
+        cv::cvtColor(bgr, bgr, cv::COLOR_BGR2GRAY);
+    }
+    if (equalize_image) {
+        LOG(WARNING) << "- Histogram Equalization for image";
+        cv::equalizeHist(bgr, bgr);
+    }
+
+    cv::Mat& depth_meters = maybe_frame->depth_frame;
+    ROBOT_CHECK(depth_meters.type() == CV_32FC1);  // type assumed if depth is distance in meters
+
+    left_frame_callback_(
+        std::make_unique<VIO::Frame>(current_k_, timestamp_frame_k, left_cam_FAKE_PARAMS, bgr));
+
+    depth_frame_callback_(
+        std::make_unique<VIO::DepthFrame>(current_k_, timestamp_frame_k, depth_meters));
+
+    // LOG(INFO) << "Finished VIO processing for frame k = " << current_k_;
+    current_k_++;
+    return true;
+}
+
+VIO::ImuMeasurement vio_imu_from_robot_imu(const ImuSample& robot_imu) {
+    Eigen::Matrix<double, 6, 1> imu_acc_gyro;
+    imu_acc_gyro << robot_imu.accel_mpss, robot_imu.gyro_radps;
+    VIO::ImuMeasurement vio_imu(vio_time_from_robot_time(robot_imu.time_of_validity), imu_acc_gyro);
+
+    return vio_imu;
+}
+
+void SpectacularDataProviderInterface::send_imu_data() const {
+    ROBOT_CHECK(imu_single_callback_, "Did you forget to register the IMU callback?");
+    // for each imu measurement..
+
+    const time::RobotTimestamp min_imu_time = spec_log_.min_imu_time();
+    const time::RobotTimestamp max_imu_time = spec_log_.max_imu_time();
+    // pull times from the accel spline
+    const math::CubicHermiteSpline<Eigen::Vector3d>& accel_spline = spec_log_.accel_spline();
+
+    for (const double& t : accel_spline.ts()) {
+        // exit if before first IMU time
+        const time::RobotTimestamp t_robot = time::RobotTimestamp() + time::as_duration(t);
+        if (t_robot < min_imu_time) {
+            continue;
+        }
+        if (t_robot > max_imu_time) {
+            break;
+        }
+        std::optional<ImuSample> imu_sample = spec_log_.get_imu_sample(t_robot);
+        ROBOT_CHECK(imu_sample.has_value());
+        auto vio_imu = vio_imu_from_robot_imu(imu_sample.value());
+        imu_single_callback_(vio_imu);
+    }
+}
+
+/* -------------------------------------------------------------------------- */
+size_t SpectacularDataProviderInterface::get_num_images() const { return spec_log_.num_frames(); }
+
+void SpectacularDataProviderInterface::print() const {
+    LOG(INFO) << "------------------ SpectacularDataProviderInterface::print ------------------\n"
+              << "Displaying info for dataset: " << dataset_path_;
+    LOG(INFO) << "-------------------------------------------------------------";
+}
+}  // namespace robot::experimental::overhead_matching

experimental/overhead_matching/kimera_spectacular_data_provider.hh

@@ -0,0 +1,76 @@
+#pragma once
+
+#include <string>
+#include <vector>
+
+#include "experimental/overhead_matching/spectacular_log.hh"
+#include "kimera-vio/dataprovider/DataProviderInterface.h"
+#include "kimera-vio/utils/Macros.h"
+
+namespace robot::experimental::overhead_matching {
+
+/*
+ * Parse all images and camera calibration for an ETH dataset.
+ */
+class SpectacularDataProviderInterface : public VIO::DataProviderInterface {
+   public:
+    KIMERA_DELETE_COPY_CONSTRUCTORS(SpectacularDataProviderInterface);
+    KIMERA_POINTER_TYPEDEFS(SpectacularDataProviderInterface);
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+    //! Ctor with params.
+    SpectacularDataProviderInterface(const std::string& dataset_path, const int initial_k,
+                                     const int final_k, const VIO::VioParams& vio_params);
+
+    virtual ~SpectacularDataProviderInterface();
+
+   public:
+    /**
+     * @brief spin Spins the dataset until it finishes. If set in sequential mode,
+     * it will return each time a frame is sent. In parallel mode, it will not
+     * return until it finishes.
+     * @return True if the dataset still has data, false otherwise.
+     */
+    bool spin() override;
+
+    bool hasData() const override;
+
+    /**
+     * @brief print Print info about dataset.
+     */
+    void print() const;
+
+   public:
+    inline const std::string& get_dataset_path() const { return dataset_path_; }
+
+   protected:
+    /**
+     * @brief spin_once Send data to VIO pipeline on a per-frame basis
+     * @return if the dataset finished or not
+     */
+    bool spin_once();
+
+    /**
+     * @brief sendImuData We send IMU data first (before frames) so that the VIO
+     * pipeline can query all IMU data between frames.
+     */
+    void send_imu_data() const;
+
+    size_t get_num_images() const;
+
+   protected:
+    VIO::VioParams vio_params_;
+
+    std::string dataset_name_;
+    std::string dataset_path_;
+
+    VIO::FrameId current_k_;
+    VIO::FrameId final_k_;  // end frame
+
+    //! Flag to signal if the IMU data has been sent to the VIO pipeline
+    bool is_imu_data_sent_ = false;
+
+    SpectacularLog spec_log_;
+};
+
+}  // namespace robot::experimental::overhead_matching