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(

common/BUILD

@@ -105,4 +105,4 @@ cc_test(
     "@bs_thread_pool",
     "@com_google_googletest//:gtest_main",
   ],
-)
+)

common/video/BUILD

@@ -0,0 +1,21 @@
+package(features=["warning_compile_flags"])
+
+cc_library(
+  name = "image_compare",
+  hdrs = ["image_compare.hh"],
+  srcs = ["image_compare.cc"],
+  deps = [
+    "@eigen//:eigen",
+    "@opencv",
+  ],
+  visibility = ["//visibility:public"],
+)
+
+cc_test(
+  name = "image_compare_test",
+  srcs = ["image_compare_test.cc"],
+  deps = [
+    ":image_compare",
+    "@com_google_googletest//:gtest_main",
+  ]
+)

common/video/image_compare.cc

@@ -0,0 +1,15 @@
+#include "opencv2/opencv.hpp"
+
+namespace robot::common::video {
+
+bool images_equal(const cv::Mat& img1, const cv::Mat& img2) {
+    if (img1.size() != img2.size() || img1.type() != img2.type()) {
+        return false;
+    }
+    cv::Mat diff;
+    cv::absdiff(img1, img2, diff);
+    diff = diff.reshape(1);
+    return cv::countNonZero(diff) == 0;
+}
+
+}  // namespace robot::common::video

common/video/image_compare.hh

@@ -0,0 +1,9 @@
+#pragma once
+
+#include "opencv2/opencv.hpp"
+
+namespace robot::common::video {
+
+bool images_equal(const cv::Mat& img1, const cv::Mat& img2);
+
+}

common/video/image_compare_test.cc

@@ -0,0 +1,33 @@
+#include "common/video/image_compare.hh"
+
+#include <gtest/gtest.h>
+
+#include "opencv2/opencv.hpp"
+
+namespace robot::common::video {
+
+TEST(ImageCompareTest, ImagesEqual_SameImages) {
+    cv::Mat img1 = cv::Mat::zeros(100, 100, CV_8UC3);
+    cv::Mat img2 = img1.clone();
+    EXPECT_TRUE(images_equal(img1, img2));
+}
+
+TEST(ImageCompareTest, ImagesEqual_DifferentSizes) {
+    cv::Mat img1 = cv::Mat::zeros(100, 100, CV_8UC3);
+    cv::Mat img2 = cv::Mat::zeros(200, 200, CV_8UC3);
+    EXPECT_FALSE(images_equal(img1, img2));
+}
+
+TEST(ImageCompareTest, ImagesEqual_DifferentTypes) {
+    cv::Mat img1 = cv::Mat::zeros(100, 100, CV_8UC3);
+    cv::Mat img2 = cv::Mat::zeros(100, 100, CV_8UC1);
+    EXPECT_FALSE(images_equal(img1, img2));
+}
+
+TEST(ImageCompareTest, ImagesEqual_DifferentContent) {
+    cv::Mat img1 = cv::Mat::zeros(100, 100, CV_8UC3);
+    cv::Mat img2 = cv::Mat::ones(100, 100, CV_8UC3);
+    EXPECT_FALSE(images_equal(img1, img2));
+}
+
+}  // namespace robot::common::video

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,26 @@ 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",
+        "//common/video:image_compare",
+    ]
+)

experimental/overhead_matching/kimera_spectacular_data_provider.cc

@@ -0,0 +1,206 @@
+#include "experimental/overhead_matching/kimera_spectacular_data_provider.hh"
+
+#include <algorithm>  // for max
+#include <fstream>
+#include <opencv2/calib3d/calib3d.hpp>
+#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_(std::numeric_limits<VIO::FrameId>::max()),
+      initial_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(initial_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_ > 0);
+
+    ROBOT_CHECK(final_k_ > initial_k_, "Value for final_k is smaller than value for initial_k",
+                final_k_, initial_k_);
+    current_k_ = initial_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_) {
+            sendImuData();
+        } else {
+            LOG(ERROR) << "Imu callback not registered! Not sending IMU data.";
+        }
+        is_imu_data_sent_ = true;
+    }
+
+    // Spin.
+    ROBOT_CHECK(final_k_ > initial_k_);
+    // We log only the first one, because we may be running in sequential mode.
+    LOG_FIRST_N(INFO, 1) << "Running dataset between frame " << initial_k_ << " and frame "
+                         << final_k_;
+    while (!shutdown_ && spinOnce()) {
+        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::spinOnce() {
+    ROBOT_CHECK(current_k_ < std::numeric_limits<VIO::FrameId>::max(),
+                "Are you sure you've initialized current_k_?");
+    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::sendImuData() 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);
+    }
+}
+
+std::string SpectacularDataProviderInterface::getDatasetName() {
+    if (dataset_name_.empty()) {
+        // Find and store actual name (rather than path) of the dataset.
+        size_t found_last_slash = dataset_path_.find_last_of("/\\");
+        std::string dataset_path_tmp = dataset_path_;
+        dataset_name_ = dataset_path_tmp.substr(found_last_slash + 1);
+        // The dataset name has a slash at the very end
+        if (found_last_slash >= dataset_path_tmp.size() - 1) {
+            // Cut the last slash.
+            dataset_path_tmp = dataset_path_tmp.substr(0, found_last_slash);
+            // Repeat the search.
+            found_last_slash = dataset_path_tmp.find_last_of("/\\");
+            // Try to pick right name.
+            dataset_name_ = dataset_path_tmp.substr(found_last_slash + 1);
+        }
+        LOG(INFO) << "Dataset name: " << dataset_name_;
+    }
+    return dataset_name_;
+}
+
+/* -------------------------------------------------------------------------- */
+size_t SpectacularDataProviderInterface::getNumImages() const { return spec_log_.num_frames(); }
+
+void SpectacularDataProviderInterface::clipFinalFrame() {
+    // Clip final_k_ to the total number of images.
+    const size_t& nr_images = getNumImages();
+    if (final_k_ > nr_images) {
+        LOG(WARNING) << "Value for final_k, " << final_k_ << " is larger than total"
+                     << " number of frames in dataset " << nr_images;
+        final_k_ = nr_images;
+        LOG(WARNING) << "Using final_k = " << final_k_;
+    }
+    ROBOT_CHECK(final_k_ <= nr_images);
+}
+/* -------------------------------------------------------------------------- */
+void SpectacularDataProviderInterface::print() const {
+    LOG(INFO) << "------------------ SpectacularDataProviderInterface::print ------------------\n"
+              << "Displaying info for dataset: " << dataset_path_;
+    LOG(INFO) << "-------------------------------------------------------------";
+}
+}  // namespace robot::experimental::overhead_matching