From b0c523e12b6779f0b524e673e72951690970632c Mon Sep 17 00:00:00 2001 From: Vladyslav Usenko Date: Fri, 10 May 2019 16:18:26 +0200 Subject: [PATCH] Fixes for datasets with many skipped frames --- include/basalt/calibration/vignette.h | 2 ++ src/calibration/cam_calib.cpp | 44 +++++++++++++++++++++------ src/calibration/cam_imu_calib.cpp | 30 ++++++++++-------- src/calibration/vignette.cpp | 7 +++-- 4 files changed, 58 insertions(+), 25 deletions(-) diff --git a/include/basalt/calibration/vignette.h b/include/basalt/calibration/vignette.h index 70bf97f..f5061db 100644 --- a/include/basalt/calibration/vignette.h +++ b/include/basalt/calibration/vignette.h @@ -51,6 +51,7 @@ class VignetteEstimator { VignetteEstimator(const VioDatasetPtr &vio_dataset, const Eigen::vector &optical_centers, + const Eigen::vector &resolutions, const std::map> &reprojected_vignette, const AprilGrid &april_grid); @@ -77,6 +78,7 @@ class VignetteEstimator { private: const VioDatasetPtr vio_dataset; Eigen::vector optical_centers; + Eigen::vector resolutions; std::map> reprojected_vignette; const AprilGrid &april_grid; diff --git a/src/calibration/cam_calib.cpp b/src/calibration/cam_calib.cpp index 696152f..20c20c9 100644 --- a/src/calibration/cam_calib.cpp +++ b/src/calibration/cam_calib.cpp @@ -172,7 +172,8 @@ void CamCalib::computeVign() { } } - VignetteEstimator ve(vio_dataset, optical_centers, reprojected_vignette2, + VignetteEstimator ve(vio_dataset, optical_centers, + calib_opt->calib->resolution, reprojected_vignette2, april_grid); ve.optimize(); @@ -353,8 +354,25 @@ void CamCalib::initCamIntrinsics() { // set resolution { - int64_t t_ns = vio_dataset->get_image_timestamps()[1]; - const auto img_data = vio_dataset->get_image_data(t_ns); + size_t img_idx = 1; + int64_t t_ns = vio_dataset->get_image_timestamps()[img_idx]; + auto img_data = vio_dataset->get_image_data(t_ns); + + // Find the frame with all valid images + while (img_idx < vio_dataset->get_image_timestamps().size()) { + bool img_data_valid = true; + for (size_t i = 0; i < vio_dataset->get_num_cams(); i++) { + if (!img_data[i].img.get()) img_data_valid = false; + } + + if (!img_data_valid) { + img_idx++; + int64_t t_ns_new = vio_dataset->get_image_timestamps()[img_idx]; + img_data = vio_dataset->get_image_data(t_ns_new); + } else { + break; + } + } Eigen::vector res; @@ -481,11 +499,17 @@ void CamCalib::initOptimization() { for (size_t j = 0; j < vio_dataset->get_image_timestamps().size(); ++j) { int64_t timestamp_ns = vio_dataset->get_image_timestamps()[j]; - TimeCamId tcid = std::make_pair(timestamp_ns, 0); - const CalibInitPoseData &cp = calib_init_poses.at(tcid); + for (size_t cam_id = 0; cam_id < calib_opt->calib->T_i_c.size(); cam_id++) { + TimeCamId tcid = std::make_pair(timestamp_ns, cam_id); + const auto cp_it = calib_init_poses.find(tcid); - calib_opt->addPoseMeasurement( - timestamp_ns, cp.T_a_c * calib_opt->calib->T_i_c[0].inverse()); + if (cp_it != calib_init_poses.end()) { + calib_opt->addPoseMeasurement( + timestamp_ns, + cp_it->second.T_a_c * calib_opt->calib->T_i_c[cam_id].inverse()); + break; + } + } } calib_opt->init(); @@ -742,7 +766,8 @@ void CamCalib::drawImageOverlay(pangolin::View &v, size_t cam_id) { glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); if (reprojected_corners.find(tcid) != reprojected_corners.end()) { - if (calib_corners.at(tcid).corner_ids.size() >= MIN_CORNERS) { + if (calib_corners.count(tcid) > 0 && + calib_corners.at(tcid).corner_ids.size() >= MIN_CORNERS) { const auto &rc = reprojected_corners.at(tcid); for (size_t i = 0; i < rc.size(); i++) { @@ -764,7 +789,8 @@ void CamCalib::drawImageOverlay(pangolin::View &v, size_t cam_id) { glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); if (reprojected_vignette.find(tcid) != reprojected_vignette.end()) { - if (calib_corners.at(tcid).corner_ids.size() >= MIN_CORNERS) { + if (calib_corners.count(tcid) > 0 && + calib_corners.at(tcid).corner_ids.size() >= MIN_CORNERS) { const auto &rc = reprojected_vignette.at(tcid); bool has_errors = false; diff --git a/src/calibration/cam_imu_calib.cpp b/src/calibration/cam_imu_calib.cpp index 8a36759..262d82b 100644 --- a/src/calibration/cam_imu_calib.cpp +++ b/src/calibration/cam_imu_calib.cpp @@ -435,20 +435,23 @@ void CamImuCalib::initOptimization() { int64_t timestamp_ns = vio_dataset->get_image_timestamps()[j]; TimeCamId tcid = std::make_pair(timestamp_ns, 0); - const CalibInitPoseData &cp = calib_init_poses.at(tcid); + const auto cp_it = calib_init_poses.find(tcid); - calib_opt->addPoseMeasurement( - timestamp_ns, cp.T_a_c * calib_opt->getCamT_i_c(0).inverse()); + if (cp_it != calib_init_poses.end()) { + calib_opt->addPoseMeasurement( + timestamp_ns, + cp_it->second.T_a_c * calib_opt->getCamT_i_c(0).inverse()); - if (!g_initialized) { - for (size_t i = 0; - i < vio_dataset->get_accel_data().size() && !g_initialized; i++) { - const basalt::AccelData &ad = vio_dataset->get_accel_data()[i]; - if (std::abs(ad.timestamp_ns - timestamp_ns) < 3000000) { - g_a_init = cp.T_a_c.so3() * ad.data; - g_initialized = true; - std::cout << "g_a initialized with " << g_a_init.transpose() - << std::endl; + if (!g_initialized) { + for (size_t i = 0; + i < vio_dataset->get_accel_data().size() && !g_initialized; i++) { + const basalt::AccelData &ad = vio_dataset->get_accel_data()[i]; + if (std::abs(ad.timestamp_ns - timestamp_ns) < 3000000) { + g_a_init = cp_it->second.T_a_c.so3() * ad.data; + g_initialized = true; + std::cout << "g_a initialized with " << g_a_init.transpose() + << std::endl; + } } } } @@ -795,7 +798,8 @@ void CamImuCalib::drawImageOverlay(pangolin::View &v, size_t cam_id) { glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); if (reprojected_corners.find(tcid) != reprojected_corners.end()) { - if (calib_corners.at(tcid).corner_ids.size() >= MIN_CORNERS) { + if (calib_corners.count(tcid) > 0 && + calib_corners.at(tcid).corner_ids.size() >= MIN_CORNERS) { const auto &rc = reprojected_corners.at(tcid); for (size_t i = 0; i < rc.size(); i++) { diff --git a/src/calibration/vignette.cpp b/src/calibration/vignette.cpp index 520c4e4..78f47f1 100644 --- a/src/calibration/vignette.cpp +++ b/src/calibration/vignette.cpp @@ -40,11 +40,13 @@ namespace basalt { VignetteEstimator::VignetteEstimator( const VioDatasetPtr &vio_dataset, const Eigen::vector &optical_centers, + const Eigen::vector &resolutions, const std::map> &reprojected_vignette, const AprilGrid &april_grid) : vio_dataset(vio_dataset), optical_centers(optical_centers), + resolutions(resolutions), reprojected_vignette(reprojected_vignette), april_grid(april_grid), vign_param(vio_dataset->get_num_cams(), @@ -278,9 +280,8 @@ void VignetteEstimator::compute_data_log(pangolin::DataLog &vign_data_log) { void VignetteEstimator::save_vign_png(const std::string &path) { for (size_t k = 0; k < vio_dataset->get_num_cams(); k++) { - auto img = - vio_dataset->get_image_data(vio_dataset->get_image_timestamps()[0])[0]; - pangolin::ManagedImage vign_img(img.img->w, img.img->h); + pangolin::ManagedImage vign_img(resolutions[k][0], + resolutions[k][1]); vign_img.Fill(0); Eigen::Vector2d oc = optical_centers[k];