diff --git a/cpp/farm_ng/core/logging/assert_within.h b/cpp/farm_ng/core/logging/assert_within.h index b8269818..12a4fedd 100644 --- a/cpp/farm_ng/core/logging/assert_within.h +++ b/cpp/farm_ng/core/logging/assert_within.h @@ -19,8 +19,6 @@ #include - - namespace farm_ng { /// Checks whether two floating point numbers x, y are close to each other. diff --git a/cpp/sophus2/common/common.h b/cpp/sophus2/common/common.h index 0a170fa6..1a00ca32 100644 --- a/cpp/sophus2/common/common.h +++ b/cpp/sophus2/common/common.h @@ -72,7 +72,7 @@ template <> inline float const kEpsilon = float(1e-5); static float const kEpsilonF32 = kEpsilon; -static float const kEpsilonF64 = kEpsilon; +static double const kEpsilonF64 = kEpsilon; /// Slightly larger than kEpsilon. template diff --git a/cpp/sophus2/concepts/params.h b/cpp/sophus2/concepts/params.h index 26a87c5f..da43adea 100644 --- a/cpp/sophus2/concepts/params.h +++ b/cpp/sophus2/concepts/params.h @@ -115,7 +115,4 @@ struct ScalarBinaryOpTraits< using ReturnType = sophus2::concepts::CompatScalarEx; }; - - - } // namespace Eigen diff --git a/cpp/sophus2/lie/impl/spiral_similarity2.h b/cpp/sophus2/lie/impl/spiral_similarity2.h index a6386dc8..f99e1578 100644 --- a/cpp/sophus2/lie/impl/spiral_similarity2.h +++ b/cpp/sophus2/lie/impl/spiral_similarity2.h @@ -10,6 +10,7 @@ #include "sophus2/concepts/lie_group.h" #include "sophus2/lie/impl/rotation2.h" #include "sophus2/lie/impl/sim_mat_w.h" +#include "sophus2/linalg/batch.h" #include "sophus2/manifold/complex.h" #include "sophus2/manifold/unit_vector.h" @@ -100,6 +101,7 @@ class SpiralSimilarity2Impl { } static auto log(Params const& complex) -> Tangent { + using std::atan2; using std::log; Tangent theta_sigma; theta_sigma[0] = diff --git a/cpp/sophus2/linalg/batch.h b/cpp/sophus2/linalg/batch.h index 63cee295..ba8d8dbe 100644 --- a/cpp/sophus2/linalg/batch.h +++ b/cpp/sophus2/linalg/batch.h @@ -11,6 +11,8 @@ #include "sophus2/common/common.h" #include "sophus2/concepts/batch.h" +#include + namespace sophus2 { // Following similar pattern as ceres::Jet @@ -41,7 +43,7 @@ struct Batch { template auto operator<<(std::ostream& os, Batch const& batch) -> std::ostream& { - os << "[" << batch.lanes << "]"; + os << "[" << batch.lanes.transpose() << "]"; return os; } @@ -55,6 +57,27 @@ inline Batch abs(Batch const& f) { return Batch{f.lanes.abs().eval()}; } +template +inline Batch tan(Batch const& f) { + return Batch{f.lanes.tan().eval()}; +} + +template +inline Batch atan2(Batch const& lhs, Batch const& rhs) { + Eigen::Array result; + using std::atan2; + + // TODO: See whether there is a more efficient way. + // First we would want to confirm that the other functions (tan, sqrt) + // have efficient versions - by inspecting the Eigen implementation + // for relevant intrinsics (AMD64 and ARM64). + for (int i = 0; i < N; ++i) { + result[i] = atan2(lhs.lanes[i], rhs.lanes[i]); + } + + return Batch{result}; +} + template inline Batch sqrt(Batch const& f) { return Batch{f.lanes.sqrt().eval()}; @@ -113,6 +136,12 @@ struct BatchTrait> { } }; +template +using BatchVector = Eigen::Vector, kRows>; + +template +using BatchMatrix = Eigen::Matrix, kRows, kCols>; + } // namespace sophus2 namespace Eigen { @@ -163,3 +192,11 @@ struct ScalarBinaryOpTraits, BinaryOp> { }; } // namespace Eigen + +// Defined in fmt namespace due to gcc bug +// https://stackoverflow.com/a/69144223 +namespace fmt { + +template +struct formatter> : ostream_formatter {}; +} // namespace fmt \ No newline at end of file diff --git a/cpp/sophus2/sensor/camera_distortion/kannala_brandt.h b/cpp/sophus2/sensor/camera_distortion/kannala_brandt.h index 50feb1cc..3079c332 100644 --- a/cpp/sophus2/sensor/camera_distortion/kannala_brandt.h +++ b/cpp/sophus2/sensor/camera_distortion/kannala_brandt.h @@ -11,6 +11,7 @@ #include "sophus2/ceres/jet_helpers.h" #include "sophus2/common/common.h" #include "sophus2/geometry/point_transform.h" +#include "sophus2/linalg/batch.h" #include "sophus2/sensor/camera_distortion/affine.h" #include @@ -24,7 +25,7 @@ class KannalaBrandtK3Transform { public: static int constexpr kNumDistortionParams = 4; static int constexpr kNumParams = kNumDistortionParams + 4; - static constexpr const std::string_view kProjectionModel = + static std::string_view constexpr const kProjectionModel = "KannalaBrandtK3: fx, fy, cx, cy, kb0, kb1, kb2, kb3"; template @@ -52,38 +53,55 @@ class KannalaBrandtK3Transform { static_assert( TPointTypeT::RowsAtCompileTime == 2, "point_camera must have exactly 2 columns"); + using Scalar = typename TPointTypeT::Scalar; - auto const k0 = params[4]; - auto const k1 = params[5]; - auto const k2 = params[6]; - auto const k3 = params[7]; + Scalar const k0 = params[4]; + Scalar const k1 = params[5]; + Scalar const k2 = params[6]; + Scalar const k3 = params[7]; - auto const radius_squared = + Scalar const radius_squared = proj_point_in_camera_z1_plane[0] * proj_point_in_camera_z1_plane[0] + proj_point_in_camera_z1_plane[1] * proj_point_in_camera_z1_plane[1]; using std::atan2; using std::sqrt; - if (radius_squared > sophus2::kEpsilonF64) { - auto const radius = sqrt(radius_squared); - auto const radius_inverse = 1.0 / radius; - auto const theta = atan2(radius, typename TPointTypeT::Scalar(1.0)); - auto const theta2 = theta * theta; - auto const theta4 = theta2 * theta2; - auto const theta6 = theta4 * theta2; - auto const theta8 = theta4 * theta4; - auto const r_distorted = - theta * (1.0 + k0 * theta2 + k1 * theta4 + k2 * theta6 + k3 * theta8); - auto const scaling = r_distorted * radius_inverse; - - return scaling * proj_point_in_camera_z1_plane.cwiseProduct( - params.template head<2>()) + + auto scaling_fn = [&]() -> Scalar { + Scalar const radius = sqrt(radius_squared); + Scalar const radius_inverse = Scalar(1.0) / radius; + Scalar const theta = atan2(radius, Scalar(1.0)); + Scalar const theta2 = theta * theta; + Scalar const theta4 = theta2 * theta2; + Scalar const theta6 = theta4 * theta2; + Scalar const theta8 = theta4 * theta4; + Scalar const r_distorted = + theta * + (Scalar(1.0) + k0 * theta2 + k1 * theta4 + k2 * theta6 + k3 * theta8); + return r_distorted * radius_inverse; + }; + + if constexpr (BatchTrait::kIsBatch) { + auto const scaling = + (radius_squared.lanes > Scalar(sophus2::kEpsilonF64).lanes) + .select(scaling_fn().lanes, Scalar(1.0).lanes); + + return Scalar{scaling} * + PixelImage( + proj_point_in_camera_z1_plane[0] * params[0], + proj_point_in_camera_z1_plane[1] * params[1]) + params.template segment<2>(2); - } // linearize r around radius=0 - - return AffineTransform::distort( - - params.template head<4>(), proj_point_in_camera_z1_plane); + } else { + // linearize r around radius=0 + Scalar scaling = Scalar(1.0); + if (radius_squared > sophus2::kEpsilonF64) { + scaling = scaling_fn(); + } + return Scalar{scaling} * + PixelImage( + proj_point_in_camera_z1_plane[0] * params[0], + proj_point_in_camera_z1_plane[1] * params[1]) + + params.template segment<2>(2); + } } template @@ -95,56 +113,58 @@ class KannalaBrandtK3Transform { using std::tan; // Undistortion - const TScalar fu = params[0]; - const TScalar fv = params[1]; - const TScalar u0 = params[2]; - const TScalar v0 = params[3]; - - const TScalar k0 = params[4]; - const TScalar k1 = params[5]; - const TScalar k2 = params[6]; - const TScalar k3 = params[7]; - - const TScalar un = (pixel_image(0) - u0) / fu; - const TScalar vn = (pixel_image(1) - v0) / fv; - const TScalar rth2 = un * un + vn * vn; - - if (rth2 < sophus2::kEpsilon * sophus2::kEpsilon) { - return Eigen::Matrix(un, vn); - } + TScalar const fu = params[0]; + TScalar const fv = params[1]; + TScalar const u0 = params[2]; + TScalar const v0 = params[3]; - const TScalar rth = sqrt(rth2); + TScalar const k0 = params[4]; + TScalar const k1 = params[5]; + TScalar const k2 = params[6]; + TScalar const k3 = params[7]; + + TScalar const un = (pixel_image(0) - u0) / fu; + TScalar const vn = (pixel_image(1) - v0) / fv; + TScalar const rth2 = un * un + vn * vn; + + // FARM_INFO("rth2: {}", jet_helpers::GetValue::impl(rth2)); + + // if (rth2 < sophus2::kEpsilon * sophus2::kEpsilon) { + // return Eigen::Matrix(un, vn); + // } + + TScalar const rth = sqrt(rth2); // Use Newtons method to solve for theta, 50 iterations max TScalar th = sqrt(rth); - for (int i = 0; i < 500; ++i) { - const TScalar th2 = th * th; - const TScalar th4 = th2 * th2; - const TScalar th6 = th4 * th2; - const TScalar th8 = th4 * th4; + for (int i = 0; i < 50; ++i) { + TScalar const th2 = th * th; + TScalar const th4 = th2 * th2; + TScalar const th6 = th4 * th2; + TScalar const th8 = th4 * th4; - const TScalar thd = + TScalar const thd = th * (TScalar(1.0) + k0 * th2 + k1 * th4 + k2 * th6 + k3 * th8); - const TScalar d_thd_wtr_th = + TScalar const d_thd_wtr_th = TScalar(1.0) + TScalar(3.0) * k0 * th2 + TScalar(5.0) * k1 * th4 + TScalar(7.0) * k2 * th6 + TScalar(9.0) * k3 * th8; - const TScalar step = (thd - rth) / d_thd_wtr_th; - th -= step; + TScalar const step = (thd - rth) / d_thd_wtr_th; + th = th - step; // has converged? - if (abs(jet_helpers::GetValue::impl(step)) < - sophus2::kEpsilon) { - break; - } + // if (abs(jet_helpers::GetValue::impl(step)) < + // sophus2::kEpsilon) { + // break; + // } } TScalar radius_undistorted = tan(th); - if (radius_undistorted < TScalar(0.0)) { - return Eigen::Matrix( - -radius_undistorted * un / rth, -radius_undistorted * vn / rth); - } + // if (radius_undistorted < TScalar(0.0)) { + // return Eigen::Matrix( + // -radius_undistorted * un / rth, -radius_undistorted * vn / rth); + // } return Eigen::Matrix( radius_undistorted * un / rth, radius_undistorted * vn / rth); } diff --git a/cpp/sophus2/sensor/camera_model.h b/cpp/sophus2/sensor/camera_model.h index 09dc93ca..4c52b997 100644 --- a/cpp/sophus2/sensor/camera_model.h +++ b/cpp/sophus2/sensor/camera_model.h @@ -213,12 +213,12 @@ class CameraModelT { [[nodiscard]] auto scale(ImageSize const& image_size) const -> CameraModelT { Params params = this->params_; - params[0] = Scalar(image_size.width) / Scalar(image_size_.width) * - params[0]; // fx + params[0] = + Scalar(image_size.width) / Scalar(image_size_.width) * params[0]; // fx params[1] = Scalar(image_size.height) / Scalar(image_size_.height) * params[1]; // fy - params[2] = Scalar(image_size.width) / Scalar(image_size_.width) * - params[2]; // cx + params[2] = + Scalar(image_size.width) / Scalar(image_size_.width) * params[2]; // cx params[3] = Scalar(image_size.height) / Scalar(image_size_.height) * params[3]; // cy return CameraModelT({image_size.width, image_size.height}, params); @@ -277,9 +277,10 @@ class CameraModelT { /// /// The point is projected onto the xy-plane at z = `depth_z`. [[nodiscard]] auto camUnproj( - PixelImage const& pixel_in_image, double depth_z) const -> PointCamera { - return Proj::unproj( - Distortion::template undistort(params_, pixel_in_image), depth_z); + PixelImage const& pixel_in_image, Scalar depth_z) const -> PointCamera { + ProjInCameraLifted dist = + Distortion::template undistort(params_, pixel_in_image); + return Proj::unproj(dist, depth_z); } /// Raw data access. To be used in ceres optimization only. @@ -414,7 +415,7 @@ class CameraModel { [[nodiscard]] auto params() const -> Eigen::VectorXd; /// Sets `params` vector. - /// + /// Batch8PinholeModel batch_model; /// Precontion: ``params.size()`` must match the number of parameters of the /// specified `projection_type` (TransformModel::kNumParams). void setParams(Eigen::VectorXd const& params); @@ -461,8 +462,8 @@ class CameraModel { /// See for details: /// https://docs.google.com/document/d/1xmhCMWklP2UoQMGaMqFnsoPWoeMvBfXN7S8-ie9k0UA/edit#heading=h.97r8rr8owwpc /// - /// If the original width [height] is odd, the new width [height] will be: - /// (width+1)/2 [height+1)/2]. + /// If the original width [height] is oddBatch8PinholeModel batch_model;, the + /// new width [height] will be: (width+1)/2 [height+1)/2]. [[nodiscard]] auto subsampleDown() const -> CameraModel; /// Subsamples pixel up, factor of 2.0. diff --git a/cpp/sophus2/sensor/camera_model_test.cpp b/cpp/sophus2/sensor/camera_model_test.cpp index 5c63c683..d9cb57d5 100644 --- a/cpp/sophus2/sensor/camera_model_test.cpp +++ b/cpp/sophus2/sensor/camera_model_test.cpp @@ -11,6 +11,7 @@ #include "sophus2/calculus/num_diff.h" #include "sophus2/image/interpolation.h" #include "sophus2/lie/se3.h" +#include "sophus2/linalg/batch.h" #include "sophus2/sensor/orthographic.h" #include @@ -32,30 +33,108 @@ auto openCvCameraModel() -> CameraModel { return CameraModel(BrownConradyModel({w, h}, get_params)); } -// TODO: move to farm_ng_utils - -// Eigen::Vector2d findImagePoint( -// cv::Mat image, int near_x, int near_y, int window) { -// SOPHUS_ASSERT_EQ(image.type(), CV_32FC1); -// double x_res = 0; -// double y_res = 0; -// double total = 0; -// for (int i = -window; i <= window; ++i) { -// for (int j = -window; j <= window; ++j) { -// double pixel = -// static_cast(image.at(i + near_y, j + near_x)); -// if (pixel < 1) { -// y_res += static_cast(i + near_y) * (1 - pixel); -// x_res += static_cast(j + near_x) * (1 - pixel); -// total += (1 - pixel); -// } -// } -// } -// if (total == 0) { -// return Eigen::Vector2d(-1, -1); -// } -// return Eigen::Vector2d(x_res / total, y_res / total); -// } +TEST(camera_model, batch_model) { + static int constexpr kBatchSize = 4; + using Batch8PinholeModel = + CameraModelT, AffineTransform, ProjectionZ1>; + using Batch8KannalaBrandtModel = CameraModelT< + Batch, + KannalaBrandtK3Model, + ProjectionZ1>; + + // std::vector pixels_image = { + // {2, 2}, + // {1, 400}, + // {320, 240}, + // {319.5, 239.5}, + // {100, 40}, + // {639, 479}, + // {10, 40}, + // {100, 43}, + // }; + + std::vector pixels_image = { + {2, 2}, + {1, 400}, + {320, 240}, + {319.5, 239.5}, + {100, 40}, + {639, 479}, + {10, 40}, + {100, 43}, + }; + + BatchVector batch_pixel; + + for (int r = 0; r < 2; ++r) { + for (int i = 0; i < kBatchSize; ++i) { + Eigen::Vector2d pixel = FARM_AT(pixels_image, i); + batch_pixel[r].lanes[i] = pixel[r]; + } + } + + ImageSize image_size{640, 480}; + { + PinholeModel model = createDefaultPinholeModel(image_size); + typename Batch8PinholeModel::Params batch_params; + + for (int r = 0; r < Batch8PinholeModel::kNumParams; ++r) { + for (int i = 0; i < kBatchSize; ++i) { + batch_params[r].lanes[i] = model.params()[r]; + } + } + Batch8PinholeModel batch_model(image_size, batch_params); + + BatchVector batch_point = + batch_model.camUnproj(batch_pixel, Batch(1.0)); + + BatchVector reproj_batch_pixel = + batch_model.camProj(batch_point); + + FARM_ASSERT_WITHIN_ABS(batch_pixel, reproj_batch_pixel, 1e-2); + } + { + Eigen::VectorXd get_params(8); + get_params << 1000, 1000, 320, 280, 0.1, 0.01, 0.001, 0.0001; + // CameraModel model = CameraModel( + // {640, 480}, CameraDistortionType::kannala_brandt_k3, get_params); + KannalaBrandtK3Model model({640, 480}, get_params); + + typename Batch8KannalaBrandtModel::Params batch_params; + + for (int r = 0; r < Batch8KannalaBrandtModel::kNumParams; ++r) { + for (int i = 0; i < kBatchSize; ++i) { + batch_params[r].lanes[i] = model.params()[r]; + } + } + Batch8KannalaBrandtModel batch_model(image_size, batch_params); + + BatchVector batch_point = ProjectionZ1::unproj( + KannalaBrandtK3Transform::undistort(batch_params, batch_pixel), + Batch(1.0)); + + for (int i = 0; i < kBatchSize; ++i) { + Eigen::Vector point = ProjectionZ1::unproj( + KannalaBrandtK3Transform::undistort(model.params(), pixels_image[i]), + 1.0); + + FARM_ASSERT_WITHIN_ABS(point.x(), batch_point.x().lanes[i], 1e-2); + FARM_ASSERT_WITHIN_ABS(point.y(), batch_point.y().lanes[i], 1e-2); + FARM_ASSERT_WITHIN_ABS(point.z(), batch_point.z().lanes[i], 1e-2); + + // FARM_INFO("[{}] {}", i, point.transpose()); + } + // FARM_INFO("{}", batch_point); + + // BatchVector reproj_batch_pixel = + // KannalaBrandtK3Transform::distort( + // batch_params, ProjectionZ1::proj(batch_point)); + + // FARM_INFO("{}", batch_point); + + // FARM_ASSERT_WITHIN_ABS(batch_pixel, reproj_batch_pixel, 1e-2); + } +} TEST(camera_model, projection_round_trip) { std::vector camera_models; @@ -320,8 +399,8 @@ TEST(camera_model, projection_round_trip) { // std::vector img_pyr; // cv::buildPyramid(img, img_pyr, 2); -// SOPHUS_ASSERT_EQ(img_pyr[1].at(0, 0), 0.0); -// SOPHUS_ASSERT_EQ(img_pyr[1].at(0, 1), 0.0); +// SOPHUS_ASSERT_EQ(img_pyr[1].at(0,createDefaultPinholeModel 0), +// 0.0); SOPHUS_ASSERT_EQ(img_pyr[1].at(0, 1), 0.0); // SOPHUS_ASSERT_EQ(img_pyr[1].at(0, 2), 0.0625); // SOPHUS_ASSERT_EQ(img_pyr[1].at(0, 3), 0.375); // pixel 3 at level // 1 SOPHUS_ASSERT_EQ(img_pyr[1].at(0, 4), 0.0625);