Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

MAINT: simplify the implementation of the loss function used in the camera calibration #394

Merged
merged 3 commits into from
Jul 16, 2024
+59 −114
Merged

MAINT: simplify the implementation of the loss function used in the camera calibration #394

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
8c13209
MAINT: change group name.
oddkiva Jul 16, 2024
9b2a72c
MAINT: simplify code.
Jul 16, 2024
f04389a
MAINT: fix compile errors.
oddkiva Jul 16, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
40 changes: 18 additions & 22 deletions cpp/src/DO/Sara/MultiViewGeometry/Calibration/PinholeCameraReprojectionError.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -30,27 +30,23 @@ namespace DO::Sara {
static constexpr auto extrinsic_parameter_count = 6;

inline PinholeCameraReprojectionError(const Eigen::Vector2d& image_pt, //
const Eigen::Vector2d& scene_pt)
const Eigen::Vector3d& scene_pt)
: image_point{image_pt}
, scene_point{scene_pt}
{
}

template <typename T>
inline auto operator()(const T* const fx, const T* const fy_normalized,
const T* const shear_normalized, //
const T* const principal_point, //
const T* const extrinsics, T* residuals) const
inline auto operator()(const T* const intrinsics, const T* const extrinsics,
T* residuals) const
-> bool
{
using Vector3 = Eigen::Matrix<T, 3, 1>;
const Vector3 scene_coords = scene_point.template cast<T>();

// 1. Apply [R|t] = extrinsics[...]
//
// a) extrinsics[0, 1, 2] are the angle-axis rotation.
const auto scene_coords = Eigen::Matrix<T, 3, 1>{
static_cast<T>(scene_point.x()), //
static_cast<T>(scene_point.y()), //
T{} //
};
auto camera_coords = Eigen::Matrix<T, 3, 1>{};
ceres::AngleAxisRotatePoint(extrinsics, scene_coords.data(),
camera_coords.data());
Expand All @@ -64,13 +60,16 @@ namespace DO::Sara {
const auto yp = camera_coords[1] / camera_coords[2];

// 3. Apply the calibration matrix.
const auto& fy = (*fy_normalized) * (*fx);
const auto& s = (*shear_normalized) * (*fx);
const auto& u0 = principal_point[0];
const auto& v0 = principal_point[1];
const auto& fx = intrinsics[0];
const auto& fy_normalized = intrinsics[1];
const auto& fy = fy_normalized * fx;
const auto& shear_normalized = intrinsics[2];
const auto& s = shear_normalized * fx;
const auto& u0 = intrinsics[3];
const auto& v0 = intrinsics[4];
// clang-format off
const auto predicted_x = (*fx) * xp + s * yp + u0;
const auto predicted_y = fy * yp + v0;
const auto predicted_x = fx * xp + s * yp + u0;
const auto predicted_y = fy * yp + v0;
// clang-format on

// The error is the difference between the predicted and observed
Expand All @@ -82,21 +81,18 @@ 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<PinholeCameraReprojectionError, //
residual_dimension, //
1 /* fx */, //
1 /* fy */, //
1 /* shear */, //
2 /* (u0, v0) */,
intrinsic_parameter_count,
extrinsic_parameter_count>(
new PinholeCameraReprojectionError(image_point, scene_point) //
);
}

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

} // namespace DO::Sara
2 changes: 1 addition & 1 deletion cpp/tools/Calibration/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,5 +21,5 @@ foreach(file ${SOURCE_FILES})
set_target_properties(
${filename} PROPERTIES COMPILE_FLAGS ${SARA_DEFINITIONS}
RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/bin)
set_property(TARGET ${filename} PROPERTY FOLDER "DRAFTS/Calibration")
set_property(TARGET ${filename} PROPERTY FOLDER "Tools/Sara/Calibration")
endforeach()
131 changes: 40 additions & 91 deletions cpp/tools/Calibration/calibrate_pinhole_camera.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,8 @@ namespace sara = DO::Sara;
class ChessboardCalibrationData
{
public:
static constexpr auto intrinsic_parameter_count =
sara::PinholeCameraReprojectionError::intrinsic_parameter_count;
static constexpr auto extrinsic_parameter_count =
sara::PinholeCameraReprojectionError::extrinsic_parameter_count;

Expand Down Expand Up @@ -77,6 +79,7 @@ class ChessboardCalibrationData

_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 @@ -129,59 +132,28 @@ class ChessboardCalibrationData
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 image_point_pos = std::size_t{};
auto scene_point_pos = std::size_t{};
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);
_observations_2d.data() + image_point_pos);
const Eigen::Vector3d scene_point = Eigen::Map<const Eigen::Vector3d>(
_observations_3d.data() + scene_point_pos);

