Skip to content

Commit

Permalink
lint
Browse files Browse the repository at this point in the history
  • Loading branch information
@efahnestock
efahnestock committed Dec 10, 2024
1 parent 7a819b2 commit 215fae4
Show file tree
Hide file tree
Showing 5 changed files with 134 additions and 153 deletions.
49 changes: 21 additions & 28 deletions experimental/overhead_matching/kimera_spectacular_data_provider.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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();
}

Expand Down Expand Up @@ -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<robot::experimental::overhead_matching::FrameGroup> maybe_frame = spec_log_.get_frame(current_k_);
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;
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<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
));
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(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;
}
Expand Down Expand Up @@ -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.
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 @@ -71,7 +71,6 @@ class SpectacularDataProviderInterface : public VIO::DataProviderInterface {
*/
void sendImuData() const;


//! Getters.
/**
* @brief getLeftImgName returns the img filename given the frame number
Expand Down
109 changes: 51 additions & 58 deletions experimental/overhead_matching/kimera_spectacular_data_provider_test.cc
View file
Original file line number Diff line number Diff line change
@@ -1,21 +1,19 @@

#include "experimental/overhead_matching/spectacular_log.hh"
#include "experimental/overhead_matching/kimera_spectacular_data_provider.hh"

#include <iostream>
#include <sstream>

#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<double>(t.time_since_epoch()).count();
Expand All @@ -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<VIO::Frame>& 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<VIO::Frame>& 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) {
Expand All @@ -79,18 +77,17 @@ bool compare_bgr_frame(const std::unique_ptr<VIO::Frame>& kimera_frame,
}
return true;
}
bool compare_depth_frame(const std::unique_ptr<VIO::DepthFrame>& kimera_depth,
const cv::Mat& spec_depth,
const time::RobotTimestamp& spec_time,
bool compare_depth_frame(const std::unique_ptr<VIO::DepthFrame>& 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
Expand Down Expand Up @@ -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<int>::max(), vio_params
);
SpectacularDataProviderInterface s_interface(log_path, 0, std::numeric_limits<int>::max(),
vio_params);

std::vector<VIO::ImuMeasurement> imu_queue;
std::vector<std::unique_ptr<VIO::Frame>> bgr_queue;
Expand All @@ -147,7 +141,8 @@ TEST(KimeraSpectacularDataProviderTest, happy_case) {
return;
};

auto depth_camera_callback = [&depth_queue](std::unique_ptr<VIO::DepthFrame>&& depth_frame) -> void {
auto depth_camera_callback =
[&depth_queue](std::unique_ptr<VIO::DepthFrame>&& depth_frame) -> void {
depth_queue.push_back(std::move(depth_frame));
return;
};
Expand All @@ -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;
}

Expand All @@ -179,10 +173,9 @@ TEST(KimeraSpectacularDataProviderTest, happy_case) {
std::optional<FrameGroup> 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
Loading

0 comments on commit 215fae4

Please sign in to comment.