diff --git a/experimental/overhead_matching/kimera_spectacular_data_provider.cc b/experimental/overhead_matching/kimera_spectacular_data_provider.cc index a2c6b406..3dcabd3a 100644 --- a/experimental/overhead_matching/kimera_spectacular_data_provider.cc +++ b/experimental/overhead_matching/kimera_spectacular_data_provider.cc @@ -61,7 +61,7 @@ bool SpectacularDataProviderInterface::spin() { // 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(); + send_imu_data(); } else { LOG(ERROR) << "Imu callback not registered! Not sending IMU data."; } @@ -73,7 +73,7 @@ bool SpectacularDataProviderInterface::spin() { // 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()) { + while (!shutdown_ && spin_once()) { if (!vio_params_.parallel_run_) { // Return, instead of blocking, when running in sequential mode. return true; @@ -86,7 +86,7 @@ bool SpectacularDataProviderInterface::spin() { bool SpectacularDataProviderInterface::hasData() const { return current_k_ < final_k_; } /* -------------------------------------------------------------------------- */ -bool SpectacularDataProviderInterface::spinOnce() { +bool SpectacularDataProviderInterface::spin_once() { ROBOT_CHECK(current_k_ < std::numeric_limits::max(), "Are you sure you've initialized current_k_?"); if (current_k_ >= final_k_) { @@ -138,7 +138,7 @@ VIO::ImuMeasurement vio_imu_from_robot_imu(const ImuSample& robot_imu) { return vio_imu; } -void SpectacularDataProviderInterface::sendImuData() const { +void SpectacularDataProviderInterface::send_imu_data() const { ROBOT_CHECK(imu_single_callback_, "Did you forget to register the IMU callback?"); // for each imu measurement.. @@ -163,32 +163,12 @@ void SpectacularDataProviderInterface::sendImuData() const { } } -std::string SpectacularDataProviderInterface::getDatasetName() { - if (dataset_name_.empty()) { - // Find and store actual name (rather than path) of the dataset. - size_t found_last_slash = dataset_path_.find_last_of("/\\"); - std::string dataset_path_tmp = dataset_path_; - dataset_name_ = dataset_path_tmp.substr(found_last_slash + 1); - // The dataset name has a slash at the very end - if (found_last_slash >= dataset_path_tmp.size() - 1) { - // Cut the last slash. - dataset_path_tmp = dataset_path_tmp.substr(0, found_last_slash); - // Repeat the search. - found_last_slash = dataset_path_tmp.find_last_of("/\\"); - // Try to pick right name. - dataset_name_ = dataset_path_tmp.substr(found_last_slash + 1); - } - LOG(INFO) << "Dataset name: " << dataset_name_; - } - return dataset_name_; -} - /* -------------------------------------------------------------------------- */ -size_t SpectacularDataProviderInterface::getNumImages() const { return spec_log_.num_frames(); } +size_t SpectacularDataProviderInterface::get_num_images() const { return spec_log_.num_frames(); } -void SpectacularDataProviderInterface::clipFinalFrame() { +void SpectacularDataProviderInterface::clip_final_frame() { // Clip final_k_ to the total number of images. - const size_t& nr_images = getNumImages(); + const size_t& nr_images = get_num_images(); if (final_k_ > nr_images) { LOG(WARNING) << "Value for final_k, " << final_k_ << " is larger than total" << " number of frames in dataset " << nr_images; diff --git a/experimental/overhead_matching/kimera_spectacular_data_provider.hh b/experimental/overhead_matching/kimera_spectacular_data_provider.hh index 167c726d..67ce24e8 100644 --- a/experimental/overhead_matching/kimera_spectacular_data_provider.hh +++ b/experimental/overhead_matching/kimera_spectacular_data_provider.hh @@ -41,26 +41,25 @@ class SpectacularDataProviderInterface : public VIO::DataProviderInterface { void print() const; public: - inline std::string getDatasetPath() const { return dataset_path_; } - std::string getDatasetName(); + inline std::string get_dataset_path() const { return dataset_path_; } protected: /** - * @brief spinOnce Send data to VIO pipeline on a per-frame basis + * @brief spin_once Send data to VIO pipeline on a per-frame basis * @return if the dataset finished or not */ - bool spinOnce(); + bool spin_once(); /** * @brief sendImuData We send IMU data first (before frames) so that the VIO * pipeline can query all IMU data between frames. */ - void sendImuData() const; + void send_imu_data() const; - size_t getNumImages() const; + size_t get_num_images() const; // Clip final frame to the number of images in the dataset. - void clipFinalFrame(); + void clip_final_frame(); protected: VIO::VioParams vio_params_; diff --git a/experimental/overhead_matching/kimera_spectacular_data_provider_test.cc b/experimental/overhead_matching/kimera_spectacular_data_provider_test.cc index 79c169ae..7110f22e 100644 --- a/experimental/overhead_matching/kimera_spectacular_data_provider_test.cc +++ b/experimental/overhead_matching/kimera_spectacular_data_provider_test.cc @@ -121,9 +121,6 @@ TEST(KimeraSpectacularDataProviderTest, happy_case) { 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); diff --git a/experimental/overhead_matching/spectacular_log.cc b/experimental/overhead_matching/spectacular_log.cc index 956c6321..1069498a 100644 --- a/experimental/overhead_matching/spectacular_log.cc +++ b/experimental/overhead_matching/spectacular_log.cc @@ -171,12 +171,9 @@ std::optional SpectacularLog::get_frame(const int frame_id) const { // Read the desired depth frame const fs::path depth_frame_path = - log_path_ / "frames2" / fmt::format("{:08d}.png", frame_info.frame_number); + log_path_ / "frames2" / fmt::format("{:08d}.tiff", 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); + cv::Mat depth_frame = cv::imread(depth_frame_path.string(), cv::IMREAD_UNCHANGED); return {{ .time_of_validity =