Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Run Kimera on SpecLog #357

Merged
merged 8 commits into from
Jan 17, 2025
Merged

Run Kimera on SpecLog #357

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
534f5d7
Merge branch 'main' of github.com:ewfuentes/robot
efahnestock Dec 17, 2024
1e119c9
Merge branch 'main' of github.com:ewfuentes/robot
efahnestock Dec 18, 2024
19192d6
updating kimera_vio sha, starting to work on kimera script
efahnestock Dec 18, 2024
d6bd390
wip
efahnestock Jan 4, 2025
add10f2
Merge branch 'main' of github.com:ewfuentes/robot
efahnestock Jan 5, 2025
31ddee8
Merge branch 'main' into run_spec_through_kimera
efahnestock Jan 5, 2025
1dd4ccd
turn off DCHECKS in kimera_vio, format
efahnestock Jan 5, 2025
fe36e16
fixing test
efahnestock Jan 5, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
11 changes: 11 additions & 0 deletions experimental/overhead_matching/BUILD
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -73,3 +73,14 @@ cc_test(
"//common/video:image_compare",
]
)

cc_binary(
name = "kimera_vio_pipeline_test",
srcs = ["kimera_vio_pipeline_test.cc"],
copts = ["-Wno-sign-compare", "-Wno-vla"],
deps = [
":kimera_spectacular_data_provider",
"@com_google_googletest//:gtest_main",
"@fmt",
]
)
14 changes: 8 additions & 6 deletions experimental/overhead_matching/kimera_spectacular_data_provider.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -85,11 +85,10 @@ bool SpectacularDataProviderInterface::spin_once() {
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);
const VIO::CameraParams left_cam_FAKE_PARAMS;
const bool equalize_image = false;
// vio_params_.frontend_params_.stereo_matching_params_.equalize_image_;

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_;

std::optional<FrameGroup> maybe_frame = spec_log_.get_frame(current_k_);

