From 3f1dd69911e71f7067ff50d5ca3cbd2f46555cb1 Mon Sep 17 00:00:00 2001 From: Ethan Fahnestock Date: Tue, 26 Nov 2024 22:37:32 -0500 Subject: [PATCH 1/8] wip from Euroc example --- experimental/overhead_matching/BUILD | 18 + .../kimera_spectacular_data_provider.cc | 527 ++++++++++++++++++ .../kimera_spectacular_data_provider.hh | 142 +++++ .../kimera_vio_pipeline_test.cc | 87 +++ 4 files changed, 774 insertions(+) create mode 100644 experimental/overhead_matching/kimera_spectacular_data_provider.cc create mode 100644 experimental/overhead_matching/kimera_spectacular_data_provider.hh create mode 100644 experimental/overhead_matching/kimera_vio_pipeline_test.cc diff --git a/experimental/overhead_matching/BUILD b/experimental/overhead_matching/BUILD index 9b5dac35..2a4ee287 100644 --- a/experimental/overhead_matching/BUILD +++ b/experimental/overhead_matching/BUILD @@ -25,3 +25,21 @@ cc_test( "@fmt", ] ) + +cc_library( + name = "kimera_spectacular_data_provider", + hdrs = ["kimera_spectacular_data_provider.hh"], + srcs = ["kimera_spectacular_data_provider.cc"], + deps = [ + "@kimera_vio", + ":spectacular_log", + ] +) +cc_test( + name = "kimera_vio_pipeline_test", + srcs = ["kimera_vio_pipeline_test.cc"], + deps = [ + ":kimera_spectacular_data_provider", + "@com_google_googletest//:gtest_main", + ] +) 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..80200137 --- /dev/null +++ b/experimental/overhead_matching/kimera_spectacular_data_provider.cc @@ -0,0 +1,527 @@ +#include "experimental/overhead_matching/kimera_spectacular_data_provider.hh" + +#include // for max +#include +#include +#include +#include // for pair<> +#include + +#include +#include +#include + +#include +#include +#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 { + +SpectacularLogDataProvider::SpectacularLogDataProvider(const std::string& dataset_path, + const int& initial_k, + const int& final_k, + const VioParams& vio_params) + : DataProviderInterface(), + vio_params_(vio_params), + dataset_path_(dataset_path), + current_k_(std::numeric_limits::max()), + initial_k_(initial_k), + final_k_(final_k), + imu_measurements_()) { + + CHECK(!dataset_path_.empty()) + << "Dataset path for SpectacularLogDataProvider 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); + + // Finish processing dataset at frame final_k. + // Last frame to process (to avoid processing the entire dataset), + // skip last frames. + CHECK_GT(final_k_, 0); + + CHECK_GT(final_k_, initial_k_) << "Value for final_k (" << final_k_ + << ") is smaller than value for" + << " initial_k (" << initial_k_ << ")."; + current_k_ = initial_k_; + + // Parse the actual dataset first, then run it. + if (!shutdown_ && !dataset_parsed_) { + LOG(INFO) << "Parsing Spectacular dataset..."; + parse(); + CHECK_GT(imu_measurements_.size(), 0u); + dataset_parsed_ = true; + } +} + +/* -------------------------------------------------------------------------- */ +SpectacularLogDataProvider::SpectacularLogDataProvider(const VioParams& vio_params) + : SpectacularLogDataProvider(FLAGS_dataset_path, + FLAGS_initial_k, + FLAGS_final_k, + vio_params) {} + +/* -------------------------------------------------------------------------- */ +SpectacularLogDataProvider::~SpectacularLogDataProvider() { + LOG(INFO) << "SpectacularLogDataProvider destructor called."; +} + +/* -------------------------------------------------------------------------- */ +bool SpectacularLogDataProvider::spin() { + if (dataset_parsed_) { + 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. + CHECK_EQ(vio_params_.camera_params_.size(), 2u); + CHECK_GT(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; + } + } + } else { + LOG(ERROR) << "Euroc dataset was not parsed."; + } + LOG_IF(INFO, shutdown_) << "SpectacularLogDataProvider shutdown requested."; + return false; +} + +bool SpectacularLogDataProvider::hasData() const { + return current_k_ < final_k_; +} + +/* -------------------------------------------------------------------------- */ +bool SpectacularLogDataProvider::spinOnce() { + CHECK_LT(current_k_, std::numeric_limits::max()) + << "Are you sure you've initialized current_k_?"; + if (current_k_ >= final_k_) { + LOG(INFO) << "Finished spinning Euroc dataset."; + return false; + } + + const CameraParams& left_cam_info = vio_params_.camera_params_.at(0); + const CameraParams& right_cam_info = vio_params_.camera_params_.at(1); + const bool& equalize_image = + vio_params_.frontend_params_.stereo_matching_params_.equalize_image_; + + const Timestamp& timestamp_frame_k = timestampAtFrame(current_k_); + VLOG(10) << "Sending left/right frames k= " << current_k_ + << " with timestamp: " << timestamp_frame_k; + + // TODO(Toni): ideally only send cv::Mat raw images...: + // - pass params to vio_pipeline ctor + // - make vio_pipeline actually equalize or transform images as necessary. + std::string left_img_filename; + bool available_left_img = getLeftImgName(current_k_, &left_img_filename); + std::string right_img_filename; + bool available_right_img = getRightImgName(current_k_, &right_img_filename); + if (available_left_img && available_right_img) { + // Both stereo images are available, send data to VIO + CHECK(left_frame_callback_); + left_frame_callback_( + std::make_unique(current_k_, + timestamp_frame_k, + // TODO(Toni): this info should be passed to + // the camera... not all the time here... + left_cam_info, + UtilsOpenCV::ReadAndConvertToGrayScale( + left_img_filename, equalize_image))); + CHECK(right_frame_callback_); + right_frame_callback_( + std::make_unique(current_k_, + timestamp_frame_k, + // TODO(Toni): this info should be passed to + // the camera... not all the time here... + right_cam_info, + UtilsOpenCV::ReadAndConvertToGrayScale( + right_img_filename, equalize_image))); + } else { + LOG(ERROR) << "Missing left/right stereo pair, proceeding to the next one."; + } + + // This is done directly when parsing the Imu data. + // imu_single_callback_(imu_meas); + + VLOG(10) << "Finished VIO processing for frame k = " << current_k_; + current_k_++; + return true; +} + +void SpectacularLogDataProvider::sendImuData() const { + CHECK(imu_single_callback_) << "Did you forget to register the IMU callback?"; + Timestamp previous_timestamp = -1; + for (const ImuMeasurement& imu_meas : imu_measurements_) { + CHECK_GT(imu_meas.timestamp_, previous_timestamp) + << "Euroc IMU data is not in chronological order!"; + previous_timestamp = imu_meas.timestamp_; + imu_single_callback_(imu_meas); + } +} + +/* -------------------------------------------------------------------------- */ +void SpectacularLogDataProvider::parse() { + VLOG(100) << "Using dataset path: " << dataset_path_; + // Parse the dataset (ETH format). + parseDataset(); + if (VLOG_IS_ON(1)) print(); + + // Send first ground-truth pose to VIO for initialization if requested. + if (vio_params_.backend_params_->autoInitialize_ == 0) { + // We want to initialize from ground-truth. + vio_params_.backend_params_->initial_ground_truth_state_ = + getGroundTruthState(timestampAtFrame(initial_k_)); + } +} + +/* -------------------------------------------------------------------------- */ +bool SpectacularLogDataProvider::parseImuData(const std::string& input_dataset_path, + const std::string& imuName) { + ///////////////// PARSE ACTUAL DATA ////////////////////////////////////////// + //#timestamp [ns],w_RS_S_x [rad s^-1],w_RS_S_y [rad s^-1],w_RS_S_z [rad s^-1], + // a_RS_S_x [m s^-2],a_RS_S_y [m s^-2],a_RS_S_z [m s^-2] + std::string filename_data = + input_dataset_path + "/mav0/" + imuName + "/data.csv"; + std::ifstream fin(filename_data.c_str()); + LOG_IF(FATAL, !fin.is_open()) << "Cannot open file: " << filename_data; + + // Skip the first line, containing the header. + std::string line; + std::getline(fin, line); + + size_t deltaCount = 0u; + Timestamp sumOfDelta = 0; + double stdDelta = 0; + double imu_rate_maxMismatch = 0; + double maxNormAcc = 0, maxNormRotRate = 0; // only for debugging + Timestamp previous_timestamp = -1; + + // Read/store imu measurements, line by line. + while (std::getline(fin, line)) { + Timestamp timestamp = 0; + gtsam::Vector6 gyr_acc_data; + for (int i = 0u; i < gyr_acc_data.size() + 1u; i++) { + int idx = line.find_first_of(','); + if (i == 0) { + timestamp = std::stoll(line.substr(0, idx)); + } else { + gyr_acc_data(i - 1) = std::stod(line.substr(0, idx)); + } + line = line.substr(idx + 1); + } + CHECK_GT(timestamp, previous_timestamp) + << "Euroc IMU data is not in chronological order!"; + Vector6 imu_accgyr; + // Acceleration first! + imu_accgyr << gyr_acc_data.tail(3), gyr_acc_data.head(3); + + double normAcc = gyr_acc_data.tail(3).norm(); + if (normAcc > maxNormAcc) maxNormAcc = normAcc; + + double normRotRate = gyr_acc_data.head(3).norm(); + if (normRotRate > maxNormRotRate) maxNormRotRate = normRotRate; + + //! Store imu measurements + imu_measurements_.push_back(ImuMeasurement(timestamp, imu_accgyr)); + + if (previous_timestamp != -1) { + sumOfDelta += (timestamp - previous_timestamp); + double deltaMismatch = + std::fabs(static_cast( + timestamp - previous_timestamp - + vio_params_.imu_params_.nominal_sampling_time_s_) * + 1e-9); + stdDelta += std::pow(deltaMismatch, 2); + imu_rate_maxMismatch = std::max(imu_rate_maxMismatch, deltaMismatch); + deltaCount += 1u; + } + previous_timestamp = timestamp; + } + + // Converted to seconds. + VLOG(10) << "IMU rate: " + << (static_cast(sumOfDelta) / + static_cast(deltaCount)) * + 1e-9 + << '\n' + << "IMU rate std: " + << std::sqrt(stdDelta / static_cast(deltaCount - 1u)) << '\n' + << "IMU rate max mismatch: " << imu_rate_maxMismatch << '\n' + << "Maximum measured rotation rate (norm):" << maxNormRotRate << '\n' + << "Maximum measured acceleration (norm): " << maxNormAcc; + fin.close(); + + return true; +} + +/* -------------------------------------------------------------------------- */ +bool SpectacularLogDataProvider::parseDataset() { + // Parse IMU data. + CHECK(parseImuData(dataset_path_, kImuName)); + + // Parse Camera data. + CameraImageLists left_cam_image_list; + CameraImageLists right_cam_image_list; + parseCameraData(kLeftCamName, &left_cam_image_list); + if (VLOG_IS_ON(1)) left_cam_image_list.print(); + parseCameraData(kRightCamName, &right_cam_image_list); + if (VLOG_IS_ON(1)) right_cam_image_list.print(); + // TODO(Toni): remove camera_names_ and camera_image_lists_... + camera_names_.push_back(kLeftCamName); + camera_names_.push_back(kRightCamName); + // WARNING Use [x] not .at() because we are adding entries that do not exist. + camera_image_lists_[kLeftCamName] = left_cam_image_list; + camera_image_lists_[kRightCamName] = right_cam_image_list; + // CHECK(sanityCheckCameraData(camera_names_, &camera_image_lists_)); + + // Parse Ground-Truth data. + static const std::string ground_truth_name = "state_groundtruth_estimate0"; + is_gt_available_ = parseGtData(dataset_path_, ground_truth_name); + + clipFinalFrame(); + + // Log Ground-Truth data. + if (logger_) { + if (is_gt_available_) { + logger_->logGtData(dataset_path_ + "/mav0/" + ground_truth_name + + "/data.csv"); + } else { + LOG(ERROR) << "Requested ground-truth data logging but no ground-truth " + "data available."; + } + } + + return true; +} + +/* -------------------------------------------------------------------------- */ +bool SpectacularLogDataProvider::parseCameraData(const std::string& cam_name, + CameraImageLists* cam_list_i) { + CHECK_NOTNULL(cam_list_i) + ->parseCamImgList(dataset_path_ + "/mav0/" + cam_name, "data.csv"); + return true; +} + +std::string SpectacularLogDataProvider::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 SpectacularLogDataProvider::getNumImages() const { + CHECK_GT(camera_names_.size(), 0u); + const std::string& left_cam_name = camera_names_.at(0); + const std::string& right_cam_name = camera_names_.at(0); + size_t n_left_images = getNumImagesForCamera(left_cam_name); + size_t n_right_images = getNumImagesForCamera(right_cam_name); + CHECK_EQ(n_left_images, n_right_images); + return n_left_images; +} + +/* -------------------------------------------------------------------------- */ +size_t SpectacularLogDataProvider::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 SpectacularLogDataProvider::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; +} + +/* -------------------------------------------------------------------------- */ +Timestamp SpectacularLogDataProvider::timestampAtFrame(const 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; +} + +void SpectacularLogDataProvider::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_; + } + CHECK_LE(final_k_, nr_images); +} +/* -------------------------------------------------------------------------- */ +void SpectacularLogDataProvider::print() const { + LOG(INFO) << "------------------ ETHDatasetParser::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(); + } + if (FLAGS_minloglevel < 1) { + gt_data_.print(); + } + LOG(INFO) << "-------------------------------------------------------------"; +} + +////////////////////////////////////////////////////////////////////////////// + +/* -------------------------------------------------------------------------- */ +MonoEurocDataProvider::MonoEurocDataProvider(const std::string& dataset_path, + const int& initial_k, + const int& final_k, + const VioParams& vio_params) + : SpectacularLogDataProvider(dataset_path, initial_k, final_k, vio_params) {} + +/* -------------------------------------------------------------------------- */ +MonoEurocDataProvider::MonoEurocDataProvider(const VioParams& vio_params) + : SpectacularLogDataProvider(vio_params) {} + +/* -------------------------------------------------------------------------- */ +MonoEurocDataProvider::~MonoEurocDataProvider() { + LOG(INFO) << "Mono ETHDataParser destructor called."; +} + +/* -------------------------------------------------------------------------- */ +bool MonoEurocDataProvider::spin() { + if (dataset_parsed_) { + 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. + // CHECK_EQ(vio_params_.camera_params_.size(), 2u); + CHECK_GT(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; + } + } + } else { + LOG(ERROR) << "Euroc dataset was not parsed."; + } + LOG_IF(INFO, shutdown_) << "SpectacularLogDataProvider shutdown requested."; + return false; +} + +/* -------------------------------------------------------------------------- */ +bool MonoEurocDataProvider::spinOnce() { + CHECK_LT(current_k_, std::numeric_limits::max()) + << "Are you sure you've initialized current_k_?"; + if (current_k_ >= final_k_) { + LOG(INFO) << "Finished spinning Euroc dataset."; + return false; + } + + const CameraParams& left_cam_info = vio_params_.camera_params_.at(0); + const bool& equalize_image = + vio_params_.frontend_params_.stereo_matching_params_.equalize_image_; + + const Timestamp& timestamp_frame_k = timestampAtFrame(current_k_); + VLOG(10) << "Sending left frame k= " << current_k_ + << " with timestamp: " << timestamp_frame_k; + + // TODO(Toni): ideally only send cv::Mat raw images...: + // - pass params to vio_pipeline ctor + // - make vio_pipeline actually equalize or transform images as necessary. + std::string left_img_filename; + bool available_left_img = getLeftImgName(current_k_, &left_img_filename); + if (available_left_img) { + // Both stereo images are available, send data to VIO + CHECK(left_frame_callback_); + left_frame_callback_( + std::make_unique(current_k_, + timestamp_frame_k, + // TODO(Toni): this info should be passed to + // the camera... not all the time here... + left_cam_info, + UtilsOpenCV::ReadAndConvertToGrayScale( + left_img_filename, equalize_image))); + } else { + LOG(ERROR) << "Missing left image, proceeding to the next one."; + } + + // This is done directly when parsing the Imu data. + // imu_single_callback_(imu_meas); + + VLOG(10) << "Finished VIO processing for frame k = " << current_k_; + current_k_++; + return true; +} + +} // namespace 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..518f016a --- /dev/null +++ b/experimental/overhead_matching/kimera_spectacular_data_provider.hh @@ -0,0 +1,142 @@ + +#pragma once + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "experimental/overhead_matching/spectacular_log.hh" +#include "kimera-vio/common/VioNavState.h" +#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 { + +class SpectacularLogDataProvider : public DataProviderInterface { + public: + KIMERA_DELETE_COPY_CONSTRUCTORS(SpectacularLogDataProvider); + KIMERA_POINTER_TYPEDEFS(SpectacularLogDataProvider); + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + //! Ctor with params. + SpectacularLogDataProvider(const std::string& dataset_path, const int& initial_k, + const int& final_k, const VioParams& vio_params); + //! Ctor from gflags + explicit SpectacularLogDataProvider(const VioParams& vio_params); + + virtual ~SpectacularLogDataProvider(); + + 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. + */ + virtual bool spin() override; + + virtual bool hasData() const override; + + /** + * @brief print Print info about dataset. + */ + void print() const; + + public: + inline std::string getDatasetPath() const { return dataset_path_; } + std::string getDatasetName(); + + protected: + /** + * @brief spinOnce Send data to VIO pipeline on a per-frame basis + * @return if the dataset finished or not + */ + virtual bool spinOnce(); + + /** + * @brief parse Parses Euroc dataset. This is done already in spin() and + * does not need to be called by the user. Left in public for experimentation. + */ + void parse(); + + /** + * @brief sendImuData We send IMU data first (before frames) so that the VIO + * pipeline can query all IMU data between frames. + */ + void sendImuData() const; + + /** + * @brief parseDataset Parse camera, gt, and imu data if using + * different Euroc format. + * @return + */ + bool parseDataset(); + + //! Parsers + bool parseImuData(const std::string& input_dataset_path, const std::string& imu_name); + + bool parseGtData(const std::string& input_dataset_path, const std::string& gtSensorName); + + bool parseCameraData(const std::string& cam_name, CameraImageLists* cam_list_i); + + //! 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; + + protected: + + VioParams vio_params_; + SpectacularLog dataset_; + + std::string dataset_name_; + std::string dataset_path_; + + FrameId current_k_; + FrameId initial_k_; // start frame + FrameId final_k_; // end frame + + //! Flag to signal when the dataset has been parsed. + bool dataset_parsed_ = false; + //! 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_; + + // EurocGtLogger::UniquePtr logger_; +}; + +} // namespace 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..2fde0ad9 --- /dev/null +++ b/experimental/overhead_matching/kimera_vio_pipeline_test.cc @@ -0,0 +1,87 @@ +#include +#include + +#include +#include +#include +#include + +#include "experimental/overhead_matching/kimera_spectacular_data_provider.hh" + +#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); + + // Build dataset parser. + VIO::DataProviderInterface::Ptr dataset_parser = std::make_unique(vio_params); + + CHECK(dataset_parser); + + VIO::Pipeline::Ptr vio_pipeline; + + switch (vio_params.frontend_type_) { + case VIO::FrontendType::kMonoImu: { + vio_pipeline = std::make_unique(vio_params); + } break; + default: { + LOG(FATAL) << "Unrecognized Frontend type: " + << VIO::to_underlying(vio_params.frontend_type_) + << ". 0: Mono, 1: Stereo."; + } break; + } + + // Register callback to shutdown data provider in case VIO pipeline + // shutsdown. + vio_pipeline->registerShutdownCallback( + std::bind(&VIO::DataProviderInterface::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)); + + + // 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; +} From eb581fb913ccd61fc0d4820c4615ab50b65b85ce Mon Sep 17 00:00:00 2001 From: Ethan Fahnestock Date: Tue, 3 Dec 2024 14:44:11 -0500 Subject: [PATCH 2/8] first pass at IMU translation, adding tests --- experimental/overhead_matching/BUILD | 9 + .../kimera_spectacular_data_provider.cc | 672 ++++++------------ .../kimera_spectacular_data_provider.hh | 70 +- .../kimera_spectacular_data_provider_test.cc | 51 ++ .../kimera_vio_pipeline_test.cc | 24 +- 5 files changed, 323 insertions(+), 503 deletions(-) create mode 100644 experimental/overhead_matching/kimera_spectacular_data_provider_test.cc diff --git a/experimental/overhead_matching/BUILD b/experimental/overhead_matching/BUILD index 2a4ee287..1de903b7 100644 --- a/experimental/overhead_matching/BUILD +++ b/experimental/overhead_matching/BUILD @@ -35,6 +35,15 @@ cc_library( ":spectacular_log", ] ) +cc_test( + name = "kimera_spectacular_data_provider_test", + srcs = ["kimera_spectacular_data_provider_test.cc"], + data = ["@spectacular_log_snippet//:files"], + deps = [ + ":kimera_spectacular_data_provider", + "@com_google_googletest//:gtest_main", + ] +) cc_test( name = "kimera_vio_pipeline_test", srcs = ["kimera_vio_pipeline_test.cc"], diff --git a/experimental/overhead_matching/kimera_spectacular_data_provider.cc b/experimental/overhead_matching/kimera_spectacular_data_provider.cc index 80200137..a41cc5ef 100644 --- a/experimental/overhead_matching/kimera_spectacular_data_provider.cc +++ b/experimental/overhead_matching/kimera_spectacular_data_provider.cc @@ -1,5 +1,11 @@ #include "experimental/overhead_matching/kimera_spectacular_data_provider.hh" +#include +#include +#include +#include +#include + #include // for max #include #include @@ -7,521 +13,287 @@ #include // for pair<> #include -#include -#include -#include - -#include -#include -#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", +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, +DEFINE_int64(initial_k, 0, "Initial frame to start processing dataset, " "previous frames will not be used."); -DEFINE_int64(final_k, - 100000, +DEFINE_int64(final_k, 100000, "Final frame to finish processing dataset, " "subsequent frames will not be used."); namespace robot::experimental::overhead_matching { -SpectacularLogDataProvider::SpectacularLogDataProvider(const std::string& dataset_path, - const int& initial_k, - const int& final_k, - const VioParams& vio_params) +/* -------------------------------------------------------------------------- */ +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::max()), + spec_log_(dataset_path), + current_k_(std::numeric_limits::max()), initial_k_(initial_k), final_k_(final_k), - imu_measurements_()) { - - CHECK(!dataset_path_.empty()) - << "Dataset path for SpectacularLogDataProvider 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); - - // Finish processing dataset at frame final_k. - // Last frame to process (to avoid processing the entire dataset), - // skip last frames. - CHECK_GT(final_k_, 0); - - CHECK_GT(final_k_, initial_k_) << "Value for final_k (" << final_k_ - << ") is smaller than value for" - << " initial_k (" << initial_k_ << ")."; - current_k_ = initial_k_; - - // Parse the actual dataset first, then run it. - if (!shutdown_ && !dataset_parsed_) { - LOG(INFO) << "Parsing Spectacular dataset..."; - parse(); - CHECK_GT(imu_measurements_.size(), 0u); - dataset_parsed_ = true; - } + imu_measurements_() { + CHECK(!dataset_path_.empty()) << "Dataset path for EurocDataProvider 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); + + // Finish processing dataset at frame final_k. + // Last frame to process (to avoid processing the entire dataset), + // skip last frames. + CHECK_GT(final_k_, 0); + + CHECK_GT(final_k_, initial_k_) + << "Value for final_k (" << final_k_ << ") is smaller than value for" + << " initial_k (" << initial_k_ << ")."; + current_k_ = initial_k_; } /* -------------------------------------------------------------------------- */ -SpectacularLogDataProvider::SpectacularLogDataProvider(const VioParams& vio_params) - : SpectacularLogDataProvider(FLAGS_dataset_path, - FLAGS_initial_k, - FLAGS_final_k, - vio_params) {} +SpectacularDataProviderInterface::SpectacularDataProviderInterface(const VIO::VioParams& vio_params) + : SpectacularDataProviderInterface(FLAGS_dataset_path, FLAGS_initial_k, FLAGS_final_k, + vio_params) {} /* -------------------------------------------------------------------------- */ -SpectacularLogDataProvider::~SpectacularLogDataProvider() { - LOG(INFO) << "SpectacularLogDataProvider destructor called."; +SpectacularDataProviderInterface::~SpectacularDataProviderInterface() { + LOG(INFO) << "SpectacularDataProviderInterface destructor called."; } /* -------------------------------------------------------------------------- */ -bool SpectacularLogDataProvider::spin() { - if (dataset_parsed_) { +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; + // 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. CHECK_EQ(vio_params_.camera_params_.size(), 2u); CHECK_GT(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_; + 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; - } + if (!vio_params_.parallel_run_) { + // Return, instead of blocking, when running in sequential mode. + return true; + } } - } else { - LOG(ERROR) << "Euroc dataset was not parsed."; - } - LOG_IF(INFO, shutdown_) << "SpectacularLogDataProvider shutdown requested."; - return false; -} - -bool SpectacularLogDataProvider::hasData() const { - return current_k_ < final_k_; -} - -/* -------------------------------------------------------------------------- */ -bool SpectacularLogDataProvider::spinOnce() { - CHECK_LT(current_k_, std::numeric_limits::max()) - << "Are you sure you've initialized current_k_?"; - if (current_k_ >= final_k_) { - LOG(INFO) << "Finished spinning Euroc dataset."; + LOG_IF(INFO, shutdown_) << "EurocDataProvider shutdown requested."; return false; - } - - const CameraParams& left_cam_info = vio_params_.camera_params_.at(0); - const CameraParams& right_cam_info = vio_params_.camera_params_.at(1); - const bool& equalize_image = - vio_params_.frontend_params_.stereo_matching_params_.equalize_image_; - - const Timestamp& timestamp_frame_k = timestampAtFrame(current_k_); - VLOG(10) << "Sending left/right frames k= " << current_k_ - << " with timestamp: " << timestamp_frame_k; - - // TODO(Toni): ideally only send cv::Mat raw images...: - // - pass params to vio_pipeline ctor - // - make vio_pipeline actually equalize or transform images as necessary. - std::string left_img_filename; - bool available_left_img = getLeftImgName(current_k_, &left_img_filename); - std::string right_img_filename; - bool available_right_img = getRightImgName(current_k_, &right_img_filename); - if (available_left_img && available_right_img) { - // Both stereo images are available, send data to VIO - CHECK(left_frame_callback_); - left_frame_callback_( - std::make_unique(current_k_, - timestamp_frame_k, - // TODO(Toni): this info should be passed to - // the camera... not all the time here... - left_cam_info, - UtilsOpenCV::ReadAndConvertToGrayScale( - left_img_filename, equalize_image))); - CHECK(right_frame_callback_); - right_frame_callback_( - std::make_unique(current_k_, - timestamp_frame_k, - // TODO(Toni): this info should be passed to - // the camera... not all the time here... - right_cam_info, - UtilsOpenCV::ReadAndConvertToGrayScale( - right_img_filename, equalize_image))); - } else { - LOG(ERROR) << "Missing left/right stereo pair, proceeding to the next one."; - } - - // This is done directly when parsing the Imu data. - // imu_single_callback_(imu_meas); - - VLOG(10) << "Finished VIO processing for frame k = " << current_k_; - current_k_++; - return true; -} - -void SpectacularLogDataProvider::sendImuData() const { - CHECK(imu_single_callback_) << "Did you forget to register the IMU callback?"; - Timestamp previous_timestamp = -1; - for (const ImuMeasurement& imu_meas : imu_measurements_) { - CHECK_GT(imu_meas.timestamp_, previous_timestamp) - << "Euroc IMU data is not in chronological order!"; - previous_timestamp = imu_meas.timestamp_; - imu_single_callback_(imu_meas); - } } -/* -------------------------------------------------------------------------- */ -void SpectacularLogDataProvider::parse() { - VLOG(100) << "Using dataset path: " << dataset_path_; - // Parse the dataset (ETH format). - parseDataset(); - if (VLOG_IS_ON(1)) print(); - - // Send first ground-truth pose to VIO for initialization if requested. - if (vio_params_.backend_params_->autoInitialize_ == 0) { - // We want to initialize from ground-truth. - vio_params_.backend_params_->initial_ground_truth_state_ = - getGroundTruthState(timestampAtFrame(initial_k_)); - } -} +bool SpectacularDataProviderInterface::hasData() const { return current_k_ < final_k_; } /* -------------------------------------------------------------------------- */ -bool SpectacularLogDataProvider::parseImuData(const std::string& input_dataset_path, - const std::string& imuName) { - ///////////////// PARSE ACTUAL DATA ////////////////////////////////////////// - //#timestamp [ns],w_RS_S_x [rad s^-1],w_RS_S_y [rad s^-1],w_RS_S_z [rad s^-1], - // a_RS_S_x [m s^-2],a_RS_S_y [m s^-2],a_RS_S_z [m s^-2] - std::string filename_data = - input_dataset_path + "/mav0/" + imuName + "/data.csv"; - std::ifstream fin(filename_data.c_str()); - LOG_IF(FATAL, !fin.is_open()) << "Cannot open file: " << filename_data; - - // Skip the first line, containing the header. - std::string line; - std::getline(fin, line); - - size_t deltaCount = 0u; - Timestamp sumOfDelta = 0; - double stdDelta = 0; - double imu_rate_maxMismatch = 0; - double maxNormAcc = 0, maxNormRotRate = 0; // only for debugging - Timestamp previous_timestamp = -1; - - // Read/store imu measurements, line by line. - while (std::getline(fin, line)) { - Timestamp timestamp = 0; - gtsam::Vector6 gyr_acc_data; - for (int i = 0u; i < gyr_acc_data.size() + 1u; i++) { - int idx = line.find_first_of(','); - if (i == 0) { - timestamp = std::stoll(line.substr(0, idx)); - } else { - gyr_acc_data(i - 1) = std::stod(line.substr(0, idx)); - } - line = line.substr(idx + 1); - } - CHECK_GT(timestamp, previous_timestamp) - << "Euroc IMU data is not in chronological order!"; - Vector6 imu_accgyr; - // Acceleration first! - imu_accgyr << gyr_acc_data.tail(3), gyr_acc_data.head(3); - - double normAcc = gyr_acc_data.tail(3).norm(); - if (normAcc > maxNormAcc) maxNormAcc = normAcc; - - double normRotRate = gyr_acc_data.head(3).norm(); - if (normRotRate > maxNormRotRate) maxNormRotRate = normRotRate; - - //! Store imu measurements - imu_measurements_.push_back(ImuMeasurement(timestamp, imu_accgyr)); - - if (previous_timestamp != -1) { - sumOfDelta += (timestamp - previous_timestamp); - double deltaMismatch = - std::fabs(static_cast( - timestamp - previous_timestamp - - vio_params_.imu_params_.nominal_sampling_time_s_) * - 1e-9); - stdDelta += std::pow(deltaMismatch, 2); - imu_rate_maxMismatch = std::max(imu_rate_maxMismatch, deltaMismatch); - deltaCount += 1u; +bool SpectacularDataProviderInterface::spinOnce() { + CHECK_LT(current_k_, std::numeric_limits::max()) + << "Are you sure you've initialized current_k_?"; + if (current_k_ >= final_k_) { + LOG(INFO) << "Finished spinning Euroc dataset."; + return false; } - previous_timestamp = timestamp; - } - - // Converted to seconds. - VLOG(10) << "IMU rate: " - << (static_cast(sumOfDelta) / - static_cast(deltaCount)) * - 1e-9 - << '\n' - << "IMU rate std: " - << std::sqrt(stdDelta / static_cast(deltaCount - 1u)) << '\n' - << "IMU rate max mismatch: " << imu_rate_maxMismatch << '\n' - << "Maximum measured rotation rate (norm):" << maxNormRotRate << '\n' - << "Maximum measured acceleration (norm): " << maxNormAcc; - fin.close(); - - return true; + + const VIO::CameraParams& left_cam_info = vio_params_.camera_params_.at(0); + const bool& equalize_image = + vio_params_.frontend_params_.stereo_matching_params_.equalize_image_; + + const VIO::Timestamp& timestamp_frame_k = timestampAtFrame(current_k_); + VLOG(10) << "Sending left frames k= " << current_k_ << " with timestamp: " << timestamp_frame_k; + + // TODO(Toni): ideally only send cv::Mat raw images...: + // - pass params to vio_pipeline ctor + // - make vio_pipeline actually equalize or transform images as necessary. + + // std::string left_img_filename; + // bool available_left_img = getLeftImgName(current_k_, &left_img_filename); + // if (available_left_img) { + // // Both stereo images are available, send data to VIO + // CHECK(left_frame_callback_); + // left_frame_callback_(std::make_unique( + // current_k_, timestamp_frame_k, + // // TODO(Toni): this info should be passed to + // // the camera... not all the time here... + // left_cam_info, + // VIO::UtilsOpenCV::ReadAndConvertToGrayScale(left_img_filename, equalize_image))); + // } else { + // LOG(ERROR) << "Missing left image, proceeding to the next one."; + // } + // This is done directly when parsing the Imu data. + // imu_single_callback_(imu_meas); + + VLOG(10) << "Finished VIO processing for frame k = " << current_k_; + current_k_++; + return true; } -/* -------------------------------------------------------------------------- */ -bool SpectacularLogDataProvider::parseDataset() { - // Parse IMU data. - CHECK(parseImuData(dataset_path_, kImuName)); - - // Parse Camera data. - CameraImageLists left_cam_image_list; - CameraImageLists right_cam_image_list; - parseCameraData(kLeftCamName, &left_cam_image_list); - if (VLOG_IS_ON(1)) left_cam_image_list.print(); - parseCameraData(kRightCamName, &right_cam_image_list); - if (VLOG_IS_ON(1)) right_cam_image_list.print(); - // TODO(Toni): remove camera_names_ and camera_image_lists_... - camera_names_.push_back(kLeftCamName); - camera_names_.push_back(kRightCamName); - // WARNING Use [x] not .at() because we are adding entries that do not exist. - camera_image_lists_[kLeftCamName] = left_cam_image_list; - camera_image_lists_[kRightCamName] = right_cam_image_list; - // CHECK(sanityCheckCameraData(camera_names_, &camera_image_lists_)); - - // Parse Ground-Truth data. - static const std::string ground_truth_name = "state_groundtruth_estimate0"; - is_gt_available_ = parseGtData(dataset_path_, ground_truth_name); - - clipFinalFrame(); - - // Log Ground-Truth data. - if (logger_) { - if (is_gt_available_) { - logger_->logGtData(dataset_path_ + "/mav0/" + ground_truth_name + - "/data.csv"); - } else { - LOG(ERROR) << "Requested ground-truth data logging but no ground-truth " - "data available."; - } - } +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(robot_imu.time_of_validity.time_since_epoch() + .count(), // timestamps are int64_t nanoseconds + imu_acc_gyro); - return true; + return vio_imu; } -/* -------------------------------------------------------------------------- */ -bool SpectacularLogDataProvider::parseCameraData(const std::string& cam_name, - CameraImageLists* cam_list_i) { - CHECK_NOTNULL(cam_list_i) - ->parseCamImgList(dataset_path_ + "/mav0/" + cam_name, "data.csv"); - return true; +void SpectacularDataProviderInterface::sendImuData() const { + 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 + time::RobotTimestamp t_robot = time::RobotTimestamp() + time::as_duration(t); + if (t < std::chrono::duration(min_imu_time.time_since_epoch()).count()) { + continue; + } + if (t > std::chrono::duration(max_imu_time.time_since_epoch()).count()) { + break; + } + std::optional imu_sample = spec_log_.get_imu_sample(t_robot); + CHECK(imu_sample.has_value()); + auto vio_imu = vio_imu_from_robot_imu(imu_sample.value()); + imu_single_callback_(vio_imu); + } } -std::string SpectacularLogDataProvider::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); +// /* -------------------------------------------------------------------------- */ +// bool SpectacularDataProviderInterface::parseDataset() { +// // Parse IMU data. +// CHECK(parseImuData(dataset_path_, kImuName)); + +// // Parse Camera data. +// VIO::CameraImageLists left_cam_image_list; +// // parseCameraData(kLeftCamName, &left_cam_image_list); +// // if (VLOG_IS_ON(1)) left_cam_image_list.print(); +// // TODO(Toni): remove camera_names_ and camera_image_lists_... +// camera_names_.push_back(kLeftCamName); +// // WARNING Use [x] not .at() because we are adding entries that do not exist. +// camera_image_lists_[kLeftCamName] = left_cam_image_list; +// // CHECK(sanityCheckCameraData(camera_names_, &camera_image_lists_)); + +// // Parse Ground-Truth data. + +// clipFinalFrame(); + +// return true; +// } + +// /* -------------------------------------------------------------------------- */ +// bool SpectacularDataProviderInterface::parseCameraData(const std::string& cam_name, +// VIO::CameraImageLists* cam_list_i) { +// CHECK_NOTNULL(cam_list_i)->parseCamImgList(dataset_path_ + "/mav0/" + cam_name, "data.csv"); +// return true; +// } + +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_; } - LOG(INFO) << "Dataset name: " << dataset_name_; - } - return dataset_name_; + return dataset_name_; } /* -------------------------------------------------------------------------- */ -size_t SpectacularLogDataProvider::getNumImages() const { - CHECK_GT(camera_names_.size(), 0u); - const std::string& left_cam_name = camera_names_.at(0); - const std::string& right_cam_name = camera_names_.at(0); - size_t n_left_images = getNumImagesForCamera(left_cam_name); - size_t n_right_images = getNumImagesForCamera(right_cam_name); - CHECK_EQ(n_left_images, n_right_images); - return n_left_images; +size_t SpectacularDataProviderInterface::getNumImages() const { + CHECK_GT(camera_names_.size(), 0u); + const std::string& left_cam_name = camera_names_.at(0); + const std::string& right_cam_name = camera_names_.at(0); + size_t n_left_images = getNumImagesForCamera(left_cam_name); + size_t n_right_images = getNumImagesForCamera(right_cam_name); + CHECK_EQ(n_left_images, n_right_images); + return n_left_images; } /* -------------------------------------------------------------------------- */ -size_t SpectacularLogDataProvider::getNumImagesForCamera( +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 SpectacularLogDataProvider::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; + const auto& iter = camera_image_lists_.find(camera_name); + CHECK(iter != camera_image_lists_.end()); + return iter->second.getNumImages(); } /* -------------------------------------------------------------------------- */ -Timestamp SpectacularLogDataProvider::timestampAtFrame(const 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; +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; } -void SpectacularLogDataProvider::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_; - } - CHECK_LE(final_k_, nr_images); -} /* -------------------------------------------------------------------------- */ -void SpectacularLogDataProvider::print() const { - LOG(INFO) << "------------------ ETHDatasetParser::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(); - } - if (FLAGS_minloglevel < 1) { - gt_data_.print(); - } - LOG(INFO) << "-------------------------------------------------------------"; +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; } -////////////////////////////////////////////////////////////////////////////// - -/* -------------------------------------------------------------------------- */ -MonoEurocDataProvider::MonoEurocDataProvider(const std::string& dataset_path, - const int& initial_k, - const int& final_k, - const VioParams& vio_params) - : SpectacularLogDataProvider(dataset_path, initial_k, final_k, vio_params) {} - -/* -------------------------------------------------------------------------- */ -MonoEurocDataProvider::MonoEurocDataProvider(const VioParams& vio_params) - : SpectacularLogDataProvider(vio_params) {} - -/* -------------------------------------------------------------------------- */ -MonoEurocDataProvider::~MonoEurocDataProvider() { - LOG(INFO) << "Mono ETHDataParser destructor called."; -} - -/* -------------------------------------------------------------------------- */ -bool MonoEurocDataProvider::spin() { - if (dataset_parsed_) { - 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; +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_; } - - // Spin. - // CHECK_EQ(vio_params_.camera_params_.size(), 2u); - CHECK_GT(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; - } - } - } else { - LOG(ERROR) << "Euroc dataset was not parsed."; - } - LOG_IF(INFO, shutdown_) << "SpectacularLogDataProvider shutdown requested."; - return false; + CHECK_LE(final_k_, nr_images); } - /* -------------------------------------------------------------------------- */ -bool MonoEurocDataProvider::spinOnce() { - CHECK_LT(current_k_, std::numeric_limits::max()) - << "Are you sure you've initialized current_k_?"; - if (current_k_ >= final_k_) { - LOG(INFO) << "Finished spinning Euroc dataset."; - return false; - } - - const CameraParams& left_cam_info = vio_params_.camera_params_.at(0); - const bool& equalize_image = - vio_params_.frontend_params_.stereo_matching_params_.equalize_image_; - - const Timestamp& timestamp_frame_k = timestampAtFrame(current_k_); - VLOG(10) << "Sending left frame k= " << current_k_ - << " with timestamp: " << timestamp_frame_k; - - // TODO(Toni): ideally only send cv::Mat raw images...: - // - pass params to vio_pipeline ctor - // - make vio_pipeline actually equalize or transform images as necessary. - std::string left_img_filename; - bool available_left_img = getLeftImgName(current_k_, &left_img_filename); - if (available_left_img) { - // Both stereo images are available, send data to VIO - CHECK(left_frame_callback_); - left_frame_callback_( - std::make_unique(current_k_, - timestamp_frame_k, - // TODO(Toni): this info should be passed to - // the camera... not all the time here... - left_cam_info, - UtilsOpenCV::ReadAndConvertToGrayScale( - left_img_filename, equalize_image))); - } else { - LOG(ERROR) << "Missing left image, proceeding to the next one."; - } - - // This is done directly when parsing the Imu data. - // imu_single_callback_(imu_meas); - - VLOG(10) << "Finished VIO processing for frame k = " << current_k_; - current_k_++; - return true; +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 +} // 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 518f016a..4d96e83d 100644 --- a/experimental/overhead_matching/kimera_spectacular_data_provider.hh +++ b/experimental/overhead_matching/kimera_spectacular_data_provider.hh @@ -1,4 +1,3 @@ - #pragma once #include @@ -23,19 +22,22 @@ namespace robot::experimental::overhead_matching { -class SpectacularLogDataProvider : public DataProviderInterface { +/* + * Parse all images and camera calibration for an ETH dataset. + */ +class SpectacularDataProviderInterface : public VIO::DataProviderInterface { public: - KIMERA_DELETE_COPY_CONSTRUCTORS(SpectacularLogDataProvider); - KIMERA_POINTER_TYPEDEFS(SpectacularLogDataProvider); + KIMERA_DELETE_COPY_CONSTRUCTORS(SpectacularDataProviderInterface); + KIMERA_POINTER_TYPEDEFS(SpectacularDataProviderInterface); EIGEN_MAKE_ALIGNED_OPERATOR_NEW //! Ctor with params. - SpectacularLogDataProvider(const std::string& dataset_path, const int& initial_k, - const int& final_k, const VioParams& vio_params); + SpectacularDataProviderInterface(const std::string& dataset_path, const int& initial_k, + const int& final_k, const VIO::VioParams& vio_params); //! Ctor from gflags - explicit SpectacularLogDataProvider(const VioParams& vio_params); + explicit SpectacularDataProviderInterface(const VIO::VioParams& vio_params); - virtual ~SpectacularLogDataProvider(); + virtual ~SpectacularDataProviderInterface(); public: /** @@ -64,31 +66,12 @@ class SpectacularLogDataProvider : public DataProviderInterface { */ virtual bool spinOnce(); - /** - * @brief parse Parses Euroc dataset. This is done already in spin() and - * does not need to be called by the user. Left in public for experimentation. - */ - void parse(); - /** * @brief sendImuData We send IMU data first (before frames) so that the VIO * pipeline can query all IMU data between frames. */ void sendImuData() const; - /** - * @brief parseDataset Parse camera, gt, and imu data if using - * different Euroc format. - * @return - */ - bool parseDataset(); - - //! Parsers - bool parseImuData(const std::string& input_dataset_path, const std::string& imu_name); - - bool parseGtData(const std::string& input_dataset_path, const std::string& gtSensorName); - - bool parseCameraData(const std::string& cam_name, CameraImageLists* cam_list_i); //! Getters. /** @@ -100,7 +83,6 @@ class SpectacularLogDataProvider : public DataProviderInterface { 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; /** @@ -113,20 +95,30 @@ class SpectacularLogDataProvider : public DataProviderInterface { 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(); + 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_; - VioParams vio_params_; - SpectacularLog dataset_; - + bool is_gt_available_; std::string dataset_name_; std::string dataset_path_; - FrameId current_k_; - FrameId initial_k_; // start frame - FrameId final_k_; // end frame + VIO::FrameId current_k_; + VIO::FrameId initial_k_; // start frame + VIO::FrameId final_k_; // end frame - //! Flag to signal when the dataset has been parsed. - bool dataset_parsed_ = false; //! Flag to signal if the IMU data has been sent to the VIO pipeline bool is_imu_data_sent_ = false; @@ -134,9 +126,9 @@ class SpectacularLogDataProvider : public DataProviderInterface { const std::string kImuName = "imu0"; //! Pre-stored imu-measurements - std::vector imu_measurements_; + std::vector imu_measurements_; - // EurocGtLogger::UniquePtr logger_; + SpectacularLog spec_log_; }; -} // namespace +} // 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..9a1dea14 --- /dev/null +++ b/experimental/overhead_matching/kimera_spectacular_data_provider_test.cc @@ -0,0 +1,51 @@ + +#include "experimental/overhead_matching/spectacular_log.hh" +#include "experimental/overhead_matching/kimera_spectacular_data_provider.hh" + +#include +#include + +#include "fmt/format.h" +#include "gtest/gtest.h" +#include "opencv2/opencv.hpp" + +#include "kimera-vio/pipeline/Pipeline-definitions.h" + +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) + << std::chrono::duration(t.time_since_epoch()).count(); + out << ss.str(); + return out; +} + +TEST(KimeraSpectacularDataProviderTest, happy_case) { + // Setup + const std::filesystem::path log_path( + "external/spectacular_log_snippet/recording_2024-11-21_13-36-30"); + SpectacularLog log(log_path); + + const std::filesystem::path vio_config_path(""); // loads default params + VIO::VioParams vio_params(vio_config_path); + + SpectacularDataProviderInterface s_interface( + log_path, 0, std::numeric_limits::max(), vio_params + ); + + // Action + +} + +} // 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 index 2fde0ad9..23834108 100644 --- a/experimental/overhead_matching/kimera_vio_pipeline_test.cc +++ b/experimental/overhead_matching/kimera_vio_pipeline_test.cc @@ -8,11 +8,10 @@ #include "experimental/overhead_matching/kimera_spectacular_data_provider.hh" -#include "kimera-vio/frontend/StereoImuSyncPacket.h" #include "kimera-vio/logging/Logger.h" -#include "kimera-vio/pipeline/MonoImuPipeline.h" +#include "kimera-vio/pipeline/RgbdImuPipeline.h" #include "kimera-vio/pipeline/Pipeline.h" -#include "kimera-vio/pipeline/StereoImuPipeline.h" +#include "kimera-vio/dataprovider/DataProviderInterface.h" #include "kimera-vio/utils/Statistics.h" #include "kimera-vio/utils/Timer.h" @@ -20,6 +19,9 @@ 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. @@ -37,21 +39,12 @@ int main(int argc, char* argv[]) { VIO::Pipeline::Ptr vio_pipeline; - switch (vio_params.frontend_type_) { - case VIO::FrontendType::kMonoImu: { - vio_pipeline = std::make_unique(vio_params); - } break; - default: { - LOG(FATAL) << "Unrecognized Frontend type: " - << VIO::to_underlying(vio_params.frontend_type_) - << ". 0: Mono, 1: Stereo."; - } break; - } + vio_pipeline = std::make_unique(vio_params); // Register callback to shutdown data provider in case VIO pipeline // shutsdown. vio_pipeline->registerShutdownCallback( - std::bind(&VIO::DataProviderInterface::shutdown, dataset_parser)); + std::bind(&VIO::DataProviderModule::shutdown, dataset_parser)); // Register callback to vio pipeline. dataset_parser->registerImuSingleCallback(std::bind( @@ -61,6 +54,9 @@ int main(int argc, char* argv[]) { 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(); From c13089ee7506c794f4a606688575d5a092931adf Mon Sep 17 00:00:00 2001 From: Ethan Fahnestock Date: Fri, 6 Dec 2024 14:14:02 -0500 Subject: [PATCH 3/8] adding imu tests --- .../kimera_spectacular_data_provider_test.cc | 73 +++++++++++++++++++ .../kimera_vio_pipeline_test.cc | 2 +- 2 files changed, 74 insertions(+), 1 deletion(-) diff --git a/experimental/overhead_matching/kimera_spectacular_data_provider_test.cc b/experimental/overhead_matching/kimera_spectacular_data_provider_test.cc index 9a1dea14..0c35c195 100644 --- a/experimental/overhead_matching/kimera_spectacular_data_provider_test.cc +++ b/experimental/overhead_matching/kimera_spectacular_data_provider_test.cc @@ -31,20 +31,93 @@ std::ostream &operator<<(std::ostream &out, const time::RobotTimestamp &t) { 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()) { + std::cout << "kimera timestamp " << kimera_imu.timestamp_ << std::endl; + std::cout << "robot timestamp " << robot_imu.time_of_validity.time_since_epoch().count() << std::endl; + std::cout << "diff " << robot_imu.time_of_validity.time_since_epoch().count() - kimera_imu.timestamp_ << std::endl; + std::cout << "timestamp" << std::endl; + return false; + } + // accel and gyro values + if (kimera_imu.acc_gyr_.rows() != 6 || kimera_imu.acc_gyr_.cols() != 1) { + std::cout << "shapes" << std::endl; + 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) { + std::cout << "imu " << i << std::endl; + return false; + } + } + return true; + } + TEST(KimeraSpectacularDataProviderTest, happy_case) { // Setup const std::filesystem::path log_path( "external/spectacular_log_snippet/recording_2024-11-21_13-36-30"); 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; + + std::cout << "VIO params:" << std::endl; + vio_params.print(); SpectacularDataProviderInterface s_interface( log_path, 0, std::numeric_limits::max(), vio_params ); + std::vector imu_queue; + + auto imu_callback = [&imu_queue](const VIO::ImuMeasurement& measurement) -> void { + imu_queue.push_back(measurement); + return; + }; + + // bind IMU callback + s_interface.registerImuSingleCallback(imu_callback); + // Action + s_interface.spin(); + + // Verification + std::cout << "Size of imu queue: " << imu_queue.size() << std::endl; + 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; + } + std::cout << "all of the imu information checked out!" << std::endl; } diff --git a/experimental/overhead_matching/kimera_vio_pipeline_test.cc b/experimental/overhead_matching/kimera_vio_pipeline_test.cc index 23834108..397bbaf7 100644 --- a/experimental/overhead_matching/kimera_vio_pipeline_test.cc +++ b/experimental/overhead_matching/kimera_vio_pipeline_test.cc @@ -33,7 +33,7 @@ int main(int argc, char* argv[]) { VIO::VioParams vio_params(FLAGS_params_folder_path); // Build dataset parser. - VIO::DataProviderInterface::Ptr dataset_parser = std::make_unique(vio_params); + VIO::DataProviderInterface::Ptr dataset_parser = std::make_unique(vio_params); CHECK(dataset_parser); From fb523ef1aca0db5e14e95d77d08d0a26e44f6e48 Mon Sep 17 00:00:00 2001 From: Ethan Fahnestock Date: Mon, 9 Dec 2024 11:33:45 -0500 Subject: [PATCH 4/8] adding patch for opengv --- WORKSPACE | 3 ++- third_party/opengv_0002-unused-parameter.patch | 13 +++++++++++++ 2 files changed, 15 insertions(+), 1 deletion(-) create mode 100644 third_party/opengv_0002-unused-parameter.patch diff --git a/WORKSPACE b/WORKSPACE index 3ba69c91..4c9f72be 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=" ) 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; From cb7219b500458d247197fda30fd5c1eb40e32ee4 Mon Sep 17 00:00:00 2001 From: Ethan Fahnestock Date: Mon, 9 Dec 2024 11:58:19 -0500 Subject: [PATCH 5/8] integrating patches to kimera vio and updating sha --- WORKSPACE | 7 +--- ...kimera_vio_0001-add-missing-includes.patch | 37 ------------------- ...kimera_vio_0002-add-override-markers.patch | 26 ------------- 3 files changed, 1 insertion(+), 69 deletions(-) delete mode 100644 third_party/kimera_vio_0001-add-missing-includes.patch delete mode 100644 third_party/kimera_vio_0002-add-override-markers.patch diff --git a/WORKSPACE b/WORKSPACE index 4c9f72be..26e4a637 100644 --- a/WORKSPACE +++ b/WORKSPACE @@ -540,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( 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(); } - }; - - /** From 1cdc33edd05dcab0648d88348af7072a6292ed2b Mon Sep 17 00:00:00 2001 From: Ethan Fahnestock Date: Mon, 9 Dec 2024 16:54:24 -0500 Subject: [PATCH 6/8] finishing up frame delivery --- experimental/overhead_matching/BUILD | 41 +++--- .../kimera_spectacular_data_provider.cc | 131 ++++++++---------- .../kimera_spectacular_data_provider.hh | 1 - .../kimera_spectacular_data_provider_test.cc | 90 ++++++++++-- .../overhead_matching/spectacular_log.cc | 4 + 5 files changed, 165 insertions(+), 102 deletions(-) diff --git a/experimental/overhead_matching/BUILD b/experimental/overhead_matching/BUILD index cb39c7b1..ad1b2cd0 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") @@ -30,13 +28,32 @@ cc_test( ":spectacular_log", "@com_google_googletest//:gtest_main", "@fmt", + "//common:video", ] ) +pybind_extension( + name = "spectacular_log_python", + srcs = ["spectacular_log_python.cc"], + data = ["//common/time:robot_time_python.so"], + deps = [":spectacular_log"], +) + +py_binary( + name = "spectacular_log_to_rosbag", + srcs = ["spectacular_log_to_rosbag.py"], + data = [":spectacular_log_python.so"], + deps = [ + requirement("rosbags"), + 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"], deps = [ "@kimera_vio", ":spectacular_log", @@ -46,31 +63,19 @@ cc_test( name = "kimera_spectacular_data_provider_test", srcs = ["kimera_spectacular_data_provider_test.cc"], data = ["@spectacular_log_snippet//:files"], + copts = ["-Wno-sign-compare"], deps = [ ":kimera_spectacular_data_provider", "@com_google_googletest//:gtest_main", + "//common:video", ] ) cc_test( name = "kimera_vio_pipeline_test", srcs = ["kimera_vio_pipeline_test.cc"], + copts = ["-Wno-sign-compare"], deps = [ ":kimera_spectacular_data_provider", "@com_google_googletest//:gtest_main", ] -pybind_extension( - name = "spectacular_log_python", - srcs = ["spectacular_log_python.cc"], - data = ["//common/time:robot_time_python.so"], - deps = [":spectacular_log"], -) - -py_binary( - name = "spectacular_log_to_rosbag", - srcs = ["spectacular_log_to_rosbag.py"], - data = [":spectacular_log_python.so"], - deps = [ - requirement("rosbags"), - requirement("numpy"), - ], -) +) \ 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 a41cc5ef..e00dde43 100644 --- a/experimental/overhead_matching/kimera_spectacular_data_provider.cc +++ b/experimental/overhead_matching/kimera_spectacular_data_provider.cc @@ -1,7 +1,7 @@ #include "experimental/overhead_matching/kimera_spectacular_data_provider.hh" #include -#include +// #include #include #include #include @@ -29,6 +29,11 @@ DEFINE_int64(final_k, 100000, 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, @@ -37,12 +42,12 @@ SpectacularDataProviderInterface::SpectacularDataProviderInterface(const std::st : DataProviderInterface(), vio_params_(vio_params), dataset_path_(dataset_path), - spec_log_(dataset_path), current_k_(std::numeric_limits::max()), initial_k_(initial_k), final_k_(final_k), - imu_measurements_() { - CHECK(!dataset_path_.empty()) << "Dataset path for EurocDataProvider is empty."; + imu_measurements_(), + spec_log_(dataset_path) { + // 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). @@ -51,6 +56,12 @@ SpectacularDataProviderInterface::SpectacularDataProviderInterface(const std::st // 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(); + } + CHECK_GT(final_k_, 0); CHECK_GT(final_k_, initial_k_) @@ -94,7 +105,7 @@ bool SpectacularDataProviderInterface::spin() { return true; } } - LOG_IF(INFO, shutdown_) << "EurocDataProvider shutdown requested."; + LOG_IF(INFO, shutdown_) << "shutdown requested."; return false; } @@ -103,50 +114,61 @@ 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_?"; + // << "Are you sure you've initialized current_k_?"; if (current_k_ >= final_k_) { - LOG(INFO) << "Finished spinning Euroc dataset."; + LOG(INFO) << "Finished spinning dataset."; return false; } - - const VIO::CameraParams& left_cam_info = vio_params_.camera_params_.at(0); - const bool& equalize_image = - vio_params_.frontend_params_.stereo_matching_params_.equalize_image_; - - const VIO::Timestamp& timestamp_frame_k = timestampAtFrame(current_k_); - VLOG(10) << "Sending left frames k= " << current_k_ << " with timestamp: " << timestamp_frame_k; - - // TODO(Toni): ideally only send cv::Mat raw images...: - // - pass params to vio_pipeline ctor - // - make vio_pipeline actually equalize or transform images as necessary. - - // std::string left_img_filename; - // bool available_left_img = getLeftImgName(current_k_, &left_img_filename); - // if (available_left_img) { - // // Both stereo images are available, send data to VIO - // CHECK(left_frame_callback_); - // left_frame_callback_(std::make_unique( - // current_k_, timestamp_frame_k, - // // TODO(Toni): this info should be passed to - // // the camera... not all the time here... - // left_cam_info, - // VIO::UtilsOpenCV::ReadAndConvertToGrayScale(left_img_filename, equalize_image))); - // } else { - // LOG(ERROR) << "Missing left image, proceeding to the next one."; - // } - // This is done directly when parsing the Imu data. - // imu_single_callback_(imu_meas); - - VLOG(10) << "Finished VIO processing for frame k = " << current_k_; + //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_); + // CHECK(maybe_frame.has_value()); + + + 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; + 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 + )); + + 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(robot_imu.time_of_validity.time_since_epoch() - .count(), // timestamps are int64_t nanoseconds + VIO::ImuMeasurement vio_imu(vio_time_from_robot_time(robot_imu.time_of_validity), imu_acc_gyro); return vio_imu; @@ -177,34 +199,6 @@ void SpectacularDataProviderInterface::sendImuData() const { } } -// /* -------------------------------------------------------------------------- */ -// bool SpectacularDataProviderInterface::parseDataset() { -// // Parse IMU data. -// CHECK(parseImuData(dataset_path_, kImuName)); - -// // Parse Camera data. -// VIO::CameraImageLists left_cam_image_list; -// // parseCameraData(kLeftCamName, &left_cam_image_list); -// // if (VLOG_IS_ON(1)) left_cam_image_list.print(); -// // TODO(Toni): remove camera_names_ and camera_image_lists_... -// camera_names_.push_back(kLeftCamName); -// // WARNING Use [x] not .at() because we are adding entries that do not exist. -// camera_image_lists_[kLeftCamName] = left_cam_image_list; -// // CHECK(sanityCheckCameraData(camera_names_, &camera_image_lists_)); - -// // Parse Ground-Truth data. - -// clipFinalFrame(); - -// return true; -// } - -// /* -------------------------------------------------------------------------- */ -// bool SpectacularDataProviderInterface::parseCameraData(const std::string& cam_name, -// VIO::CameraImageLists* cam_list_i) { -// CHECK_NOTNULL(cam_list_i)->parseCamImgList(dataset_path_ + "/mav0/" + cam_name, "data.csv"); -// return true; -// } std::string SpectacularDataProviderInterface::getDatasetName() { if (dataset_name_.empty()) { @@ -230,10 +224,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); - const std::string& right_cam_name = camera_names_.at(0); size_t n_left_images = getNumImagesForCamera(left_cam_name); - size_t n_right_images = getNumImagesForCamera(right_cam_name); - CHECK_EQ(n_left_images, n_right_images); return n_left_images; } diff --git a/experimental/overhead_matching/kimera_spectacular_data_provider.hh b/experimental/overhead_matching/kimera_spectacular_data_provider.hh index 4d96e83d..10e5e500 100644 --- a/experimental/overhead_matching/kimera_spectacular_data_provider.hh +++ b/experimental/overhead_matching/kimera_spectacular_data_provider.hh @@ -11,7 +11,6 @@ #include #include "experimental/overhead_matching/spectacular_log.hh" -#include "kimera-vio/common/VioNavState.h" #include "kimera-vio/dataprovider/DataProviderInterface-definitions.h" #include "kimera-vio/dataprovider/DataProviderInterface.h" #include "kimera-vio/frontend/Frame.h" diff --git a/experimental/overhead_matching/kimera_spectacular_data_provider_test.cc b/experimental/overhead_matching/kimera_spectacular_data_provider_test.cc index 0c35c195..8deb8054 100644 --- a/experimental/overhead_matching/kimera_spectacular_data_provider_test.cc +++ b/experimental/overhead_matching/kimera_spectacular_data_provider_test.cc @@ -9,20 +9,12 @@ #include "gtest/gtest.h" #include "opencv2/opencv.hpp" +#include "common/video.hh" + #include "kimera-vio/pipeline/Pipeline-definitions.h" 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) @@ -62,6 +54,52 @@ bool compare_imu_samples(const robot::experimental::overhead_matching::ImuSample 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::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::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( @@ -96,20 +134,37 @@ TEST(KimeraSpectacularDataProviderTest, happy_case) { ); 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 - s_interface.spin(); + while (s_interface.spin()) { + + } // Verification - std::cout << "Size of imu queue: " << imu_queue.size() << std::endl; 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)); @@ -117,7 +172,16 @@ TEST(KimeraSpectacularDataProviderTest, happy_case) { EXPECT_TRUE(compare_imu_samples(original_imu_samples[ i ], imu_queue[ i ])); break; } - std::cout << "all of the imu information checked out!" << std::endl; + + // 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)); + } } diff --git a/experimental/overhead_matching/spectacular_log.cc b/experimental/overhead_matching/spectacular_log.cc index 206c8678..956c6321 100644 --- a/experimental/overhead_matching/spectacular_log.cc +++ b/experimental/overhead_matching/spectacular_log.cc @@ -172,7 +172,11 @@ 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); + + // TODO: change to 32FC1 depth in meters cv::Mat depth_frame = cv::imread(depth_frame_path.string(), cv::IMREAD_GRAYSCALE); + // temp solution: + depth_frame.convertTo(depth_frame, CV_32FC1); return {{ .time_of_validity = From a6ac7fc4069fe4e16f5113cd114d0ce1354b5729 Mon Sep 17 00:00:00 2001 From: Ethan Fahnestock Date: Mon, 9 Dec 2024 16:58:15 -0500 Subject: [PATCH 7/8] updating log test to use video common --- .../overhead_matching/spectacular_log_test.cc | 15 +++------------ 1 file changed, 3 insertions(+), 12 deletions(-) diff --git a/experimental/overhead_matching/spectacular_log_test.cc b/experimental/overhead_matching/spectacular_log_test.cc index a1f91317..739dc7a5 100644 --- a/experimental/overhead_matching/spectacular_log_test.cc +++ b/experimental/overhead_matching/spectacular_log_test.cc @@ -7,19 +7,10 @@ #include "fmt/format.h" #include "gtest/gtest.h" #include "opencv2/opencv.hpp" +#include "common/video.hh" 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) @@ -63,8 +54,8 @@ TEST(SpectacularLogTest, happy_case) { fmt::format("frames2/{:08d}.png", frame_id)); const cv::Mat depth_frame = cv::imread(depth_path, cv::IMREAD_GRAYSCALE); - EXPECT_TRUE(images_equal(expected_frame, frame.bgr_frame)); - EXPECT_TRUE(images_equal(depth_frame, frame.depth_frame)); + EXPECT_TRUE(common::images_equal(expected_frame, frame.bgr_frame)); + EXPECT_TRUE(common::images_equal(depth_frame, frame.depth_frame)); for (int i = 0; i < FRAME_SKIP - 1; i++) { video.grab(); From 87a149378286f7526f2784b8858e7eec18a96bf3 Mon Sep 17 00:00:00 2001 From: Ethan Fahnestock Date: Mon, 9 Dec 2024 17:12:39 -0500 Subject: [PATCH 8/8] lint --- .../kimera_spectacular_data_provider.cc | 49 +++---- .../kimera_spectacular_data_provider.hh | 1 - .../kimera_spectacular_data_provider_test.cc | 109 +++++++-------- .../kimera_vio_pipeline_test.cc | 126 +++++++++--------- .../overhead_matching/spectacular_log_test.cc | 2 +- 5 files changed, 134 insertions(+), 153 deletions(-) diff --git a/experimental/overhead_matching/kimera_spectacular_data_provider.cc b/experimental/overhead_matching/kimera_spectacular_data_provider.cc index e00dde43..e34f4fa4 100644 --- a/experimental/overhead_matching/kimera_spectacular_data_provider.cc +++ b/experimental/overhead_matching/kimera_spectacular_data_provider.cc @@ -58,7 +58,8 @@ SpectacularDataProviderInterface::SpectacularDataProviderInterface(const std::st // 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"; + 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(); } @@ -119,57 +120,50 @@ bool SpectacularDataProviderInterface::spinOnce() { 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); + // 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_; + // vio_params_.frontend_params_.stereo_matching_params_.equalize_image_; - std::optional maybe_frame = spec_log_.get_frame(current_k_); + std::optional maybe_frame = + spec_log_.get_frame(current_k_); // CHECK(maybe_frame.has_value()); - - 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; + 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); + // 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; - CHECK(depth_meters.type() == CV_32FC1); // type assumed if depth is distance in meters + 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 - )); - - depth_frame_callback_(std::make_unique( - current_k_, - timestamp_frame_k, - depth_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); + VIO::ImuMeasurement vio_imu(vio_time_from_robot_time(robot_imu.time_of_validity), imu_acc_gyro); return vio_imu; } @@ -199,7 +193,6 @@ void SpectacularDataProviderInterface::sendImuData() const { } } - std::string SpectacularDataProviderInterface::getDatasetName() { if (dataset_name_.empty()) { // Find and store actual name (rather than path) of the dataset. diff --git a/experimental/overhead_matching/kimera_spectacular_data_provider.hh b/experimental/overhead_matching/kimera_spectacular_data_provider.hh index 10e5e500..7ea2323d 100644 --- a/experimental/overhead_matching/kimera_spectacular_data_provider.hh +++ b/experimental/overhead_matching/kimera_spectacular_data_provider.hh @@ -71,7 +71,6 @@ class SpectacularDataProviderInterface : public VIO::DataProviderInterface { */ void sendImuData() const; - //! Getters. /** * @brief getLeftImgName returns the img filename given the frame number diff --git a/experimental/overhead_matching/kimera_spectacular_data_provider_test.cc b/experimental/overhead_matching/kimera_spectacular_data_provider_test.cc index 8deb8054..5c865fac 100644 --- a/experimental/overhead_matching/kimera_spectacular_data_provider_test.cc +++ b/experimental/overhead_matching/kimera_spectacular_data_provider_test.cc @@ -1,21 +1,19 @@ -#include "experimental/overhead_matching/spectacular_log.hh" #include "experimental/overhead_matching/kimera_spectacular_data_provider.hh" #include #include +#include "common/video.hh" +#include "experimental/overhead_matching/spectacular_log.hh" #include "fmt/format.h" #include "gtest/gtest.h" -#include "opencv2/opencv.hpp" - -#include "common/video.hh" - #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::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(); @@ -25,40 +23,40 @@ std::ostream &operator<<(std::ostream &out, const time::RobotTimestamp &t) { 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()) { - std::cout << "kimera timestamp " << kimera_imu.timestamp_ << std::endl; - std::cout << "robot timestamp " << robot_imu.time_of_validity.time_since_epoch().count() << std::endl; - std::cout << "diff " << robot_imu.time_of_validity.time_since_epoch().count() - kimera_imu.timestamp_ << std::endl; - std::cout << "timestamp" << std::endl; - return false; - } - // accel and gyro values - if (kimera_imu.acc_gyr_.rows() != 6 || kimera_imu.acc_gyr_.cols() != 1) { - std::cout << "shapes" << std::endl; - 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) { - std::cout << "imu " << i << std::endl; - 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) { + // timestamp + if (kimera_imu.timestamp_ != robot_imu.time_of_validity.time_since_epoch().count()) { + std::cout << "kimera timestamp " << kimera_imu.timestamp_ << std::endl; + std::cout << "robot timestamp " << robot_imu.time_of_validity.time_since_epoch().count() + << std::endl; + std::cout << "diff " + << robot_imu.time_of_validity.time_since_epoch().count() - kimera_imu.timestamp_ + << std::endl; + std::cout << "timestamp" << std::endl; + return false; + } + // accel and gyro values + if (kimera_imu.acc_gyr_.rows() != 6 || kimera_imu.acc_gyr_.cols() != 1) { + std::cout << "shapes" << std::endl; + 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) { + std::cout << "imu " << i << std::endl; + 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) { @@ -79,18 +77,17 @@ bool compare_bgr_frame(const std::unique_ptr& kimera_frame, } return true; } -bool compare_depth_frame(const std::unique_ptr& kimera_depth, - const cv::Mat& spec_depth, - const time::RobotTimestamp& spec_time, +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::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; + std::cout << "ID mismatch! " << kimera_depth->id_ << " " + << spec_time.time_since_epoch().count() << std::endl; return false; } // check index number @@ -120,18 +117,15 @@ TEST(KimeraSpectacularDataProviderTest, happy_case) { original_imu_samples.push_back(maybe_sample.value()); } - - - const std::filesystem::path vio_config_path(""); // loads default params + const std::filesystem::path vio_config_path(""); // loads default params VIO::VioParams vio_params(vio_config_path); vio_params.parallel_run_ = false; - std::cout << "VIO params:" << std::endl; + std::cout << "VIO params:" << std::endl; vio_params.print(); - SpectacularDataProviderInterface s_interface( - log_path, 0, std::numeric_limits::max(), vio_params - ); + SpectacularDataProviderInterface s_interface(log_path, 0, std::numeric_limits::max(), + vio_params); std::vector imu_queue; std::vector> bgr_queue; @@ -147,7 +141,8 @@ TEST(KimeraSpectacularDataProviderTest, happy_case) { return; }; - auto depth_camera_callback = [&depth_queue](std::unique_ptr&& depth_frame) -> void { + auto depth_camera_callback = + [&depth_queue](std::unique_ptr&& depth_frame) -> void { depth_queue.push_back(std::move(depth_frame)); return; }; @@ -161,15 +156,14 @@ TEST(KimeraSpectacularDataProviderTest, happy_case) { // 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)); + // 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 ])); + EXPECT_TRUE(compare_imu_samples(original_imu_samples[i], imu_queue[i])); break; } @@ -179,10 +173,9 @@ TEST(KimeraSpectacularDataProviderTest, happy_case) { 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)); + 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 index 397bbaf7..02f72fe9 100644 --- a/experimental/overhead_matching/kimera_vio_pipeline_test.cc +++ b/experimental/overhead_matching/kimera_vio_pipeline_test.cc @@ -7,77 +7,73 @@ #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/RgbdImuPipeline.h" #include "kimera-vio/pipeline/Pipeline.h" -#include "kimera-vio/dataprovider/DataProviderInterface.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", +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; + // 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_test.cc b/experimental/overhead_matching/spectacular_log_test.cc index 739dc7a5..e6ded070 100644 --- a/experimental/overhead_matching/spectacular_log_test.cc +++ b/experimental/overhead_matching/spectacular_log_test.cc @@ -4,10 +4,10 @@ #include #include +#include "common/video.hh" #include "fmt/format.h" #include "gtest/gtest.h" #include "opencv2/opencv.hpp" -#include "common/video.hh" namespace robot::experimental::overhead_matching {