From 0772e16301b50adb2264931413a6020f551c51ca Mon Sep 17 00:00:00 2001 From: Hauke Strasdat Date: Fri, 5 Apr 2024 17:00:59 -0700 Subject: [PATCH] batch: camera models --- cpp/farm_ng/core/logging/assert_within.h | 2 - cpp/sophus2/common/common.h | 2 +- cpp/sophus2/concepts/params.h | 3 - cpp/sophus2/lie/impl/spiral_similarity2.h | 2 + cpp/sophus2/linalg/CMakeLists.txt | 2 +- cpp/sophus2/linalg/batch.h | 97 +++++++- .../sensor/camera_distortion/kannala_brandt.h | 211 +++++++++++------- cpp/sophus2/sensor/camera_model.h | 21 +- cpp/sophus2/sensor/camera_model_test.cpp | 114 ++++++++-- 9 files changed, 325 insertions(+), 129 deletions(-) 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/CMakeLists.txt b/cpp/sophus2/linalg/CMakeLists.txt index 12af66e2..3dd06360 100644 --- a/cpp/sophus2/linalg/CMakeLists.txt +++ b/cpp/sophus2/linalg/CMakeLists.txt @@ -28,7 +28,7 @@ farm_ng_add_library(sophus2_linalg ${sophus2_linalg_h} ) -target_link_libraries(sophus2_linalg INTERFACE sophus2_common sophus_concept) +target_link_libraries(sophus2_linalg INTERFACE sophus2_common sophus_concept farm_ng_core_pipeline) foreach(test_basename ${sophus2_linalg_src_prefixes}) farm_ng_add_test(${test_basename} diff --git a/cpp/sophus2/linalg/batch.h b/cpp/sophus2/linalg/batch.h index 63cee295..0e917433 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 @@ -31,17 +33,58 @@ struct Batch { Batch operator-() const { return Batch(-this->lanes); } + // This is required and we cannot change the defintion, for Batch to act as a + // scalar. bool operator==(Batch const& y) { return (this->lanes == y.lanes).all(); } + Batch cwLessEqual(Batch const& rhs) const { + return Batch((this->lanes <= rhs.lanes).eval()); + } + + Batch cwLess(Batch const& rhs) const { + return Batch((this->lanes < rhs.lanes).eval()); + } + + Batch cwGreaterEqual(Batch const& rhs) const { + return Batch((this->lanes >= rhs.lanes).eval()); + } + + Batch cwGreater(Batch const& rhs) const { + return Batch((this->lanes > rhs.lanes).eval()); + } + + template + Batch select( + Batch const& lhs, + Batch const& rhs) const { + return Batch(this->lanes.select(lhs.lanes, rhs.lanes)); + } + + template + Eigen::Matrix, kRows, kCols> select( + Eigen::Matrix, kRows, kCols> const& lhs, + Eigen::Matrix, kRows, kCols> const& rhs) const { + static_assert(kRows >= 0); + static_assert(kCols >= 0); + + Eigen::Matrix, kRows, kCols> result; + for (int r = 0; r < kRows; ++r) { + for (int c = 0; c < kCols; ++c) { + result(r, c) = this->select(lhs(r, c), rhs(r, c)); + } + } + return result; + } + Eigen::Array lanes; }; template auto operator<<(std::ostream& os, Batch const& batch) -> std::ostream& { - os << "[" << batch.lanes << "]"; + os << "[" << batch.lanes.transpose() << "]"; return os; } @@ -55,6 +98,37 @@ 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 atan(Batch const& f) { + return Batch{f.lanes.atan().eval()}; +} + +template +inline Batch pow(Batch const& f, T v) { + return Batch{f.lanes.pow(v).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()}; @@ -80,13 +154,13 @@ inline Batch operator/(Batch const& f, Batch const& g) { } template -inline Batch operator*(Batch const& f, TT s) { - return Batch{(f.lanes * s).eval()}; +inline Batch operator/(Batch const& f, TT s) { + return Batch{(f.lanes / s).eval()}; } template inline Batch operator*(TT s, Batch const& f) { - return Batch{(s * f.lanes).eval()}; + return Batch{(s / f.lanes).eval()}; } template @@ -113,10 +187,15 @@ struct BatchTrait> { } }; +template +using BatchVector = Eigen::Vector, kRows>; + +template +using BatchMatrix = Eigen::Matrix, kRows, kCols>; + } // namespace sophus2 namespace Eigen { - // This is mirrored from ceres::Jet. template struct NumTraits> { @@ -163,3 +242,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 diff --git a/cpp/sophus2/sensor/camera_distortion/kannala_brandt.h b/cpp/sophus2/sensor/camera_distortion/kannala_brandt.h index 50feb1cc..be9043cc 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,54 @@ 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) { + Scalar const scaling = + radius_squared.cwGreaterEqual(Scalar(sophus2::kEpsilonF64)) + .select(scaling_fn(), Scalar(1.0)); + + return 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,58 +112,77 @@ 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]; + + 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; + + Eigen::Matrix orig(un, vn); + if constexpr (BatchTrait::kIsBatch) { + // for batch types check this in the very end only + } else if (rth2 < sophus2::kEpsilon * sophus2::kEpsilon) { + // early exit for non-batch type + return orig; } - const TScalar rth = sqrt(rth2); + 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; - // has converged? - if (abs(jet_helpers::GetValue::impl(step)) < - sophus2::kEpsilon) { + TScalar const step = (thd - rth) / d_thd_wtr_th; + th = th - step; + + if (BatchTrait::allLessEqual( + abs(jet_helpers::GetValue::impl(step)), + sophus2::kEpsilon)) { + // TODO: also exit early if rth2_close_to_zero is true break; } } TScalar radius_undistorted = tan(th); - if (radius_undistorted < TScalar(0.0)) { + if constexpr (BatchTrait::kIsBatch) { + Batch::kBatchSize> rth2_close_to_zero = + rth2.cwLessEqual( + sophus2::kEpsilon * sophus2::kEpsilon); + + TScalar sign = radius_undistorted.cwLessEqual(TScalar(0.0)) + .select(TScalar(-1.0), TScalar(1.0)); + + return rth2_close_to_zero.select( + orig, + Eigen::Matrix( + sign * radius_undistorted * un / rth, + sign * radius_undistorted * vn / rth)); + } else { + TScalar sign = radius_undistorted < TScalar(0.0) ? -1.0 : 1.0; return Eigen::Matrix( - -radius_undistorted * un / rth, -radius_undistorted * vn / rth); + sign * radius_undistorted * un / rth, + sign * radius_undistorted * vn / rth); } - return Eigen::Matrix( - radius_undistorted * un / rth, radius_undistorted * vn / rth); } template @@ -178,53 +214,62 @@ class KannalaBrandtK3Transform { using std::atan2; using std::sqrt; - Eigen::Matrix dx; - - if (radius_squared < sophus2::kEpsilonSqrtF64) { - // clang-format off - dx << // - fx, 0, - 0, fy; - // clang-format on - return dx; + Eigen::Matrix dx0; + dx0(0, 0) = fx; + dx0(0, 1) = Scalar(0); + dx0(1, 0) = Scalar(0); + dx0(1, 1) = fy; + + if constexpr (BatchTrait::kIsBatch) { + // for batch types check this in the very end only + } else if (radius_squared < sophus2::kEpsilonSqrtF64) { + // early exit for non-batch type + return dx0; } + using std::atan; using std::atan2; using std::pow; using std::sqrt; - Scalar const c0 = pow(a, 2); - Scalar const c1 = pow(b, 2); + Scalar const c0 = a * a; + Scalar const c1 = b * b; Scalar const c2 = c0 + c1; Scalar const c3 = pow(c2, 5.0 / 2.0); - Scalar const c4 = c2 + 1; + Scalar const c4 = c2 + Scalar(1); Scalar const c5 = atan(sqrt(c2)); - Scalar const c6 = pow(c5, 2); + Scalar const c6 = c5 * c5; // c5 ^ 2 Scalar const c7 = c6 * k[0]; - Scalar const c8 = pow(c5, 4); + Scalar const c8 = c6 * c6; // c5 ^ 4 Scalar const c9 = c8 * k[1]; - Scalar const c10 = pow(c5, 6); + Scalar const c10 = c6 * c8; // c5 ^ 6 Scalar const c11 = c10 * k[2]; - Scalar const c12 = pow(c5, 8) * k[3]; - Scalar const c13 = 1.0 * c4 * c5 * (c11 + c12 + c7 + c9 + 1.0); + Scalar const c12 = c8 * c8 * k[3]; // c5 ^ 6 * k[3] + Scalar const c13 = 1.0 * c4 * c5 * (c11 + c12 + c7 + c9 + Scalar(1.0)); Scalar const c14 = c13 * c3; Scalar const c15 = pow(c2, 3.0 / 2.0); Scalar const c16 = c13 * c15; - Scalar const c17 = - 1.0 * c11 + 1.0 * c12 + - 2.0 * c6 * (4 * c10 * k[3] + 2 * c6 * k[1] + 3 * c8 * k[2] + k[0]) + - 1.0 * c7 + 1.0 * c9 + 1.0; - Scalar const c18 = c17 * pow(c2, 2); - Scalar const c19 = 1.0 / c4; - Scalar const c20 = c19 / pow(c2, 3); + Scalar const c17 = Scalar(1.0) * c11 + Scalar(1.0) * c12 + + Scalar(2.0) * c6 * + (Scalar(4.0) * c10 * k[3] + Scalar(2.0) * c6 * k[1] + + Scalar(3.0) * c8 * k[2] + k[0]) + + Scalar(1.0) * c7 + Scalar(1.0) * c9 + Scalar(1.0); + Scalar const c18 = c17 * c2 * c2; + Scalar const c19 = Scalar(1.0) / c4; + Scalar const c20 = c19 / (c2 * c2 * c2); Scalar const c21 = a * b * c19 * (-c13 * c2 + c15 * c17) / c3; - + Eigen::Matrix dx; dx(0, 0) = c20 * fx * (-c0 * c16 + c0 * c18 + c14); dx(0, 1) = c21 * fx; dx(1, 0) = c21 * fy; dx(1, 1) = c20 * fy * (-c1 * c16 + c1 * c18 + c14); + if constexpr (BatchTrait::kIsBatch) { + return (radius_squared.cwLessEqual(sophus2::kEpsilon)) + .select(dx0, dx); + } + return dx; } }; 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..f824e61d 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,95 @@ 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 = 8; + using Batch8PinholeModel = + CameraModelT, AffineTransform, ProjectionZ1>; + using Batch8KannalaBrandtModel = CameraModelT< + Batch, + KannalaBrandtK3Transform, + ProjectionZ1>; + + 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; + 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 = + batch_model.camUnproj(batch_pixel, Batch(1.0)); + BatchVector reproj_batch_pixel = + batch_model.camProj(batch_point); + + auto dx = KannalaBrandtK3Transform::dxDistort( + batch_params, ProjectionZ1::proj(batch_point)); + + for (int i = 0; i < kBatchSize; ++i) { + Eigen::Vector point = model.camUnproj(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); + + Eigen::Vector reproj_pixel = model.camProj(point); + + FARM_ASSERT_WITHIN_ABS( + reproj_pixel.x(), reproj_batch_pixel.x().lanes[i], 1e-2); + FARM_ASSERT_WITHIN_ABS( + reproj_pixel.y(), reproj_batch_pixel.y().lanes[i], 1e-2); + } + + FARM_ASSERT_WITHIN_ABS(batch_pixel, reproj_batch_pixel, 1e-2); + } +} TEST(camera_model, projection_round_trip) { std::vector camera_models;