Expand All @@ -111,8 +110,11 @@ bool SpectacularDataProviderInterface::spin_once() {
cv::Mat& depth_meters = maybe_frame->depth_frame;
ROBOT_CHECK(depth_meters.type() == CV_32FC1); // type assumed if depth is distance in meters

// scale depth image to match rgb image size
cv::resize(depth_meters, depth_meters, bgr.size());

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

depth_frame_callback_(
std::make_unique<VIO::DepthFrame>(current_k_, timestamp_frame_k, depth_meters));
Expand Down
7 changes: 4 additions & 3 deletions experimental/overhead_matching/kimera_spectacular_data_provider_test.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -115,6 +115,7 @@ TEST(KimeraSpectacularDataProviderTest, happy_case) {

const std::filesystem::path vio_config_path(""); // loads default params
VIO::VioParams vio_params(vio_config_path);
vio_params.camera_params_.push_back(VIO::CameraParams());
vio_params.parallel_run_ = false;

SpectacularDataProviderInterface s_interface(log_path, 0, std::numeric_limits<int>::max(),
Expand Down Expand Up @@ -153,8 +154,6 @@ TEST(KimeraSpectacularDataProviderTest, happy_case) {

// 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));
for (size_t i = 0; i < imu_queue.size(); i++) {
EXPECT_TRUE(compare_imu_samples(original_imu_samples[i], imu_queue[i]));
break;
Expand All @@ -166,8 +165,10 @@ TEST(KimeraSpectacularDataProviderTest, happy_case) {
std::optional<FrameGroup> fg = log.get_frame(i);
EXPECT_TRUE(fg.has_value());

auto rescaled_depth = fg->depth_frame;
cv::resize(rescaled_depth, rescaled_depth, fg->bgr_frame.size());
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_depth_frame(depth_queue[i], rescaled_depth, fg->time_of_validity, i));
}
}

Expand Down
29 changes: 19 additions & 10 deletions experimental/overhead_matching/kimera_vio_pipeline_test.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,17 +7,19 @@
#include <utility>

#include "experimental/overhead_matching/kimera_spectacular_data_provider.hh"
#include "fmt/core.h"
#include "kimera-vio/dataprovider/DataProviderInterface.h"
#include "kimera-vio/logging/Logger.h"
#include "kimera-vio/pipeline/Pipeline.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",
DEFINE_string(params_folder_path, "/home/ekf/software/robot/data/iphoneSpectacularParams",
"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).");
DEFINE_string(dataset_path, "/home/ekf/software/robot/data/Walk-to-work",
// DEFINE_string(dataset_path, "/home/ekf/software/robot/data/20241212_090634",
"Path of dataset");
Comment on lines -17 to +22
Copy link
Owner

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I've been using the cxxopts library to handle argument flags. Up to you if you want to switch to that. Here is an example:

robot/experimental/pokerbots/pokerbot_master.cc

Line 213 in 43f2180

cxxopts::Options options("cfr_train", "I wanna be the very best, like no one ever was.");

I see that this isn't a cc_test. Is there something that is making it difficult to do that instead? I could imagine it being useful to have a binary to run Kimera on some log.

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Unfortunately cxxopts and google flags don't play well together, we would likely have to add a bandaid or remove all gflags from used kimera components.

Re cc_test vs cc_binary -- I picked cc_binary because I thought it would be useful to have a binary to run on logs. If we change it to a cc_test would we still be able to use it to get vio solutions from different bags?

I can make it a test and make sure the final pose is somewhat reasonable. Where should I put the iphone spectacular params? (this repo, pulled in as a zip, standalone repo?)

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

in another pr break out an interface to create and run pipeline on a spec log. Create a test that uses this, and use this interface in the binary as well.


int main(int argc, char* argv[]) {
// Initialize Google's flags library.
Expand All @@ -28,10 +30,14 @@ int main(int argc, char* argv[]) {
// Parse VIO parameters from gflags.
VIO::VioParams vio_params(FLAGS_params_folder_path);

std::cout << " print camera params:" << std::endl;
vio_params.print();
// for (const auto& cam : vio_params.camera_params_) cam.print();

// Build dataset parser.
VIO::DataProviderInterface::Ptr dataset_parser =
std::make_unique<robot::experimental::overhead_matching::SpectacularDataProviderInterface>(
vio_params);
FLAGS_dataset_path, 0, std::numeric_limits<int>::max(), vio_params);

CHECK(dataset_parser);

Expand All @@ -41,23 +47,26 @@ int main(int argc, char* argv[]) {

// Register callback to shutdown data provider in case VIO pipeline
// shutsdown.
vio_pipeline->registerShutdownCallback(
std::bind(&VIO::DataProviderModule::shutdown, dataset_parser));
vio_pipeline->registerShutdownCallback([&dataset_parser]() { dataset_parser->shutdown(); });

// Register callback to vio pipeline.
dataset_parser->registerImuSingleCallback(
std::bind(&VIO::Pipeline::fillSingleImuQueue, vio_pipeline, std::placeholders::_1));
[&vio_pipeline](const auto& arg) { vio_pipeline->fillSingleImuQueue(arg); });
// 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));
[&vio_pipeline](auto arg) { vio_pipeline->fillLeftFrameQueue(std::move(arg)); });

dataset_parser->registerDepthFrameCallback(
std::bind(&VIO::RgbdImuPipeline::fillDepthFrameQueue, vio_pipeline, std::placeholders::_1));
dataset_parser->registerDepthFrameCallback([&vio_pipeline](auto arg) {
auto rgbd_pipeline = std::dynamic_pointer_cast<VIO::RgbdImuPipeline>(vio_pipeline);
ROBOT_CHECK(rgbd_pipeline);
rgbd_pipeline->fillDepthFrameQueue(std::move(arg));
});

// Spin dataset.
auto tic = VIO::utils::Timer::tic();
bool is_pipeline_successful = false;
LOG(WARNING) << "Starting to spin pipeline";
while (dataset_parser->spin() && vio_pipeline->spin()) {
continue;
};
Expand Down
3 changes: 3 additions & 0 deletions third_party/BUILD.kimera_vio
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -253,6 +253,9 @@ cc_library(
"@kimera_rpgo",
":triangle",
],
copts = [
"-DNDEBUG", # Disable DCHECK
],
strip_include_prefix = "include",
visibility = ["//visibility:public"],
)
Expand Down