Fixes for datasets with many skipped frames

include/basalt/calibration/vignette.h

@@ -51,6 +51,7 @@ class VignetteEstimator {
 
   VignetteEstimator(const VioDatasetPtr &vio_dataset,
                     const Eigen::vector<Eigen::Vector2d> &optical_centers,
+                    const Eigen::vector<Eigen::Vector2i> &resolutions,
                     const std::map<TimeCamId, Eigen::vector<Eigen::Vector3d>>
                         &reprojected_vignette,
                     const AprilGrid &april_grid);
@@ -77,6 +78,7 @@ class VignetteEstimator {
  private:
   const VioDatasetPtr vio_dataset;
   Eigen::vector<Eigen::Vector2d> optical_centers;
+  Eigen::vector<Eigen::Vector2i> resolutions;
   std::map<TimeCamId, Eigen::vector<Eigen::Vector3d>> reprojected_vignette;
   const AprilGrid &april_grid;
 

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];
+    size_t img_idx = 1;
-    const auto img_data = vio_dataset->get_image_data(t_ns);
+    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<Eigen::Vector2i> 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);
+    for (size_t cam_id = 0; cam_id < calib_opt->calib->T_i_c.size(); cam_id++) {
-    const CalibInitPoseData &cp = calib_init_poses.at(tcid);
+      TimeCamId tcid = std::make_pair(timestamp_ns, cam_id);
+      const auto cp_it = calib_init_poses.find(tcid);
 
-    calib_opt->addPoseMeasurement(
+      if (cp_it != calib_init_poses.end()) {
-        timestamp_ns, cp.T_a_c * calib_opt->calib->T_i_c[0].inverse());
+        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;

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(
+    if (cp_it != calib_init_poses.end()) {
-        timestamp_ns, cp.T_a_c * calib_opt->getCamT_i_c(0).inverse());
+      calib_opt->addPoseMeasurement(
+          timestamp_ns,
+          cp_it->second.T_a_c * calib_opt->getCamT_i_c(0).inverse());
 
-    if (!g_initialized) {
+      if (!g_initialized) {
-      for (size_t i = 0;
+        for (size_t i = 0;
-           i < vio_dataset->get_accel_data().size() && !g_initialized; i++) {
+             i < vio_dataset->get_accel_data().size() && !g_initialized; i++) {
-        const basalt::AccelData &ad = vio_dataset->get_accel_data()[i];
+          const basalt::AccelData &ad = vio_dataset->get_accel_data()[i];
-        if (std::abs(ad.timestamp_ns - timestamp_ns) < 3000000) {
+          if (std::abs(ad.timestamp_ns - timestamp_ns) < 3000000) {
-          g_a_init = cp.T_a_c.so3() * ad.data;
+            g_a_init = cp_it->second.T_a_c.so3() * ad.data;
-          g_initialized = true;
+            g_initialized = true;
-          std::cout << "g_a initialized with " << g_a_init.transpose()
+            std::cout << "g_a initialized with " << g_a_init.transpose()
-                    << std::endl;
+                      << 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++) {

src/calibration/vignette.cpp

@@ -40,11 +40,13 @@ namespace basalt {
 VignetteEstimator::VignetteEstimator(
     const VioDatasetPtr &vio_dataset,
     const Eigen::vector<Eigen::Vector2d> &optical_centers,
+    const Eigen::vector<Eigen::Vector2i> &resolutions,
     const std::map<TimeCamId, Eigen::vector<Eigen::Vector3d>>
         &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 =
+    pangolin::ManagedImage<uint16_t> vign_img(resolutions[k][0],
-        vio_dataset->get_image_data(vio_dataset->get_image_timestamps()[0])[0];
+                                              resolutions[k][1]);
-    pangolin::ManagedImage<uint16_t> vign_img(img.img->w, img.img->h);
     vign_img.Fill(0);
 
     Eigen::Vector2d oc = optical_centers[k];