diff --git a/WORKSPACE b/WORKSPACE index 2aed5497..39967373 100644 --- a/WORKSPACE +++ b/WORKSPACE @@ -530,7 +530,7 @@ http_archive( urls = ["https://github.com/ewfuentes/Kimera-VIO/archive/master.zip"], strip_prefix = "Kimera-VIO-master", build_file = "//third_party:BUILD.kimera_vio", - integrity = "sha256-+hQBAPv4bDRbYnH1iXeuHTNsWnfXgqZ0FoUTv6usQ/k=", + integrity = "sha256-dWzux1ioSB8LfovhQFAcbmlotNo19x++7ptw9S64UGM=", ) http_archive( diff --git a/experimental/overhead_matching/BUILD b/experimental/overhead_matching/BUILD index 7c9b8d96..2339e911 100644 --- a/experimental/overhead_matching/BUILD +++ b/experimental/overhead_matching/BUILD @@ -73,3 +73,14 @@ cc_test( "//common/video:image_compare", ] ) + +cc_test( + 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", + ] +) diff --git a/experimental/overhead_matching/kimera_vio_pipeline_test.cc b/experimental/overhead_matching/kimera_vio_pipeline_test.cc index 02f72fe9..9d507e80 100644 --- a/experimental/overhead_matching/kimera_vio_pipeline_test.cc +++ b/experimental/overhead_matching/kimera_vio_pipeline_test.cc @@ -31,6 +31,9 @@ int main(int argc, char* argv[]) { // Build dataset parser. VIO::DataProviderInterface::Ptr dataset_parser = std::make_unique( + FLAGS_dataset_path, + 0, + std::numeric_limits::max(), vio_params); CHECK(dataset_parser); @@ -42,18 +45,22 @@ 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)); + [&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)); + [&vio_pipeline](auto arg){ + auto rgbd_pipeline = std::dynamic_pointer_cast(vio_pipeline); + ROBOT_CHECK(rgbd_pipeline); + rgbd_pipeline->fillDepthFrameQueue(std::move(arg)); + }); // Spin dataset. auto tic = VIO::utils::Timer::tic();