Skip to content

Commit

Permalink
addresing pr comments
Browse files Browse the repository at this point in the history
  • Loading branch information
@efahnestock
efahnestock committed Dec 10, 2024
1 parent dbb2926 commit 26e4de2
Show file tree
Hide file tree
Showing 4 changed files with 23 additions and 174 deletions.
15 changes: 0 additions & 15 deletions common/video.cc
View file

This file was deleted.

9 changes: 0 additions & 9 deletions common/video.hh
View file

This file was deleted.

116 changes: 20 additions & 96 deletions experimental/overhead_matching/kimera_spectacular_data_provider.cc
View file
Original file line number Diff line number Diff line change
@@ -1,31 +1,14 @@
#include "experimental/overhead_matching/kimera_spectacular_data_provider.hh"

#include <gflags/gflags.h>
// #include <glog/logging.h>
#include <gtsam/base/Vector.h>
#include <gtsam/geometry/Cal3DS2.h>
#include <gtsam/geometry/Pose3.h>

#include <algorithm> // for max
#include <fstream>
#include <map>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/core/core.hpp>
#include <string>
#include <utility> // for pair<>
#include <vector>

#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 {

Expand All @@ -45,13 +28,12 @@ SpectacularDataProviderInterface::SpectacularDataProviderInterface(const std::st
current_k_(std::numeric_limits<VIO::FrameId>::max()),
initial_k_(initial_k),
final_k_(final_k),
imu_measurements_(),
spec_log_(dataset_path) {
// CHECK(!dataset_path_.empty()) << "Dataset path for SpectacularDataProvider is empty.";
ROBOT_CHECK(!dataset_path_.empty(), "Dataset path for SpectacularDataProvider is empty.");

// Start processing dataset from frame initial_k.
// Useful to skip a bunch of images at the beginning (imu calibration).
CHECK_GE(initial_k_, 0);
ROBOT_CHECK(initial_k_ >= 0);

// Finish processing dataset at frame final_k.
// Last frame to process (to avoid processing the entire dataset),
Expand All @@ -63,25 +45,17 @@ SpectacularDataProviderInterface::SpectacularDataProviderInterface(const std::st
final_k_ = spec_log_.num_frames();
}

CHECK_GT(final_k_, 0);
ROBOT_CHECK(final_k_ > 0);

CHECK_GT(final_k_, initial_k_)
<< "Value for final_k (" << final_k_ << ") is smaller than value for"
<< " initial_k (" << initial_k_ << ").";
ROBOT_CHECK(final_k_ > initial_k_, "Value for final_k is smaller than value for initial_k",
final_k_, initial_k_);
current_k_ = initial_k_;
}

/* -------------------------------------------------------------------------- */
SpectacularDataProviderInterface::SpectacularDataProviderInterface(const VIO::VioParams& vio_params)
: SpectacularDataProviderInterface(FLAGS_dataset_path, FLAGS_initial_k, FLAGS_final_k,
vio_params) {}

/* -------------------------------------------------------------------------- */
SpectacularDataProviderInterface::~SpectacularDataProviderInterface() {
LOG(INFO) << "SpectacularDataProviderInterface destructor called.";
}

/* -------------------------------------------------------------------------- */
bool SpectacularDataProviderInterface::spin() {
if (!is_imu_data_sent_) {
// First, send all the IMU data. The flag is to avoid sending it several
Expand All @@ -95,7 +69,7 @@ bool SpectacularDataProviderInterface::spin() {
}

// Spin.
CHECK_GT(final_k_, initial_k_);
ROBOT_CHECK(final_k_ > initial_k_);
// We log only the first one, because we may be running in sequential mode.
LOG_FIRST_N(INFO, 1) << "Running dataset between frame " << initial_k_ << " and frame "
<< final_k_;
Expand All @@ -113,8 +87,8 @@ bool SpectacularDataProviderInterface::hasData() const { return current_k_ < fin

/* -------------------------------------------------------------------------- */
bool SpectacularDataProviderInterface::spinOnce() {
CHECK_LT(current_k_, std::numeric_limits<VIO::FrameId>::max())
<< "Are you sure you've initialized current_k_?";
ROBOT_CHECK(current_k_ < std::numeric_limits<VIO::FrameId>::max(),
"Are you sure you've initialized current_k_?");
if (current_k_ >= final_k_) {
LOG(INFO) << "Finished spinning dataset.";
return false;
Expand All @@ -125,9 +99,7 @@ bool SpectacularDataProviderInterface::spinOnce() {
const bool& equalize_image = false;
// vio_params_.frontend_params_.stereo_matching_params_.equalize_image_;

std::optional<robot::experimental::overhead_matching::FrameGroup> maybe_frame =
spec_log_.get_frame(current_k_);
// CHECK(maybe_frame.has_value());
std::optional<FrameGroup> maybe_frame = spec_log_.get_frame(current_k_);

const VIO::Timestamp& timestamp_frame_k =
vio_time_from_robot_time(maybe_frame->time_of_validity);
Expand All @@ -145,9 +117,8 @@ bool SpectacularDataProviderInterface::spinOnce() {
}

cv::Mat depth_meters = maybe_frame->depth_frame;
CHECK(depth_meters.type() == CV_32FC1); // type assumed if depth is distance in meters
ROBOT_CHECK(depth_meters.type() == CV_32FC1); // type assumed if depth is distance in meters

// CHECK(left_frame_callback_);
left_frame_callback_(
std::make_unique<VIO::Frame>(current_k_, timestamp_frame_k, left_cam_FAKE_PARAMS, bgr));

Expand All @@ -168,25 +139,25 @@ VIO::ImuMeasurement vio_imu_from_robot_imu(const ImuSample& robot_imu) {
}

void SpectacularDataProviderInterface::sendImuData() const {
CHECK(imu_single_callback_) << "Did you forget to register the IMU callback?";
ROBOT_CHECK(imu_single_callback_, "Did you forget to register the IMU callback?");
// for each imu measurement..

const time::RobotTimestamp min_imu_time = spec_log_.min_imu_time();
const time::RobotTimestamp max_imu_time = spec_log_.max_imu_time();
// pull times from the accel spline
const math::CubicHermiteSpline<Eigen::Vector3d> accel_spline = spec_log_.accel_spline();
const math::CubicHermiteSpline<Eigen::Vector3d>& accel_spline = spec_log_.accel_spline();

for (const double& t : accel_spline.ts()) {
// exit if before first IMU time
time::RobotTimestamp t_robot = time::RobotTimestamp() + time::as_duration(t);
if (t < std::chrono::duration<double>(min_imu_time.time_since_epoch()).count()) {
const time::RobotTimestamp t_robot = time::RobotTimestamp() + time::as_duration(t);
if (t_robot < min_imu_time) {
continue;
}
if (t > std::chrono::duration<double>(max_imu_time.time_since_epoch()).count()) {
if (t_robot > max_imu_time) {
break;
}
std::optional<ImuSample> imu_sample = spec_log_.get_imu_sample(t_robot);
CHECK(imu_sample.has_value());
ROBOT_CHECK(imu_sample.has_value());
auto vio_imu = vio_imu_from_robot_imu(imu_sample.value());
imu_single_callback_(vio_imu);
}
Expand All @@ -213,45 +184,7 @@ std::string SpectacularDataProviderInterface::getDatasetName() {
}

/* -------------------------------------------------------------------------- */
size_t SpectacularDataProviderInterface::getNumImages() const {
CHECK_GT(camera_names_.size(), 0u);
const std::string& left_cam_name = camera_names_.at(0);
size_t n_left_images = getNumImagesForCamera(left_cam_name);
return n_left_images;
}

/* -------------------------------------------------------------------------- */
size_t SpectacularDataProviderInterface::getNumImagesForCamera(
const std::string& camera_name) const {
const auto& iter = camera_image_lists_.find(camera_name);
CHECK(iter != camera_image_lists_.end());
return iter->second.getNumImages();
}

/* -------------------------------------------------------------------------- */
bool SpectacularDataProviderInterface::getImgName(const std::string& camera_name, const size_t& k,
std::string* img_filename) const {
CHECK_NOTNULL(img_filename);
const auto& iter = camera_image_lists_.find(camera_name);
CHECK(iter != camera_image_lists_.end());
const auto& img_lists = iter->second.img_lists_;
if (k < img_lists.size()) {
*img_filename = img_lists.at(k).second;
return true;
} else {
LOG(ERROR) << "Requested image #: " << k << " but we only have " << img_lists.size()
<< " images.";
}
return false;
}

/* -------------------------------------------------------------------------- */
VIO::Timestamp SpectacularDataProviderInterface::timestampAtFrame(
const VIO::FrameId& frame_number) {
CHECK_GT(camera_names_.size(), 0);
CHECK_LT(frame_number, camera_image_lists_.at(camera_names_[0]).img_lists_.size());
return camera_image_lists_.at(camera_names_[0]).img_lists_[frame_number].first;
}
size_t SpectacularDataProviderInterface::getNumImages() const { return spec_log_.num_frames(); }

void SpectacularDataProviderInterface::clipFinalFrame() {
// Clip final_k_ to the total number of images.
Expand All @@ -262,21 +195,12 @@ void SpectacularDataProviderInterface::clipFinalFrame() {
final_k_ = nr_images;
LOG(WARNING) << "Using final_k = " << final_k_;
}
CHECK_LE(final_k_, nr_images);
ROBOT_CHECK(final_k_ <= nr_images);
}
/* -------------------------------------------------------------------------- */
void SpectacularDataProviderInterface::print() const {
LOG(INFO) << "------------------ SpectacularDataProviderInterface::print ------------------\n"
<< "Displaying info for dataset: " << dataset_path_;
// For each of the 2 cameras.
CHECK_EQ(vio_params_.camera_params_.size(), camera_names_.size());
for (size_t i = 0; i < camera_names_.size(); i++) {
LOG(INFO) << "\n"
<< (i == 0 ? "Left" : "Right") << " camera name: " << camera_names_[i]
<< ", with params:\n";
vio_params_.camera_params_.at(i).print();
camera_image_lists_.at(camera_names_[i]).print();
}
LOG(INFO) << "-------------------------------------------------------------";
}
} // namespace robot::experimental::overhead_matching
57 changes: 3 additions & 54 deletions experimental/overhead_matching/kimera_spectacular_data_provider.hh
View file
Original file line number Diff line number Diff line change
@@ -1,22 +1,10 @@
#pragma once

#include <gtsam/base/Vector.h>
#include <gtsam/geometry/Cal3DS2.h>
#include <gtsam/geometry/Pose3.h>

#include <map>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/core/core.hpp>
#include <string>
#include <vector>

#include "experimental/overhead_matching/spectacular_log.hh"
#include "kimera-vio/dataprovider/DataProviderInterface-definitions.h"
#include "kimera-vio/dataprovider/DataProviderInterface.h"
#include "kimera-vio/frontend/Frame.h"
#include "kimera-vio/frontend/StereoImuSyncPacket.h"
#include "kimera-vio/frontend/StereoMatchingParams.h"
#include "kimera-vio/logging/Logger.h"
#include "kimera-vio/utils/Macros.h"

namespace robot::experimental::overhead_matching {
Expand All @@ -33,8 +21,6 @@ class SpectacularDataProviderInterface : public VIO::DataProviderInterface {
//! Ctor with params.
SpectacularDataProviderInterface(const std::string& dataset_path, const int& initial_k,
const int& final_k, const VIO::VioParams& vio_params);
//! Ctor from gflags
explicit SpectacularDataProviderInterface(const VIO::VioParams& vio_params);

virtual ~SpectacularDataProviderInterface();

Expand All @@ -45,9 +31,9 @@ class SpectacularDataProviderInterface : public VIO::DataProviderInterface {
* return until it finishes.
* @return True if the dataset still has data, false otherwise.
*/
virtual bool spin() override;
bool spin() override;

virtual bool hasData() const override;
bool hasData() const override;

/**
* @brief print Print info about dataset.
Expand All @@ -63,53 +49,22 @@ class SpectacularDataProviderInterface : public VIO::DataProviderInterface {
* @brief spinOnce Send data to VIO pipeline on a per-frame basis
* @return if the dataset finished or not
*/
virtual bool spinOnce();
bool spinOnce();

/**
* @brief sendImuData We send IMU data first (before frames) so that the VIO
* pipeline can query all IMU data between frames.
*/
void sendImuData() const;

//! Getters.
/**
* @brief getLeftImgName returns the img filename given the frame number
* @param[in] k frame number
* @param[out] img_name returned filename of the img
* @return if k is larger than the number of frames, returns false, otw true.
*/
inline bool getLeftImgName(const size_t& k, std::string* img_name) const {
return getImgName("cam0", k, img_name);
}
size_t getNumImages() const;
size_t getNumImagesForCamera(const std::string& camera_name) const;
/**
* @brief getImgName returns the img filename given the frame number
* @param[in] camera_name camera id such as "cam0"/"cam1"
* @param[in] k frame number
* @param[out] img_filename returned filename of the img
* @return if k is larger than the number of frames, returns false, otw true.
*/
bool getImgName(const std::string& camera_name, const size_t& k,
std::string* img_filename) const;

// Get timestamp of a given pair of stereo images (synchronized).
VIO::Timestamp timestampAtFrame(const VIO::FrameId& frame_number);

// Clip final frame to the number of images in the dataset.
void clipFinalFrame();

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<std::string> camera_names_;
// Map from camera name to its images
std::map<std::string, VIO::CameraImageLists> camera_image_lists_;

bool is_gt_available_;
std::string dataset_name_;
std::string dataset_path_;

Expand All @@ -120,12 +75,6 @@ class SpectacularDataProviderInterface : public VIO::DataProviderInterface {
//! Flag to signal if the IMU data has been sent to the VIO pipeline
bool is_imu_data_sent_ = false;

const std::string kLeftCamName = "cam0";
const std::string kImuName = "imu0";

//! Pre-stored imu-measurements
std::vector<VIO::ImuMeasurement> imu_measurements_;

SpectacularLog spec_log_;
};

Expand Down

0 comments on commit 26e4de2

Please sign in to comment.