Skip to content

Commit

Permalink
WIP: save work but buggy...
Browse files Browse the repository at this point in the history
  • Loading branch information
Odd Kiva committed Jun 8, 2024
1 parent a425ec6 commit b6d8a10
Show file tree
Hide file tree
Showing 2 changed files with 71 additions and 109 deletions.
132 changes: 43 additions & 89 deletions cpp/drafts/Calibration/tool/calibrate_omnidirectional_cameras.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
Expand Down Expand Up @@ -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<const Eigen::Vector2d>(
_observations_2d.data() + data_pos);
const Eigen::Vector2d scene_point = Eigen::Map<const Eigen::Vector2d>(
_observations_3d.data() + data_pos);
const Eigen::Vector2d image_point =
Eigen::Map<const Eigen::Vector2d>(obs_ptr);
const Eigen::Vector3d scene_point =
Eigen::Map<const Eigen::Vector3d>(scene_ptr);

auto cost_function =
sara::OmnidirectionalCameraReprojectionError::create(image_point,
Expand All @@ -171,21 +173,23 @@ class ChessboardCalibrationProblem
problem.AddResidualBlock(cost_function, loss_fn, //
mutable_intrinsics(), mutable_extrinsics(n));

data_pos += 2;
obs_ptr += 2;
scene_ptr += 3;
}
}

static constexpr auto fx = 0;
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);
Expand All @@ -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})
Expand All @@ -222,7 +226,8 @@ class ChessboardCalibrationProblem
// parameter to 1.
//
// By default freeze the principal point.
auto intrinsics_to_freeze = std::vector<int>{};
auto intrinsics_to_freeze =
std::vector<int>{fy_normalized, alpha, u0, v0, k2};
switch (_camera_type)
{
case CameraType::Fisheye:
Expand All @@ -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);
}

Expand All @@ -250,89 +255,38 @@ 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,
0, fy, v0,
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<double> { 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<const Eigen::Vector2d>(
_observations_2d.data() + data_pos);

const Eigen::Vector3d p3 = Eigen::Map<const Eigen::Vector2d>(
_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<int> _num_points_at_image;
Expand Down
48 changes: 28 additions & 20 deletions cpp/src/DO/Sara/MultiViewGeometry/Calibration/OmnidirectionalCameraReprojectionError.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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}
{
Expand Down Expand Up @@ -59,15 +59,16 @@ namespace DO::Sara {

template <typename T>
inline auto apply_distortion(const Eigen::Matrix<T, 2, 1>& 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<T, 2, 1>
{
using Vector2 = Eigen::Matrix<T, 2, 1>;

// 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.;
Expand All @@ -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<T>(scene_point.x()), //
static_cast<T>(scene_point.y()), //
T{} //
};
const Vector3 scene_coords = scene_point.template cast<T>();
auto camera_coords = Vector3{};
ceres::AngleAxisRotatePoint(extrinsics, scene_coords.data(),
camera_coords.data());
Expand All @@ -103,17 +100,28 @@ namespace DO::Sara {
const auto t = Eigen::Map<const Vector3>{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;
Expand All @@ -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, //
Expand All @@ -145,7 +153,7 @@ namespace DO::Sara {
}

Eigen::Vector2d image_point;
Eigen::Vector2d scene_point;
Eigen::Vector3d scene_point;
};

} // namespace DO::Sara

0 comments on commit b6d8a10

Please sign in to comment.