Skip to content

Commit

Permalink
batch camera models
Browse files Browse the repository at this point in the history
  • Loading branch information
@strasdat
strasdat committed Apr 3, 2024
1 parent 49ae7b4 commit d7e85b1
Show file tree
Hide file tree
Showing 7 changed files with 191 additions and 100 deletions.
2 changes: 0 additions & 2 deletions cpp/farm_ng/core/logging/assert_within.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,8 +19,6 @@

#include <Eigen/Core>



namespace farm_ng {

/// Checks whether two floating point numbers x, y are close to each other.
Expand Down
2 changes: 1 addition & 1 deletion cpp/sophus2/common/common.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -72,7 +72,7 @@ template <>
inline float const kEpsilon<float> = float(1e-5);

static float const kEpsilonF32 = kEpsilon<float>;
static float const kEpsilonF64 = kEpsilon<double>;
static double const kEpsilonF64 = kEpsilon<double>;

/// Slightly larger than kEpsilon.
template <class TScalar>
Expand Down
3 changes: 0 additions & 3 deletions cpp/sophus2/concepts/params.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -115,7 +115,4 @@ struct ScalarBinaryOpTraits<
using ReturnType = sophus2::concepts::CompatScalarEx<TT>;
};




} // namespace Eigen
23 changes: 23 additions & 0 deletions cpp/sophus2/linalg/batch.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -55,6 +55,23 @@ inline Batch<T, N> abs(Batch<T, N> const& f) {
return Batch{f.lanes.abs().eval()};
}

template <class T, int N>
inline Batch<T, N> tan(Batch<T, N> const& f) {
return Batch{f.lanes.tan().eval()};
}

template <class T, int N>
inline Batch<T, N> atan2(Batch<T, N> const& lhs, Batch<T, N> const& rhs) {
Eigen::Array<T, N, 1> result;
using std::atan2;

for (int i = 0; i < N; ++i) {
result[i] = atan2(lhs.lanes[i], rhs.lanes[i]);
}

return Batch{result};
}

template <class T, int N>
inline Batch<T, N> sqrt(Batch<T, N> const& f) {
return Batch{f.lanes.sqrt().eval()};
Expand Down Expand Up @@ -113,6 +130,12 @@ struct BatchTrait<Batch<TScalar, kNum>> {
}
};

template <class TScalar, int kRows, int kBatchSize>
using BatchVector = Eigen::Vector<Batch<TScalar, kBatchSize>, kRows>;

template <class TScalar, int kRows, int kCols, int kBatchSize>
using BatchMatrix = Eigen::Matrix<Batch<TScalar, kBatchSize>, kRows, kCols>;

} // namespace sophus2

namespace Eigen {
Expand Down
138 changes: 80 additions & 58 deletions cpp/sophus2/sensor/camera_distortion/kannala_brandt.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 <Eigen/Dense>
Expand All @@ -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 <class TScalar>
Expand Down Expand Up @@ -52,38 +53,59 @@ 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;

if constexpr (BatchTrait<Scalar>::kIsBatch) {
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);
auto const scaling =
(radius_squared.lanes > Scalar(sophus2::kEpsilonF64).lanes)
.select((r_distorted * radius_inverse).lanes, Scalar(1.0).lanes);

return Scalar{scaling} *
PixelImage<Scalar>(
proj_point_in_camera_z1_plane[0] * params[0],
proj_point_in_camera_z1_plane[1] * params[1]) +
params.template segment<2>(2);
} else {
// linearize r around radius=0
Scalar scaling = Scalar(1.0);
if (radius_squared > sophus2::kEpsilonF64) {
Scalar const radius = sqrt(radius_squared);
Scalar const radius_inverse = Scalar(1.0) / radius;
Scalar const theta = atan2(radius, typename TPointTypeT::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);
scaling = r_distorted * radius_inverse;
}
return scaling * proj_point_in_camera_z1_plane.cwiseProduct(
params.template head<2>()) +
params.template segment<2>(2);
} // linearize r around radius=0

return AffineTransform::distort(

params.template head<4>(), proj_point_in_camera_z1_plane);
}
}

template <class TScalar>
Expand All @@ -95,56 +117,56 @@ 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<TScalar> * sophus2::kEpsilon<TScalar>) {
return Eigen::Matrix<TScalar, 2, 1>(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;

const TScalar rth = sqrt(rth2);
// if (rth2 < sophus2::kEpsilon<TScalar> * sophus2::kEpsilon<TScalar>) {
// return Eigen::Matrix<TScalar, 2, 1>(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<TScalar>::impl(step)) <
sophus2::kEpsilon<TScalar>) {
break;
}
// if (abs(jet_helpers::GetValue<TScalar>::impl(step)) <
// sophus2::kEpsilon<TScalar>) {
// break;
// }
}

TScalar radius_undistorted = tan(th);

if (radius_undistorted < TScalar(0.0)) {
return Eigen::Matrix<TScalar, 2, 1>(
-radius_undistorted * un / rth, -radius_undistorted * vn / rth);
}
// if (radius_undistorted < TScalar(0.0)) {
// return Eigen::Matrix<TScalar, 2, 1>(
// -radius_undistorted * un / rth, -radius_undistorted * vn / rth);
// }
return Eigen::Matrix<TScalar, 2, 1>(
radius_undistorted * un / rth, radius_undistorted * vn / rth);
}
Expand Down
21 changes: 11 additions & 10 deletions cpp/sophus2/sensor/camera_model.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -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.
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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.
Expand Down
Loading

0 comments on commit d7e85b1

Please sign in to comment.