diff --git a/WORKSPACE b/WORKSPACE index 0ecff6a5..c7354554 100644 --- a/WORKSPACE +++ b/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( diff --git a/common/video/image_compare.cc b/common/video/image_compare.cc index 145befb6..7bca7aea 100644 --- a/common/video/image_compare.cc +++ b/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; } diff --git a/common/video/image_compare.hh b/common/video/image_compare.hh index c9d56195..6332b5e0 100644 --- a/common/video/image_compare.hh +++ b/common/video/image_compare.hh @@ -6,4 +6,4 @@ namespace robot::common::video { bool images_equal(const cv::Mat& img1, const cv::Mat& img2); -} \ No newline at end of file +} diff --git a/experimental/overhead_matching/BUILD b/experimental/overhead_matching/BUILD index f807768c..7c9b8d96 100644 --- a/experimental/overhead_matching/BUILD +++ b/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", + ] +) diff --git a/experimental/overhead_matching/kimera_spectacular_data_provider.cc b/experimental/overhead_matching/kimera_spectacular_data_provider.cc new file mode 100644 index 00000000..a9fe99ae --- /dev/null +++ b/experimental/overhead_matching/kimera_spectacular_data_provider.cc @@ -0,0 +1,166 @@ +#include "experimental/overhead_matching/kimera_spectacular_data_provider.hh" + +#include +#include +#include + +#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 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(current_k_, timestamp_frame_k, left_cam_FAKE_PARAMS, bgr)); + + depth_frame_callback_( + std::make_unique(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 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& 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 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 diff --git a/experimental/overhead_matching/kimera_spectacular_data_provider.hh b/experimental/overhead_matching/kimera_spectacular_data_provider.hh new file mode 100644 index 00000000..2b3ef5f7 --- /dev/null +++ b/experimental/overhead_matching/kimera_spectacular_data_provider.hh @@ -0,0 +1,76 @@ +#pragma once + +#include +#include + +#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 \ No newline at end of file diff --git a/experimental/overhead_matching/kimera_spectacular_data_provider_test.cc b/experimental/overhead_matching/kimera_spectacular_data_provider_test.cc new file mode 100644 index 00000000..eac241e8 --- /dev/null +++ b/experimental/overhead_matching/kimera_spectacular_data_provider_test.cc @@ -0,0 +1,174 @@ + +#include "experimental/overhead_matching/kimera_spectacular_data_provider.hh" + +#include +#include + +#include "common/video/image_compare.hh" +#include "experimental/overhead_matching/spectacular_log.hh" +#include "fmt/core.h" +#include "gtest/gtest.h" +#include "kimera-vio/pipeline/Pipeline-definitions.h" +#include "opencv2/opencv.hpp" + +namespace robot::experimental::overhead_matching { + +std::ostream& operator<<(std::ostream& out, const time::RobotTimestamp& t) { + std::ostringstream ss; + ss << std::fixed << std::setprecision(9) + << std::chrono::duration(t.time_since_epoch()).count(); + out << ss.str(); + return out; +} + +bool compare_imu_samples(const robot::experimental::overhead_matching::ImuSample& robot_imu, + const VIO::ImuMeasurement& kimera_imu) { + // timestamp + if (kimera_imu.timestamp_ != robot_imu.time_of_validity.time_since_epoch().count()) { + fmt::print("Kimera timestamp: {} | robot timestamp: {}", kimera_imu.timestamp_, + robot_imu.time_of_validity.time_since_epoch().count()); + fmt::print("diff {}", + robot_imu.time_of_validity.time_since_epoch().count() - kimera_imu.timestamp_); + return false; + } + // accel and gyro values + if (kimera_imu.acc_gyr_.rows() != 6 || kimera_imu.acc_gyr_.cols() != 1) { + fmt::print("shapes"); + return false; + } + for (int i = 0; i < 6; i++) { + Eigen::Vector3d active_vec; + if (i < 3) { + active_vec = robot_imu.accel_mpss; + } else { + active_vec = robot_imu.gyro_radps; + } + + if (std::abs(active_vec(i % 3) - kimera_imu.acc_gyr_(i, 0)) > 1e-9) { + fmt::print("imu ", i); + return false; + } + } + return true; +} + +bool compare_bgr_frame(const std::unique_ptr& kimera_frame, const cv::Mat& spec_bgr, + const time::RobotTimestamp& spec_time, const int& sequence_index) { + // check image + cv::Mat grey_image; + if (spec_bgr.channels() > 1) { + cv::cvtColor(spec_bgr, grey_image, cv::COLOR_BGR2GRAY); + } else { + grey_image = spec_bgr; + } + if (!common::video::images_equal(kimera_frame->img_, grey_image)) { + return false; + } + // check timestamps + if (kimera_frame->timestamp_ != spec_time.time_since_epoch().count()) { + return false; + } + // check index number + if (kimera_frame->id_ != sequence_index) { + return false; + } + return true; +} +bool compare_depth_frame(const std::unique_ptr& kimera_depth, + const cv::Mat& spec_depth, const time::RobotTimestamp& spec_time, + const int& sequence_index) { + // check image + if (!common::video::images_equal(kimera_depth->depth_img_, spec_depth)) { + return false; + } + // check timestamps + if (kimera_depth->timestamp_ != spec_time.time_since_epoch().count()) { + std::cout << "ID mismatch! " << kimera_depth->id_ << " " + << spec_time.time_since_epoch().count() << std::endl; + return false; + } + // check index number + if (kimera_depth->id_ != sequence_index) { + return false; + } + return true; +} + +TEST(KimeraSpectacularDataProviderTest, happy_case) { + // Setup + const std::filesystem::path log_path("external/spectacular_log_snippet/20241212_150605"); + SpectacularLog log(log_path); + + // get initial IMU information + std::vector times; + std::vector original_imu_samples; + for (const double& t : log.accel_spline().ts()) { + time::RobotTimestamp robot_time = time::RobotTimestamp() + time::as_duration(t); + if (robot_time < log.min_imu_time() || robot_time > log.max_imu_time()) { + continue; + } + times.push_back(t); + auto maybe_sample = log.get_imu_sample(robot_time); + EXPECT_TRUE(maybe_sample.has_value()); + original_imu_samples.push_back(maybe_sample.value()); + } + + const std::filesystem::path vio_config_path(""); // loads default params + VIO::VioParams vio_params(vio_config_path); + vio_params.parallel_run_ = false; + + SpectacularDataProviderInterface s_interface(log_path, 0, std::numeric_limits::max(), + vio_params); + + std::vector imu_queue; + std::vector> bgr_queue; + std::vector> depth_queue; + + auto imu_callback = [&imu_queue](const VIO::ImuMeasurement& measurement) -> void { + imu_queue.push_back(measurement); + return; + }; + + auto color_camera_callback = [&bgr_queue](std::unique_ptr bgr_frame) -> void { + bgr_queue.push_back(std::move(bgr_frame)); + return; + }; + + auto depth_camera_callback = + [&depth_queue](std::unique_ptr depth_frame) -> void { + depth_queue.push_back(std::move(depth_frame)); + return; + }; + + // bind IMU callback + s_interface.registerImuSingleCallback(imu_callback); + // bind images callback + s_interface.registerLeftFrameCallback(color_camera_callback); + // bind depth callback + s_interface.registerDepthFrameCallback(depth_camera_callback); + + // Action + while (s_interface.spin()) { + } + + // Verification + EXPECT_TRUE(imu_queue.size() == times.size()); + // for (auto [imu_true, imu_kimera] : std::views::zip(original_imu_samples, imu_queue)) { + // EXPECT_TRUE(compare_imu_samples(imu_true, imu_kimera)); + for (size_t i = 0; i < imu_queue.size(); i++) { + EXPECT_TRUE(compare_imu_samples(original_imu_samples[i], imu_queue[i])); + break; + } + + // bgr and depth images + EXPECT_TRUE(depth_queue.size() == log.num_frames() && bgr_queue.size() == log.num_frames()); + for (size_t i = 0; i < log.num_frames(); i++) { + std::optional fg = log.get_frame(i); + EXPECT_TRUE(fg.has_value()); + + EXPECT_TRUE(compare_bgr_frame(bgr_queue[i], fg->bgr_frame, fg->time_of_validity, i)); + EXPECT_TRUE(compare_depth_frame(depth_queue[i], fg->depth_frame, fg->time_of_validity, i)); + } +} + +} // namespace robot::experimental::overhead_matching diff --git a/experimental/overhead_matching/kimera_vio_pipeline_test.cc b/experimental/overhead_matching/kimera_vio_pipeline_test.cc new file mode 100644 index 00000000..02f72fe9 --- /dev/null +++ b/experimental/overhead_matching/kimera_vio_pipeline_test.cc @@ -0,0 +1,79 @@ +#include +#include + +#include +#include +#include +#include + +#include "experimental/overhead_matching/kimera_spectacular_data_provider.hh" +#include "kimera-vio/dataprovider/DataProviderInterface.h" +#include "kimera-vio/logging/Logger.h" +#include "kimera-vio/pipeline/Pipeline.h" +#include "kimera-vio/pipeline/RgbdImuPipeline.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."); +DEFINE_string(dataset_path, "/Users/Luca/data/MH_01_easy", + "Path of dataset (i.e. Euroc, /Users/Luca/data/MH_01_easy)."); + +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); + + // Build dataset parser. + VIO::DataProviderInterface::Ptr dataset_parser = + std::make_unique( + vio_params); + + CHECK(dataset_parser); + + VIO::Pipeline::Ptr vio_pipeline; + + vio_pipeline = std::make_unique(vio_params); + + // Register callback to shutdown data provider in case VIO pipeline + // shutsdown. + vio_pipeline->registerShutdownCallback( + std::bind(&VIO::DataProviderModule::shutdown, dataset_parser)); + + // Register callback to vio pipeline. + dataset_parser->registerImuSingleCallback( + std::bind(&VIO::Pipeline::fillSingleImuQueue, vio_pipeline, std::placeholders::_1)); + // We use blocking variants to avoid overgrowing the input queues (use + // the non-blocking versions with real sensor streams) + dataset_parser->registerLeftFrameCallback( + std::bind(&VIO::Pipeline::fillLeftFrameQueue, vio_pipeline, std::placeholders::_1)); + + dataset_parser->registerDepthFrameCallback( + std::bind(&VIO::RgbdImuPipeline::fillDepthFrameQueue, vio_pipeline, std::placeholders::_1)); + + // Spin dataset. + auto tic = VIO::utils::Timer::tic(); + bool is_pipeline_successful = false; + while (dataset_parser->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) { + // Log overall time of pipeline run. + VIO::PipelineLogger logger; + logger.logPipelineOverallTiming(spin_duration); + } + + return is_pipeline_successful ? EXIT_SUCCESS : EXIT_FAILURE; +} diff --git a/experimental/overhead_matching/spectacular_log.cc b/experimental/overhead_matching/spectacular_log.cc index 206c8678..1069498a 100644 --- a/experimental/overhead_matching/spectacular_log.cc +++ b/experimental/overhead_matching/spectacular_log.cc @@ -171,8 +171,9 @@ std::optional SpectacularLog::get_frame(const int frame_id) const { // Read the desired depth frame const fs::path depth_frame_path = - log_path_ / "frames2" / fmt::format("{:08d}.png", frame_info.frame_number); - cv::Mat depth_frame = cv::imread(depth_frame_path.string(), cv::IMREAD_GRAYSCALE); + log_path_ / "frames2" / fmt::format("{:08d}.tiff", frame_info.frame_number); + + cv::Mat depth_frame = cv::imread(depth_frame_path.string(), cv::IMREAD_UNCHANGED); return {{ .time_of_validity = diff --git a/experimental/overhead_matching/spectacular_log_test.cc b/experimental/overhead_matching/spectacular_log_test.cc index a0c2dd43..6f79d504 100644 --- a/experimental/overhead_matching/spectacular_log_test.cc +++ b/experimental/overhead_matching/spectacular_log_test.cc @@ -5,22 +5,13 @@ #include #include "common/matplotlib.hh" +#include "common/video/image_compare.hh" #include "fmt/format.h" #include "gtest/gtest.h" #include "opencv2/opencv.hpp" namespace robot::experimental::overhead_matching { -bool images_equal(cv::Mat img1, 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; -} - std::ostream &operator<<(std::ostream &out, const time::RobotTimestamp &t) { std::ostringstream ss; ss << std::fixed << std::setprecision(9) @@ -31,8 +22,7 @@ std::ostream &operator<<(std::ostream &out, const time::RobotTimestamp &t) { TEST(SpectacularLogTest, happy_case) { // Setup - const std::filesystem::path log_path( - "external/spectacular_log_snippet/recording_2024-11-21_13-36-30"); + const std::filesystem::path log_path("external/spectacular_log_snippet/20241212_150605"); SpectacularLog log(log_path); // Action @@ -89,11 +79,11 @@ TEST(SpectacularLogTest, happy_case) { const auto frame = log.get_frame(frame_id).value(); const std::filesystem::path depth_path(log_path / - fmt::format("frames2/{:08d}.png", frame_id)); - const cv::Mat depth_frame = cv::imread(depth_path, cv::IMREAD_GRAYSCALE); + fmt::format("frames2/{:08d}.tiff", frame_id)); + const cv::Mat depth_frame = cv::imread(depth_path, cv::IMREAD_UNCHANGED); - EXPECT_TRUE(images_equal(expected_frame, frame.bgr_frame)); - EXPECT_TRUE(images_equal(depth_frame, frame.depth_frame)); + EXPECT_TRUE(common::video::images_equal(expected_frame, frame.bgr_frame)); + EXPECT_TRUE(common::video::images_equal(depth_frame, frame.depth_frame)); for (int i = 0; i < FRAME_SKIP - 1; i++) { video.grab(); diff --git a/third_party/kimera_vio_0001-add-missing-includes.patch b/third_party/kimera_vio_0001-add-missing-includes.patch deleted file mode 100644 index 17b8d97b..00000000 --- a/third_party/kimera_vio_0001-add-missing-includes.patch +++ /dev/null @@ -1,37 +0,0 @@ -diff --git a/include/kimera-vio/mesh/MeshOptimization.h b/include/kimera-vio/mesh/MeshOptimization.h -index fa1da651..7d73d93e 100644 ---- a/include/kimera-vio/mesh/MeshOptimization.h -+++ b/include/kimera-vio/mesh/MeshOptimization.h -@@ -21,6 +21,7 @@ - #include - - #include -+#include - - #include - #include -diff --git a/include/kimera-vio/utils/UtilsNumerical.h b/include/kimera-vio/utils/UtilsNumerical.h -index 47fb8c9e..2c21f977 100644 ---- a/include/kimera-vio/utils/UtilsNumerical.h -+++ b/include/kimera-vio/utils/UtilsNumerical.h -@@ -16,6 +16,7 @@ - - #include - #include -+#include - - namespace VIO { - -diff --git a/src/frontend/RgbdCamera.cpp b/src/frontend/RgbdCamera.cpp -index 761567e7..29344a7f 100644 ---- a/src/frontend/RgbdCamera.cpp -+++ b/src/frontend/RgbdCamera.cpp -@@ -15,6 +15,8 @@ - - #include "kimera-vio/frontend/RgbdCamera.h" - -+#include -+ - #include "kimera-vio/frontend/Camera.h" - - namespace VIO { diff --git a/third_party/kimera_vio_0002-add-override-markers.patch b/third_party/kimera_vio_0002-add-override-markers.patch deleted file mode 100644 index c4b441f4..00000000 --- a/third_party/kimera_vio_0002-add-override-markers.patch +++ /dev/null @@ -1,26 +0,0 @@ -diff --git a/include/kimera-vio/dataprovider/MonoDataProviderModule.h b/include/kimera-vio/dataprovider/MonoDataProviderModule.h -index 6c3b3a0e..23849b7f 100644 ---- a/include/kimera-vio/dataprovider/MonoDataProviderModule.h -+++ b/include/kimera-vio/dataprovider/MonoDataProviderModule.h -@@ -100,7 +100,7 @@ class MonoDataProviderModule : public DataProviderModule { - virtual void shutdownQueues() override; - - //! Checks if the module has work to do (should check input queues are empty) -- virtual inline bool hasWork() const { return !left_frame_queue_.empty(); } -+ virtual inline bool hasWork() const override { return !left_frame_queue_.empty(); } - - protected: - //! Input data -diff --git a/include/kimera-vio/frontend/optical-flow/OpticalFlowPredictor.h b/include/kimera-vio/frontend/optical-flow/OpticalFlowPredictor.h -index 3ff1ee77..8957eb51 100644 ---- a/include/kimera-vio/frontend/optical-flow/OpticalFlowPredictor.h -+++ b/include/kimera-vio/frontend/optical-flow/OpticalFlowPredictor.h -@@ -63,7 +63,7 @@ class NoOpticalFlowPredictor : public OpticalFlowPredictor { - bool predictSparseFlow(const KeypointsCV& prev_kps, - const gtsam::Rot3& /* inter_frame */, - KeypointsCV* next_kps) override; -- cv::Mat predictDenseFlow(const gtsam::Rot3& cam1_R_cam2) { return cv::Mat(); } -+ cv::Mat predictDenseFlow(const gtsam::Rot3& cam1_R_cam2) override { return cv::Mat(); } - }; - - /** diff --git a/third_party/opengv_0002-unused-parameter.patch b/third_party/opengv_0002-unused-parameter.patch new file mode 100644 index 00000000..2eb6938f --- /dev/null +++ b/third_party/opengv_0002-unused-parameter.patch @@ -0,0 +1,13 @@ +diff --git a/include/opengv/sac/implementation/SampleConsensusProblem.hpp b/include/opengv/sac/implementation/SampleConsensusProblem.hpp +index 7ae8ab2..896bb72 100644 +--- a/include/opengv/sac/implementation/SampleConsensusProblem.hpp ++++ b/include/opengv/sac/implementation/SampleConsensusProblem.hpp +@@ -54,7 +54,7 @@ opengv::sac::SampleConsensusProblem::~SampleConsensusProblem() + + template + bool opengv::sac::SampleConsensusProblem::isSampleGood( +- const std::vector & sample) const ++ [[maybe_unused]] const std::vector & sample) const + { + // Default implementation + return true;