diff --git a/cpp/drafts/Calibration/tool/calibrate_omnidirectional_cameras.cpp b/cpp/drafts/Calibration/tool/calibrate_omnidirectional_cameras.cpp index 5c3991deb..872a5bd08 100644 --- a/cpp/drafts/Calibration/tool/calibrate_omnidirectional_cameras.cpp +++ b/cpp/drafts/Calibration/tool/calibrate_omnidirectional_cameras.cpp @@ -101,6 +101,7 @@ class ChessboardCalibrationProblem _observations_3d.push_back(x * square_size); _observations_3d.push_back(y * square_size); + _observations_3d.push_back(0); ++num_points; } @@ -153,16 +154,17 @@ class ChessboardCalibrationProblem auto transform_into_ceres_problem(ceres::Problem& problem) -> void { auto loss_fn = nullptr; // new ceres::HuberLoss{1.0}; - auto data_pos = std::size_t{}; + auto obs_ptr = _observations_2d.data(); + auto scene_ptr = _observations_3d.data(); for (auto n = 0; n < _num_images; ++n) { const auto& num_points = _num_points_at_image[n]; for (auto p = 0; p < num_points; ++p) { - const Eigen::Vector2d image_point = Eigen::Map( - _observations_2d.data() + data_pos); - const Eigen::Vector2d scene_point = Eigen::Map( - _observations_3d.data() + data_pos); + const Eigen::Vector2d image_point = + Eigen::Map(obs_ptr); + const Eigen::Vector3d scene_point = + Eigen::Map(scene_ptr); auto cost_function = sara::OmnidirectionalCameraReprojectionError::create(image_point, @@ -171,7 +173,8 @@ class ChessboardCalibrationProblem problem.AddResidualBlock(cost_function, loss_fn, // mutable_intrinsics(), mutable_extrinsics(n)); - data_pos += 2; + obs_ptr += 2; + scene_ptr += 3; } } @@ -179,13 +182,14 @@ class ChessboardCalibrationProblem static constexpr auto fy_normalized = 1; // alpha is the normalized_shear. static constexpr auto alpha = 2; // shear = alpha * fx - static constexpr auto u0 = 3; - static constexpr auto v0 = 4; - static constexpr auto k0 = 5; - static constexpr auto k1 = 6; - static constexpr auto p0 = 7; - static constexpr auto p1 = 8; - static constexpr auto xi = 9; + [[maybe_unused]] static constexpr auto u0 = 3; + [[maybe_unused]] static constexpr auto v0 = 4; + static constexpr auto xi = 5; + static constexpr auto k0 = 6; + static constexpr auto k1 = 7; + static constexpr auto k2 = 8; + static constexpr auto p0 = 9; + static constexpr auto p1 = 10; // Bounds on the calibration matrix. problem.SetParameterLowerBound(mutable_intrinsics(), fx, 500); @@ -202,10 +206,10 @@ class ChessboardCalibrationProblem // Bounds on the distortion coefficients. // - We should not need any further adjustment on the bounds. - for (const auto& dist_coeff : {k0, k1, p0, p1}) + for (const auto& idx : {k0, k1, p0, p1}) { - problem.SetParameterLowerBound(mutable_intrinsics(), dist_coeff, -1); - problem.SetParameterUpperBound(mutable_intrinsics(), dist_coeff, 1); + problem.SetParameterLowerBound(mutable_intrinsics(), idx, -1.); + problem.SetParameterUpperBound(mutable_intrinsics(), idx, 1.); } // for (const auto& i : {u0, v0}) @@ -222,7 +226,8 @@ class ChessboardCalibrationProblem // parameter to 1. // // By default freeze the principal point. - auto intrinsics_to_freeze = std::vector{}; + auto intrinsics_to_freeze = + std::vector{fy_normalized, alpha, u0, v0, k2}; switch (_camera_type) { case CameraType::Fisheye: @@ -240,8 +245,8 @@ class ChessboardCalibrationProblem } // Impose a fixed principal point. - auto intrinsic_manifold = - new ceres::SubsetManifold{10, intrinsics_to_freeze}; + auto intrinsic_manifold = new ceres::SubsetManifold{ + intrinsic_parameter_count, intrinsics_to_freeze}; problem.SetManifold(mutable_intrinsics(), intrinsic_manifold); } @@ -250,17 +255,25 @@ class ChessboardCalibrationProblem { // Copy back the final parameter to the omnidirectional camera parameter // object. - const auto fx = mutable_intrinsics()[0]; - const auto fy = mutable_intrinsics()[1] * fx; - const auto alpha = mutable_intrinsics()[2]; + + // Calibration matrix. + const auto& fx = mutable_intrinsics()[0]; + const auto& fy_normalized = mutable_intrinsics()[1]; + const auto& alpha = mutable_intrinsics()[2]; + const auto fy = fy_normalized * fx; const auto shear = fx * alpha; - const auto u0 = mutable_intrinsics()[3]; - const auto v0 = mutable_intrinsics()[4]; - const auto k0 = mutable_intrinsics()[5]; - const auto k1 = mutable_intrinsics()[6]; - const auto p0 = mutable_intrinsics()[7]; - const auto p1 = mutable_intrinsics()[8]; - const auto xi = mutable_intrinsics()[9]; + const auto& u0 = mutable_intrinsics()[3]; + const auto& v0 = mutable_intrinsics()[4]; + + // Mirror parameter. + const auto& xi = mutable_intrinsics()[5]; + + // Distortion parameters. + const auto& k0 = mutable_intrinsics()[6]; + const auto& k1 = mutable_intrinsics()[7]; + const auto& k2 = mutable_intrinsics()[8]; + const auto& p0 = mutable_intrinsics()[9]; + const auto& p1 = mutable_intrinsics()[10]; // clang-format off auto K = Eigen::Matrix3d{}; K << fx, shear, u0, @@ -268,71 +281,12 @@ class ChessboardCalibrationProblem 0, 0, 1; // clang-format on camera.set_calibration_matrix(K); - camera.radial_distortion_coefficients << k0, k1, 0; + camera.radial_distortion_coefficients << k0, k1, k2; camera.tangential_distortion_coefficients << p0, p1; camera.xi = xi; } - auto reinitialize_extrinsic_parameters() -> void - { - throw std::runtime_error{"Implementation incomplete!"}; - - const auto to_camera = - [](const double*) -> sara::OmnidirectionalCamera { return {}; }; - const auto camera = to_camera(_intrinsics.data()); - - auto data_pos = std::size_t{}; - - for (auto image_index = 0; image_index < _num_images; ++image_index) - { - const auto& num_points = _num_points_at_image[image_index]; - auto p2ns = Eigen::MatrixXd{3, num_points}; - auto p3s = Eigen::MatrixXd{3, num_points}; - - auto c = 0; - for (auto y = 0; y < _h; ++y) - { - for (auto x = 0; x < _w; ++x) - { - const Eigen::Vector2d p2 = Eigen::Map( - _observations_2d.data() + data_pos); - - const Eigen::Vector3d p3 = Eigen::Map( - _observations_2d.data() + data_pos) - .homogeneous(); - - const Eigen::Vector3d ray = camera.backproject(p2); - const Eigen::Vector2d p2n = ray.hnormalized(); - - p2ns.col(c) << p2n, 1; - p3s.col(c) = p3; - - ++c; - data_pos += 2; - } - } - - auto extrinsics = mutable_extrinsics(image_index); - auto angle_axis_ptr = extrinsics; - auto translation_ptr = extrinsics + 3; - -#if 0 - const auto angle_axis = Eigen::AngleAxisd{Rs.front()}; - const auto& angle = angle_axis.angle(); - const auto& axis = angle_axis.axis(); -#else - for (auto k = 0; k < 3; ++k) - angle_axis_ptr[k] = 0; // angle * axis(k); - - for (auto k = 0; k < 3; ++k) - translation_ptr[k] = 0; // ts.front()(k); -#endif - } - } - private: - int _w = 0; - int _h = 0; int _num_images = 0; std::vector _num_points_at_image; diff --git a/cpp/src/DO/Sara/MultiViewGeometry/Calibration/OmnidirectionalCameraReprojectionError.hpp b/cpp/src/DO/Sara/MultiViewGeometry/Calibration/OmnidirectionalCameraReprojectionError.hpp index 95c18a846..0ba79077d 100644 --- a/cpp/src/DO/Sara/MultiViewGeometry/Calibration/OmnidirectionalCameraReprojectionError.hpp +++ b/cpp/src/DO/Sara/MultiViewGeometry/Calibration/OmnidirectionalCameraReprojectionError.hpp @@ -26,12 +26,12 @@ namespace DO::Sara { struct OmnidirectionalCameraReprojectionError { static constexpr auto residual_dimension = 2; - static constexpr auto intrinsic_parameter_count = 10; + static constexpr auto intrinsic_parameter_count = 11; static constexpr auto extrinsic_parameter_count = 6; inline OmnidirectionalCameraReprojectionError( const Eigen::Vector2d& image_point, // - const Eigen::Vector2d& scene_point) + const Eigen::Vector3d& scene_point) : image_point{image_point} , scene_point{scene_point} { @@ -59,7 +59,7 @@ namespace DO::Sara { template inline auto apply_distortion(const Eigen::Matrix& mu, const T& k1, - const T& k2, const T& p1, + const T& k2, const T& k3, const T& p1, const T& p2) const -> Eigen::Matrix { using Vector2 = Eigen::Matrix; @@ -67,7 +67,8 @@ namespace DO::Sara { // Radial component (additive). const auto r2 = mu.squaredNorm(); const auto r4 = r2 * r2; - const Vector2 radial_factor = mu * (k1 * r2 + k2 * r4); + const auto r6 = r4 * r2; + const Vector2 radial_factor = mu * (k1 * r2 + k2 * r4 + k3 * r6); // Tangential component (additive). static constexpr auto two = 2.; @@ -90,11 +91,7 @@ namespace DO::Sara { // 1. Apply [R|t] = extrinsics[...] // // a) extrinsics[0, 1, 2] are the angle-axis rotation. - const auto scene_coords = Vector3{ - static_cast(scene_point.x()), // - static_cast(scene_point.y()), // - T{} // - }; + const Vector3 scene_coords = scene_point.template cast(); auto camera_coords = Vector3{}; ceres::AngleAxisRotatePoint(extrinsics, scene_coords.data(), camera_coords.data()); @@ -103,17 +100,28 @@ namespace DO::Sara { const auto t = Eigen::Map{extrinsics + 3}; camera_coords += t; - const auto& xi = intrinsics[9]; + // Project the scene point to the spherical mirror by shooting a ray. + // The ray leaves the scene point, passes through the optical center and + // hits the spherical mirror. + const auto& xi = intrinsics[5]; const auto m = apply_mirror_transformation(camera_coords, xi); - // Distortion. - const auto& k1 = intrinsics[5]; - const auto& k2 = intrinsics[6]; - const auto& p1 = intrinsics[7]; - const auto& p2 = intrinsics[8]; - const Vector2 m_distorted = apply_distortion(m, k1, k2, p1, p2); - - // Apply the calibration matrix. + // The ray is then reflected by the spherical mirror and finally hits the + // image plane. + // + // The spherical mirror or the image plane has an imperfect geometry, so + // apply the distortion. (?). + // + // TODO: re-read the paper just to make sure we understood right. + const auto& k1 = intrinsics[6]; + const auto& k2 = intrinsics[7]; + const auto& k3 = intrinsics[8]; + const auto& p1 = intrinsics[9]; + const auto& p2 = intrinsics[10]; + const Vector2 m_distorted = apply_distortion(m, k1, k2, k3, p1, p2); + + // Apply the calibration matrix to get the pixel coordinates of the point + // of impact. const auto& fx = intrinsics[0]; const auto fy_normalized = intrinsics[1]; const auto fy = fy_normalized * fx; @@ -135,7 +143,7 @@ namespace DO::Sara { } static inline auto create(const Eigen::Vector2d& image_point, - const Eigen::Vector2d& scene_point) + const Eigen::Vector3d& scene_point) { return new ceres::AutoDiffCostFunction< OmnidirectionalCameraReprojectionError, // @@ -145,7 +153,7 @@ namespace DO::Sara { } Eigen::Vector2d image_point; - Eigen::Vector2d scene_point; + Eigen::Vector3d scene_point; }; } // namespace DO::Sara