From 619f6be6bdf8b681d457dd48380c682979fb311a Mon Sep 17 00:00:00 2001 From: Vladyslav Usenko Date: Mon, 18 Nov 2019 07:33:04 +0000 Subject: [PATCH] Pinhole init --- include/basalt/calibration/aprilgrid.h | 3 + .../basalt/calibration/calibration_helper.h | 11 +- include/basalt/optimization/poses_optimize.h | 18 +-- src/calibration/calibraiton_helper.cpp | 122 ++++++++++++++++-- src/calibration/cam_calib.cpp | 65 +++++++++- thirdparty/basalt-headers | 2 +- 6 files changed, 184 insertions(+), 37 deletions(-) diff --git a/include/basalt/calibration/aprilgrid.h b/include/basalt/calibration/aprilgrid.h index dc618e4..019a136 100644 --- a/include/basalt/calibration/aprilgrid.h +++ b/include/basalt/calibration/aprilgrid.h @@ -44,6 +44,9 @@ struct AprilGrid { Eigen::aligned_vector aprilgrid_corner_pos_3d; Eigen::aligned_vector 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 diff --git a/include/basalt/calibration/calibration_helper.h b/include/basalt/calibration/calibration_helper.h index a71ebc9..49bb151 100644 --- a/include/basalt/calibration/calibration_helper.h +++ b/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 #include #include #include @@ -85,9 +86,13 @@ class CalibHelper { static bool initializeIntrinsics( const Eigen::aligned_vector& corners, - const std::vector& corner_ids, - const Eigen::aligned_vector& aprilgrid_corner_pos_3d, - int cols, int rows, Eigen::Vector4d& init_intr); + const std::vector& corner_ids, const AprilGrid& aprilgrid, int cols, + int rows, Eigen::Vector4d& init_intr); + + static bool initializeIntrinsicsPinhole( + const std::vector pinhole_corners, + const AprilGrid& aprilgrid, int cols, int rows, + Eigen::Vector4d& init_intr); private: inline static double square(double x) { return x * x; } diff --git a/include/basalt/optimization/poses_optimize.h b/include/basalt/optimization/poses_optimize.h index 23cabdd..b2f2056 100644 --- a/include/basalt/optimization/poses_optimize.h +++ b/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 #include #include -#include - 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 &corners, - const std::vector &corner_ids, - const Eigen::aligned_vector &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); } diff --git a/src/calibration/calibraiton_helper.cpp b/src/calibration/calibraiton_helper.cpp index c4cfe61..2597fc2 100644 --- a/src/calibration/calibraiton_helper.cpp +++ b/src/calibration/calibraiton_helper.cpp @@ -47,6 +47,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include + namespace basalt { template @@ -171,9 +173,8 @@ void CalibHelper::initCamPoses( bool CalibHelper::initializeIntrinsics( const Eigen::aligned_vector &corners, - const std::vector &corner_ids, - const Eigen::aligned_vector &aprilgrid_corner_pos_3d, - int cols, int rows, Eigen::Vector4d &init_intr) { + const std::vector &corner_ids, const AprilGrid &aprilgrid, int cols, + int rows, Eigen::Vector4d &init_intr) { // First, initialize the image center at the center of the image. Eigen::aligned_map id_to_corner; @@ -181,17 +182,17 @@ bool CalibHelper::initializeIntrinsics( id_to_corner[corner_ids[i]] = corners[i]; } - double _xi = 1.0; - double _cu = cols / 2.0 - 0.5; - double _cv = rows / 2.0 - 0.5; + const double _xi = 1.0; + const double _cu = cols / 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::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_rows = 6; + const size_t target_cols = aprilgrid.getTagCols(); + 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, - num_inliers)) { + aprilgrid.aprilgrid_corner_pos_3d, + 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 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 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(0, 0) -= H.at(2, 0) * _cu; + H.at(0, 1) -= H.at(2, 1) * _cu; + H.at(0, 2) -= H.at(2, 2) * _cu; + H.at(1, 0) -= H.at(2, 0) * _cv; + H.at(1, 1) -= H.at(2, 1) * _cv; + H.at(1, 2) -= H.at(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(j, 0); + double t1 = H.at(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::Ptr &calib, size_t cam_id, const Eigen::aligned_vector &aprilgrid_corner_pos_3d, diff --git a/src/calibration/cam_calib.cpp b/src/calibration/cam_calib.cpp index b2c5ef9..da21587 100644 --- a/src/calibration/cam_calib.cpp +++ b/src/calibration/cam_calib.cpp @@ -450,8 +450,14 @@ void CamCalib::initCamIntrinsics() { calib_opt->resetCalib(vio_dataset->get_num_cams(), cam_types); + std::vector 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 &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( - j, cid.corners, cid.corner_ids, april_grid.aprilgrid_corner_pos_3d, - img_vec[j].img->w, img_vec[j].img->h); - if (success) break; + Eigen::Vector4d init_intr; + + bool success = CalibHelper::initializeIntrinsics( + 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 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 &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); } } } diff --git a/thirdparty/basalt-headers b/thirdparty/basalt-headers index 34668f6..d4c3666 160000 --- a/thirdparty/basalt-headers +++ b/thirdparty/basalt-headers @@ -1 +1 @@ -Subproject commit 34668f662181af61b504400fa80c7f4416c7f6e2 +Subproject commit d4c366688fa4c6daa74822dffdca3cefd0e04223