Merge branch 'pinhole_init' into 'master'

Pinhole init

See merge request basalt/basalt!37

include/basalt/calibration/aprilgrid.h

@@ -44,6 +44,9 @@ struct AprilGrid {
   Eigen::aligned_vector<Eigen::Vector4d> aprilgrid_corner_pos_3d;
   Eigen::aligned_vector<Eigen::Vector4d> aprilgrid_vignette_pos_3d;
 
+  inline int getTagCols() const { return tagCols; }
+  inline int getTagRows() const { return tagRows; }
+
  private:
   int tagCols;        // number of apriltags
   int tagRows;        // number of apriltags

include/basalt/calibration/calibration_helper.h

@@ -34,6 +34,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 #pragma once
 
+#include <basalt/calibration/aprilgrid.h>
 #include <basalt/io/dataset_io.h>
 #include <basalt/utils/common_types.h>
 #include <basalt/calibration/calibration.hpp>
@@ -85,9 +86,13 @@ class CalibHelper {
 
   static bool initializeIntrinsics(
       const Eigen::aligned_vector<Eigen::Vector2d>& corners,
-      const std::vector<int>& corner_ids,
+      const std::vector<int>& corner_ids, const AprilGrid& aprilgrid, int cols,
-      const Eigen::aligned_vector<Eigen::Vector4d>& aprilgrid_corner_pos_3d,
+      int rows, Eigen::Vector4d& init_intr);
-      int cols, int rows, Eigen::Vector4d& init_intr);
+
+  static bool initializeIntrinsicsPinhole(
+      const std::vector<CalibCornerData*> pinhole_corners,
+      const AprilGrid& aprilgrid, int cols, int rows,
+      Eigen::Vector4d& init_intr);
 
  private:
   inline static double square(double x) { return x * x; }

include/basalt/optimization/poses_optimize.h

@@ -34,11 +34,10 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 #pragma once
 
+#include <basalt/calibration/aprilgrid.h>
 #include <basalt/calibration/calibration_helper.h>
 #include <basalt/optimization/poses_linearize.h>
 
-#include <basalt/camera/unified_camera.hpp>
-
 namespace basalt {
 
 class PosesOptimization {
@@ -61,21 +60,6 @@ class PosesOptimization {
   PosesOptimization()
       : lambda(1e-6), min_lambda(1e-12), max_lambda(100), lambda_vee(2) {}
 
-  bool initializeIntrinsics(
-      size_t cam_id, const Eigen::aligned_vector<Eigen::Vector2d> &corners,
-      const std::vector<int> &corner_ids,
-      const Eigen::aligned_vector<Eigen::Vector4d> &aprilgrid_corner_pos_3d,
-      int cols, int rows) {
-    Eigen::Vector4d init_intr;
-
-    bool val = CalibHelper::initializeIntrinsics(
-        corners, corner_ids, aprilgrid_corner_pos_3d, cols, rows, init_intr);
-
-    calib->intrinsics[cam_id].setFromInit(init_intr);
-
-    return val;
-  }
-
   Vector2 getOpticalCenter(size_t i) {
     return calib->intrinsics[i].getParam().template segment<2>(2);
   }

src/calibration/calibraiton_helper.cpp

@@ -47,6 +47,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <opengv/sac_problems/absolute_pose/AbsolutePoseSacProblem.hpp>
 #include <opengv/sac_problems/relative_pose/CentralRelativePoseSacProblem.hpp>
 
+#include <opencv2/calib3d/calib3d.hpp>
+
 namespace basalt {
 
 template <class CamT>
@@ -171,9 +173,8 @@ void CalibHelper::initCamPoses(
 
 bool CalibHelper::initializeIntrinsics(
     const Eigen::aligned_vector<Eigen::Vector2d> &corners,
-    const std::vector<int> &corner_ids,
+    const std::vector<int> &corner_ids, const AprilGrid &aprilgrid, int cols,
-    const Eigen::aligned_vector<Eigen::Vector4d> &aprilgrid_corner_pos_3d,
+    int rows, Eigen::Vector4d &init_intr) {
-    int cols, int rows, Eigen::Vector4d &init_intr) {
   // First, initialize the image center at the center of the image.
 
   Eigen::aligned_map<int, Eigen::Vector2d> id_to_corner;
@@ -181,17 +182,17 @@ bool CalibHelper::initializeIntrinsics(
     id_to_corner[corner_ids[i]] = corners[i];
   }
 
-  double _xi = 1.0;
+  const double _xi = 1.0;
-  double _cu = cols / 2.0 - 0.5;
+  const double _cu = cols / 2.0 - 0.5;
-  double _cv = rows / 2.0 - 0.5;
+  const double _cv = rows / 2.0 - 0.5;
 
   /// Initialize some temporaries needed.
   double gamma0 = 0.0;
   double minReprojErr = std::numeric_limits<double>::max();
 
   // Now we try to find a non-radial line to initialize the focal length
-  const size_t target_cols = 6;
+  const size_t target_cols = aprilgrid.getTagCols();
-  const size_t target_rows = 6;
+  const size_t target_rows = aprilgrid.getTagRows();
 
   bool success = false;
   for (int tag_corner_offset = 0; tag_corner_offset < 2; tag_corner_offset++)
@@ -270,14 +271,14 @@ bool CalibHelper::initializeIntrinsics(
         size_t num_inliers;
         Sophus::SE3d T_target_camera;
         if (!estimateTransformation(cam_calib, corners, corner_ids,
-                                    aprilgrid_corner_pos_3d, T_target_camera,
+                                    aprilgrid.aprilgrid_corner_pos_3d,
-                                    num_inliers)) {
+                                    T_target_camera, num_inliers)) {
           continue;
         }
 
         double reprojErr = 0.0;
         size_t numReprojected = computeReprojectionError(
-            cam_calib, corners, corner_ids, aprilgrid_corner_pos_3d,
+            cam_calib, corners, corner_ids, aprilgrid.aprilgrid_corner_pos_3d,
             T_target_camera, reprojErr);
 
         // std::cerr << "numReprojected " << numReprojected << " reprojErr "
@@ -301,6 +302,105 @@ bool CalibHelper::initializeIntrinsics(
   return success;
 }
 
+bool CalibHelper::initializeIntrinsicsPinhole(
+    const std::vector<CalibCornerData *> pinhole_corners,
+    const AprilGrid &aprilgrid, int cols, int rows,
+    Eigen::Vector4d &init_intr) {
+  // First, initialize the image center at the center of the image.
+
+  const double _cu = cols / 2.0 - 0.5;
+  const double _cv = rows / 2.0 - 0.5;
+
+  // Z. Zhang, A Flexible New Technique for Camera Calibration, PAMI 2000
+
+  size_t nImages = pinhole_corners.size();
+
+  //  Eigen::MatrixXd A(2 * nImages, 2);
+  //  Eigen::VectorXd b(2 * nImages);
+
+  Eigen::MatrixXd A(nImages * 2, 2);
+  Eigen::VectorXd b(nImages * 2, 1);
+
+  int i = 0;
+
+  for (const CalibCornerData *ccd : pinhole_corners) {
+    const auto &corners = ccd->corners;
+    const auto &corner_ids = ccd->corner_ids;
+
+    std::vector<cv::Point2f> M(corners.size()), imagePoints(corners.size());
+    for (size_t j = 0; j < corners.size(); ++j) {
+      M.at(j) =
+          cv::Point2f(aprilgrid.aprilgrid_corner_pos_3d[corner_ids[j]][0],
+                      aprilgrid.aprilgrid_corner_pos_3d[corner_ids[j]][1]);
+
+      //    std::cout << "corner "
+      //              <<
+      //              aprilgrid.aprilgrid_corner_pos_3d[corner_ids[j]].transpose()
+      //              << std::endl;
+
+      imagePoints.at(j) = cv::Point2f(corners[j][0], corners[j][1]);
+    }
+
+    cv::Mat H = cv::findHomography(M, imagePoints);
+
+    if (H.empty()) return false;
+
+    // std::cout << H << std::endl;
+
+    H.at<double>(0, 0) -= H.at<double>(2, 0) * _cu;
+    H.at<double>(0, 1) -= H.at<double>(2, 1) * _cu;
+    H.at<double>(0, 2) -= H.at<double>(2, 2) * _cu;
+    H.at<double>(1, 0) -= H.at<double>(2, 0) * _cv;
+    H.at<double>(1, 1) -= H.at<double>(2, 1) * _cv;
+    H.at<double>(1, 2) -= H.at<double>(2, 2) * _cv;
+
+    double h[3], v[3], d1[3], d2[3];
+    double n[4] = {0, 0, 0, 0};
+
+    for (int j = 0; j < 3; ++j) {
+      double t0 = H.at<double>(j, 0);
+      double t1 = H.at<double>(j, 1);
+      h[j] = t0;
+      v[j] = t1;
+      d1[j] = (t0 + t1) * 0.5;
+      d2[j] = (t0 - t1) * 0.5;
+      n[0] += t0 * t0;
+      n[1] += t1 * t1;
+      n[2] += d1[j] * d1[j];
+      n[3] += d2[j] * d2[j];
+    }
+
+    for (int j = 0; j < 4; ++j) {
+      n[j] = 1.0 / sqrt(n[j]);
+    }
+
+    for (int j = 0; j < 3; ++j) {
+      h[j] *= n[0];
+      v[j] *= n[1];
+      d1[j] *= n[2];
+      d2[j] *= n[3];
+    }
+
+    A(i * 2, 0) = h[0] * v[0];
+    A(i * 2, 1) = h[1] * v[1];
+    A(i * 2 + 1, 0) = d1[0] * d2[0];
+    A(i * 2 + 1, 1) = d1[1] * d2[1];
+    b(i * 2, 0) = -h[2] * v[2];
+    b(i * 2 + 1, 0) = -d1[2] * d2[2];
+
+    i++;
+  }
+
+  Eigen::Vector2d f = (A.transpose() * A).ldlt().solve(A.transpose() * b);
+
+  double fx = sqrt(fabs(1.0 / f(0)));
+  double fy = sqrt(fabs(1.0 / f(1)));
+
+  init_intr << fx, fy, _cu, _cv;
+
+  return true;
+}
+
 void CalibHelper::computeInitialPose(
     const Calibration<double>::Ptr &calib, size_t cam_id,
     const Eigen::aligned_vector<Eigen::Vector4d> &aprilgrid_corner_pos_3d,

src/calibration/cam_calib.cpp

@@ -450,8 +450,14 @@ void CamCalib::initCamIntrinsics() {
 
   calib_opt->resetCalib(vio_dataset->get_num_cams(), cam_types);
 
+  std::vector<bool> cam_initialized(vio_dataset->get_num_cams(), false);
+
+  int inc = 1;
+  if (vio_dataset->get_image_timestamps().size() > 100) inc = 3;
+
   for (size_t j = 0; j < vio_dataset->get_num_cams(); j++) {
-    for (size_t i = 0; i < vio_dataset->get_image_timestamps().size(); i++) {
+    for (size_t i = 0; i < vio_dataset->get_image_timestamps().size();
+         i += inc) {
       const int64_t timestamp_ns = vio_dataset->get_image_timestamps()[i];
       const std::vector<basalt::ImageData> &img_vec =
           vio_dataset->get_image_data(timestamp_ns);
@@ -461,10 +467,59 @@ void CamCalib::initCamIntrinsics() {
       if (calib_corners.find(tcid) != calib_corners.end()) {
         CalibCornerData cid = calib_corners.at(tcid);
 
-        bool success = calib_opt->initializeIntrinsics(
+        Eigen::Vector4d init_intr;
-            j, cid.corners, cid.corner_ids, april_grid.aprilgrid_corner_pos_3d,
+
-            img_vec[j].img->w, img_vec[j].img->h);
+        bool success = CalibHelper::initializeIntrinsics(
-        if (success) break;
+            cid.corners, cid.corner_ids, april_grid, img_vec[j].img->w,
+            img_vec[j].img->h, init_intr);
+
+        if (success) {
+          cam_initialized[j] = true;
+          calib_opt->calib->intrinsics[j].setFromInit(init_intr);
+          break;
+        }
+      }
+    }
+  }
+
+  // Try perfect pinhole initialization for cameras that are not initalized.
+  for (size_t j = 0; j < vio_dataset->get_num_cams(); j++) {
+    if (!cam_initialized[j]) {
+      std::vector<CalibCornerData *> pinhole_corners;
+      int w, h;
+
+      for (size_t i = 0; i < vio_dataset->get_image_timestamps().size();
+           i += inc) {
+        const int64_t timestamp_ns = vio_dataset->get_image_timestamps()[i];
+        const std::vector<basalt::ImageData> &img_vec =
+            vio_dataset->get_image_data(timestamp_ns);
+
+        TimeCamId tcid(timestamp_ns, j);
+
+        auto it = calib_corners.find(tcid);
+        if (it != calib_corners.end()) {
+          if (it->second.corners.size() > 8) {
+            pinhole_corners.emplace_back(&it->second);
+          }
+        }
+
+        w = img_vec[j].img->w;
+        h = img_vec[j].img->h;
+      }
+
+      Eigen::Vector4d init_intr;
+
+      bool success = CalibHelper::initializeIntrinsicsPinhole(
+          pinhole_corners, april_grid, w, h, init_intr);
+
+      if (success) {
+        cam_initialized[j] = true;
+
+        std::cout << "Initialized camera " << j
+                  << " with pinhole model. You should set pinhole model for "
+                     "this camera!"
+                  << std::endl;
+        calib_opt->calib->intrinsics[j].setFromInit(init_intr);
       }
     }
   }

thirdparty/basalt-headers

@@ -1 +1 @@
-Subproject commit 34668f662181af61b504400fa80c7f4416c7f6e2
+Subproject commit d4c366688fa4c6daa74822dffdca3cefd0e04223