auto cost_function = sara::PinholeCameraReprojectionError::create(
image_point, scene_point);

problem.AddResidualBlock(cost_function, loss_fn, //
&_intrinsics.fx(), &_intrinsics.fy(), //
&_intrinsics.shear(), //
_intrinsics.principal_point().data(), //
problem.AddResidualBlock(cost_function, loss_fn, mutable_intrinsics(),
mutable_extrinsics(n));

data_pos += 2;
image_point_pos += 2;
scene_point_pos += 3;
}
}

// Bounds on fx.
problem.SetParameterLowerBound(&_intrinsics.fx(), 0, 500);
problem.SetParameterUpperBound(&_intrinsics.fx(), 0, 5000);

// Bounds on fy (NORMALIZED).
problem.SetParameterLowerBound(&_intrinsics.fy(), 0, 0.1);
problem.SetParameterUpperBound(&_intrinsics.fy(), 0, 2.0);

// Bounds on the shear (NORMALIZED).
// - We should not need any further adjustment on the bounds.
problem.SetParameterLowerBound(&_intrinsics.shear(), 0, -1.);
problem.SetParameterUpperBound(&_intrinsics.shear(), 0, 1.);

// So far no need for (u0, v0)
}

auto fix_fy_normalized(ceres::Problem& problem) -> void
{
problem.SetParameterBlockConstant(&_intrinsics.fy());
}

auto fix_shear_normalized(ceres::Problem& problem) -> void
{
problem.SetParameterBlockConstant(&_intrinsics.shear());
}

auto fix_principal_point(ceres::Problem& problem) -> void
{
problem.SetParameterBlockConstant(_intrinsics.principal_point().data());
}

auto copy_camera_intrinsics(sara::v2::PinholeCamera<double>& camera) -> void
Expand All @@ -202,59 +174,12 @@ class ChessboardCalibrationData
camera.v0() = v0;
}

auto reinitialize_extrinsic_parameters() -> void
auto camera_intrinsics() -> sara::v2::PinholeCamera<double>&
{
throw std::runtime_error{"Implementation incomplete!"};

const auto camera = sara::v2::PinholeCamera<double>{};

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;
}
}

// TODO: reestimate the extrinsics with the PnP algorithm.
auto extrinsics = mutable_extrinsics(image_index);
auto angle_axis_ptr = extrinsics;
auto translation_ptr = extrinsics + 3;

for (auto k = 0; k < 3; ++k)
angle_axis_ptr[k] = 0;

for (auto k = 0; k < 3; ++k)
translation_ptr[k] = 0;
}
return _intrinsics;
}

private:
int _w = 0;
int _h = 0;
int _num_images = 0;

std::vector<int> _num_points_at_image;
Expand Down Expand Up @@ -371,9 +296,33 @@ auto sara_graphics_main(int argc, char** argv) -> int
SARA_DEBUG << "Instantiating Ceres Problem..." << std::endl;
auto problem = ceres::Problem{};
calibration_data.transform_into_ceres_problem(problem);
calibration_data.fix_fy_normalized(problem);
calibration_data.fix_shear_normalized(problem);
calibration_data.fix_principal_point(problem);

// By default freeze the principal point.
auto intrinsics_to_freeze = std::vector<int>{};

// Fix the normalized focal length fy as 1.
calibration_data.camera_intrinsics().fy() = 1.;
intrinsics_to_freeze.emplace_back(1);

// Fix normalized shear value as 0.
calibration_data.camera_intrinsics().shear() = 0.;
intrinsics_to_freeze.emplace_back(2);

// Fix the fixed principal point.
intrinsics_to_freeze.emplace_back(3);
intrinsics_to_freeze.emplace_back(4);

#if CERES_VERSION_MAJOR == 2 && CERES_VERSION_MINOR >= 2
auto intrinsic_manifold = new ceres::SubsetManifold{
calibration_data.intrinsic_parameter_count, intrinsics_to_freeze};
problem.SetManifold(calibration_data.mutable_intrinsics(),
intrinsic_manifold);
#else
auto intrinsic_manifold = new ceres::SubsetParameterization{
calibration_data.intrinsic_parameter_count, intrinsics_to_freeze};
problem.SetParameterization(calibration_data.mutable_intrinsics(),
intrinsic_manifold);
#endif

// Restarting the optimization solver is better than increasing the number of
// iterations.
Expand Down