diff --git a/common/video.cc b/common/video.cc deleted file mode 100644 index 79be9f9f..00000000 --- a/common/video.cc +++ /dev/null @@ -1,15 +0,0 @@ -#include "opencv2/opencv.hpp" - -namespace robot::common { - -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 diff --git a/common/video.hh b/common/video.hh deleted file mode 100644 index 4661059d..00000000 --- a/common/video.hh +++ /dev/null @@ -1,9 +0,0 @@ -#pragma once - -#include "opencv2/opencv.hpp" - -namespace robot::common { - -bool images_equal(const cv::Mat& img1, const cv::Mat& img2); - -} \ No newline at end of file diff --git a/experimental/overhead_matching/kimera_spectacular_data_provider.cc b/experimental/overhead_matching/kimera_spectacular_data_provider.cc index 3a6fe319..a2c6b406 100644 --- a/experimental/overhead_matching/kimera_spectacular_data_provider.cc +++ b/experimental/overhead_matching/kimera_spectacular_data_provider.cc @@ -1,31 +1,14 @@ #include "experimental/overhead_matching/kimera_spectacular_data_provider.hh" -#include -// #include -#include -#include -#include - #include // for max #include -#include +#include +#include #include -#include // for pair<> #include -#include "kimera-vio/frontend/StereoFrame.h" #include "kimera-vio/imu-frontend/ImuFrontend-definitions.h" #include "kimera-vio/logging/Logger.h" -#include "kimera-vio/utils/YamlParser.h" - -DEFINE_string(dataset_path, "/Users/Luca/data/MH_01_easy", - "Path of dataset (i.e. Euroc, /Users/Luca/data/MH_01_easy)."); -DEFINE_int64(initial_k, 0, - "Initial frame to start processing dataset, " - "previous frames will not be used."); -DEFINE_int64(final_k, 100000, - "Final frame to finish processing dataset, " - "subsequent frames will not be used."); namespace robot::experimental::overhead_matching { @@ -45,13 +28,12 @@ SpectacularDataProviderInterface::SpectacularDataProviderInterface(const std::st current_k_(std::numeric_limits::max()), initial_k_(initial_k), final_k_(final_k), - imu_measurements_(), spec_log_(dataset_path) { - // CHECK(!dataset_path_.empty()) << "Dataset path for SpectacularDataProvider is empty."; + 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). - CHECK_GE(initial_k_, 0); + ROBOT_CHECK(initial_k_ >= 0); // Finish processing dataset at frame final_k. // Last frame to process (to avoid processing the entire dataset), @@ -63,25 +45,17 @@ SpectacularDataProviderInterface::SpectacularDataProviderInterface(const std::st final_k_ = spec_log_.num_frames(); } - CHECK_GT(final_k_, 0); + ROBOT_CHECK(final_k_ > 0); - CHECK_GT(final_k_, initial_k_) - << "Value for final_k (" << final_k_ << ") is smaller than value for" - << " initial_k (" << initial_k_ << ")."; + 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(const VIO::VioParams& vio_params) - : SpectacularDataProviderInterface(FLAGS_dataset_path, FLAGS_initial_k, FLAGS_final_k, - vio_params) {} - -/* -------------------------------------------------------------------------- */ 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 @@ -95,7 +69,7 @@ bool SpectacularDataProviderInterface::spin() { } // Spin. - CHECK_GT(final_k_, initial_k_); + 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_; @@ -113,8 +87,8 @@ bool SpectacularDataProviderInterface::hasData() const { return current_k_ < fin /* -------------------------------------------------------------------------- */ bool SpectacularDataProviderInterface::spinOnce() { - CHECK_LT(current_k_, std::numeric_limits::max()) - << "Are you sure you've initialized current_k_?"; + ROBOT_CHECK(current_k_ < std::numeric_limits::max(), + "Are you sure you've initialized current_k_?"); if (current_k_ >= final_k_) { LOG(INFO) << "Finished spinning dataset."; return false; @@ -125,9 +99,7 @@ bool SpectacularDataProviderInterface::spinOnce() { const bool& equalize_image = false; // vio_params_.frontend_params_.stereo_matching_params_.equalize_image_; - std::optional maybe_frame = - spec_log_.get_frame(current_k_); - // CHECK(maybe_frame.has_value()); + 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); @@ -145,9 +117,8 @@ bool SpectacularDataProviderInterface::spinOnce() { } cv::Mat depth_meters = maybe_frame->depth_frame; - CHECK(depth_meters.type() == CV_32FC1); // type assumed if depth is distance in meters + ROBOT_CHECK(depth_meters.type() == CV_32FC1); // type assumed if depth is distance in meters - // CHECK(left_frame_callback_); left_frame_callback_( std::make_unique(current_k_, timestamp_frame_k, left_cam_FAKE_PARAMS, bgr)); @@ -168,25 +139,25 @@ VIO::ImuMeasurement vio_imu_from_robot_imu(const ImuSample& robot_imu) { } void SpectacularDataProviderInterface::sendImuData() const { - CHECK(imu_single_callback_) << "Did you forget to register the IMU callback?"; + 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(); + const math::CubicHermiteSpline& accel_spline = spec_log_.accel_spline(); for (const double& t : accel_spline.ts()) { // exit if before first IMU time - time::RobotTimestamp t_robot = time::RobotTimestamp() + time::as_duration(t); - if (t < std::chrono::duration(min_imu_time.time_since_epoch()).count()) { + const time::RobotTimestamp t_robot = time::RobotTimestamp() + time::as_duration(t); + if (t_robot < min_imu_time) { continue; } - if (t > std::chrono::duration(max_imu_time.time_since_epoch()).count()) { + if (t_robot > max_imu_time) { break; } std::optional imu_sample = spec_log_.get_imu_sample(t_robot); - CHECK(imu_sample.has_value()); + ROBOT_CHECK(imu_sample.has_value()); auto vio_imu = vio_imu_from_robot_imu(imu_sample.value()); imu_single_callback_(vio_imu); } @@ -213,45 +184,7 @@ std::string SpectacularDataProviderInterface::getDatasetName() { } /* -------------------------------------------------------------------------- */ -size_t SpectacularDataProviderInterface::getNumImages() const { - CHECK_GT(camera_names_.size(), 0u); - const std::string& left_cam_name = camera_names_.at(0); - size_t n_left_images = getNumImagesForCamera(left_cam_name); - return n_left_images; -} - -/* -------------------------------------------------------------------------- */ -size_t SpectacularDataProviderInterface::getNumImagesForCamera( - const std::string& camera_name) const { - const auto& iter = camera_image_lists_.find(camera_name); - CHECK(iter != camera_image_lists_.end()); - return iter->second.getNumImages(); -} - -/* -------------------------------------------------------------------------- */ -bool SpectacularDataProviderInterface::getImgName(const std::string& camera_name, const size_t& k, - std::string* img_filename) const { - CHECK_NOTNULL(img_filename); - const auto& iter = camera_image_lists_.find(camera_name); - CHECK(iter != camera_image_lists_.end()); - const auto& img_lists = iter->second.img_lists_; - if (k < img_lists.size()) { - *img_filename = img_lists.at(k).second; - return true; - } else { - LOG(ERROR) << "Requested image #: " << k << " but we only have " << img_lists.size() - << " images."; - } - return false; -} - -/* -------------------------------------------------------------------------- */ -VIO::Timestamp SpectacularDataProviderInterface::timestampAtFrame( - const VIO::FrameId& frame_number) { - CHECK_GT(camera_names_.size(), 0); - CHECK_LT(frame_number, camera_image_lists_.at(camera_names_[0]).img_lists_.size()); - return camera_image_lists_.at(camera_names_[0]).img_lists_[frame_number].first; -} +size_t SpectacularDataProviderInterface::getNumImages() const { return spec_log_.num_frames(); } void SpectacularDataProviderInterface::clipFinalFrame() { // Clip final_k_ to the total number of images. @@ -262,21 +195,12 @@ void SpectacularDataProviderInterface::clipFinalFrame() { final_k_ = nr_images; LOG(WARNING) << "Using final_k = " << final_k_; } - CHECK_LE(final_k_, nr_images); + ROBOT_CHECK(final_k_ <= nr_images); } /* -------------------------------------------------------------------------- */ void SpectacularDataProviderInterface::print() const { LOG(INFO) << "------------------ SpectacularDataProviderInterface::print ------------------\n" << "Displaying info for dataset: " << dataset_path_; - // For each of the 2 cameras. - CHECK_EQ(vio_params_.camera_params_.size(), camera_names_.size()); - for (size_t i = 0; i < camera_names_.size(); i++) { - LOG(INFO) << "\n" - << (i == 0 ? "Left" : "Right") << " camera name: " << camera_names_[i] - << ", with params:\n"; - vio_params_.camera_params_.at(i).print(); - camera_image_lists_.at(camera_names_[i]).print(); - } 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 index 7ea2323d..167c726d 100644 --- a/experimental/overhead_matching/kimera_spectacular_data_provider.hh +++ b/experimental/overhead_matching/kimera_spectacular_data_provider.hh @@ -1,22 +1,10 @@ #pragma once -#include -#include -#include - -#include -#include -#include #include #include #include "experimental/overhead_matching/spectacular_log.hh" -#include "kimera-vio/dataprovider/DataProviderInterface-definitions.h" #include "kimera-vio/dataprovider/DataProviderInterface.h" -#include "kimera-vio/frontend/Frame.h" -#include "kimera-vio/frontend/StereoImuSyncPacket.h" -#include "kimera-vio/frontend/StereoMatchingParams.h" -#include "kimera-vio/logging/Logger.h" #include "kimera-vio/utils/Macros.h" namespace robot::experimental::overhead_matching { @@ -33,8 +21,6 @@ class SpectacularDataProviderInterface : public VIO::DataProviderInterface { //! Ctor with params. SpectacularDataProviderInterface(const std::string& dataset_path, const int& initial_k, const int& final_k, const VIO::VioParams& vio_params); - //! Ctor from gflags - explicit SpectacularDataProviderInterface(const VIO::VioParams& vio_params); virtual ~SpectacularDataProviderInterface(); @@ -45,9 +31,9 @@ class SpectacularDataProviderInterface : public VIO::DataProviderInterface { * return until it finishes. * @return True if the dataset still has data, false otherwise. */ - virtual bool spin() override; + bool spin() override; - virtual bool hasData() const override; + bool hasData() const override; /** * @brief print Print info about dataset. @@ -63,7 +49,7 @@ class SpectacularDataProviderInterface : public VIO::DataProviderInterface { * @brief spinOnce Send data to VIO pipeline on a per-frame basis * @return if the dataset finished or not */ - virtual bool spinOnce(); + bool spinOnce(); /** * @brief sendImuData We send IMU data first (before frames) so that the VIO @@ -71,30 +57,7 @@ class SpectacularDataProviderInterface : public VIO::DataProviderInterface { */ void sendImuData() const; - //! Getters. - /** - * @brief getLeftImgName returns the img filename given the frame number - * @param[in] k frame number - * @param[out] img_name returned filename of the img - * @return if k is larger than the number of frames, returns false, otw true. - */ - inline bool getLeftImgName(const size_t& k, std::string* img_name) const { - return getImgName("cam0", k, img_name); - } size_t getNumImages() const; - size_t getNumImagesForCamera(const std::string& camera_name) const; - /** - * @brief getImgName returns the img filename given the frame number - * @param[in] camera_name camera id such as "cam0"/"cam1" - * @param[in] k frame number - * @param[out] img_filename returned filename of the img - * @return if k is larger than the number of frames, returns false, otw true. - */ - bool getImgName(const std::string& camera_name, const size_t& k, - std::string* img_filename) const; - - // Get timestamp of a given pair of stereo images (synchronized). - VIO::Timestamp timestampAtFrame(const VIO::FrameId& frame_number); // Clip final frame to the number of images in the dataset. void clipFinalFrame(); @@ -102,14 +65,6 @@ class SpectacularDataProviderInterface : public VIO::DataProviderInterface { protected: VIO::VioParams vio_params_; - /// Images data. - // TODO(Toni): remove camera_names_ and camera_image_lists_... - // This matches the names of the folders in the dataset - std::vector camera_names_; - // Map from camera name to its images - std::map camera_image_lists_; - - bool is_gt_available_; std::string dataset_name_; std::string dataset_path_; @@ -120,12 +75,6 @@ class SpectacularDataProviderInterface : public VIO::DataProviderInterface { //! Flag to signal if the IMU data has been sent to the VIO pipeline bool is_imu_data_sent_ = false; - const std::string kLeftCamName = "cam0"; - const std::string kImuName = "imu0"; - - //! Pre-stored imu-measurements - std::vector imu_measurements_; - SpectacularLog spec_log_; };