Skip to content

Commit

Permalink
finishing up frame delivery
Browse files Browse the repository at this point in the history
  • Loading branch information
@efahnestock
efahnestock committed Dec 9, 2024
1 parent cb7219b commit 1cdc33e
Show file tree
Hide file tree
Showing 5 changed files with 165 additions and 102 deletions.
41 changes: 23 additions & 18 deletions experimental/overhead_matching/BUILD
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,3 @@


package(features=["warning_compile_flags"])

load("@pybind11_bazel//:build_defs.bzl", "pybind_extension")
Expand Down Expand Up @@ -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",
Expand All @@ -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"),
],
)
)
131 changes: 61 additions & 70 deletions experimental/overhead_matching/kimera_spectacular_data_provider.cc
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
#include "experimental/overhead_matching/kimera_spectacular_data_provider.hh"

#include <gflags/gflags.h>
#include <glog/logging.h>
// #include <glog/logging.h>
#include <gtsam/base/Vector.h>
#include <gtsam/geometry/Cal3DS2.h>
#include <gtsam/geometry/Pose3.h>
Expand Down Expand Up @@ -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,
Expand All @@ -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<VIO::FrameId>::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).
Expand All @@ -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_)
Expand Down Expand Up @@ -94,7 +105,7 @@ bool SpectacularDataProviderInterface::spin() {
return true;
}
}
LOG_IF(INFO, shutdown_) << "EurocDataProvider shutdown requested.";
LOG_IF(INFO, shutdown_) << "shutdown requested.";
return false;
}

Expand All @@ -103,50 +114,61 @@ 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_?";
// << "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<VIO::AnmsAlgorithmType>(
// 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<robot::experimental::overhead_matching::FrameGroup> 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<VIO::Frame>(
current_k_,
timestamp_frame_k,
left_cam_FAKE_PARAMS,
bgr
));

depth_frame_callback_(std::make_unique<VIO::DepthFrame>(
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<double, 6, 1> 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;
Expand Down Expand Up @@ -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()) {
Expand All @@ -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;
}

Expand Down
1 change: 0 additions & 1 deletion experimental/overhead_matching/kimera_spectacular_data_provider.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,6 @@
#include <vector>

#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"
Expand Down
Loading

0 comments on commit 1cdc33e

Please sign in to comment.