26 #include "ceres/ceres.h"
27 #include "ceres/rotation.h"
44 bool NeedUseInvertIntrinsicsPipeline(
const CameraIntrinsics* intrinsics) {
46 intrinsics->GetDistortionModelType();
63 void ApplyDistortionModelUsingIntrinsicsBlock(
64 const CameraIntrinsics* invariant_intrinsics,
65 const T*
const intrinsics_block,
66 const T& normalized_x,
67 const T& normalized_y,
71 const T& focal_length =
73 const T& principal_point_x =
75 const T& principal_point_y =
80 switch (invariant_intrinsics->GetDistortionModelType()) {
122 LOG(FATAL) <<
"Unsupported distortion model.";
152 LOG(FATAL) <<
"Unknown distortion model.";
167 template <
typename T>
168 void InvertDistortionModelUsingIntrinsicsBlock(
169 const CameraIntrinsics* invariant_intrinsics,
170 const T*
const intrinsics_block,
176 const T& focal_length =
178 const T& principal_point_x =
180 const T& principal_point_y =
185 switch (invariant_intrinsics->GetDistortionModelType()) {
189 LOG(FATAL) <<
"Unsupported distortion model.";
200 invariant_intrinsics->image_width(),
201 invariant_intrinsics->image_height(),
212 LOG(FATAL) <<
"Unknown distortion model.";
215 template <
typename T>
216 void NormalizedToImageSpace(
const T*
const intrinsics_block,
217 const T& normalized_x,
218 const T& normalized_y,
222 const T& focal_length =
224 const T& principal_point_x =
226 const T& principal_point_y =
229 *image_x = normalized_x * focal_length + principal_point_x;
230 *image_y = normalized_y * focal_length + principal_point_y;
239 struct ReprojectionErrorApplyIntrinsics {
240 ReprojectionErrorApplyIntrinsics(
const CameraIntrinsics* invariant_intrinsics,
241 const double observed_distorted_x,
242 const double observed_distorted_y,
249 template <
typename T>
254 T* residuals)
const {
258 ceres::AngleAxisRotatePoint(R_t, X,
x);
272 T predicted_distorted_x, predicted_distorted_y;
277 &predicted_distorted_x,
278 &predicted_distorted_y);
298 struct ReprojectionErrorInvertIntrinsics {
299 ReprojectionErrorInvertIntrinsics(
300 const CameraIntrinsics* invariant_intrinsics,
301 const double observed_distorted_x,
302 const double observed_distorted_y,
309 template <
typename T>
314 T* residuals)
const {
317 const T& principal_point_x =
319 const T& principal_point_y =
325 ceres::AngleAxisRotatePoint(R_t, X,
x);
340 T predicted_x = focal_length * xn + principal_point_x;
341 T predicted_y = focal_length * yn + principal_point_y;
343 T observed_undistorted_normalized_x, observed_undistorted_normalized_y;
344 InvertDistortionModelUsingIntrinsicsBlock(
349 &observed_undistorted_normalized_x,
350 &observed_undistorted_normalized_y);
352 T observed_undistorted_image_x, observed_undistorted_image_y;
353 NormalizedToImageSpace(intrinsics,
354 observed_undistorted_normalized_x,
355 observed_undistorted_normalized_y,
356 &observed_undistorted_image_x,
357 &observed_undistorted_image_y);
360 residuals[0] = (predicted_x - observed_undistorted_image_x) *
weight_;
361 residuals[1] = (predicted_y - observed_undistorted_image_y) *
weight_;
373 void BundleIntrinsicsLogMessage(
const int bundle_intrinsics) {
375 LG <<
"Bundling only camera positions.";
377 std::string bundling_message =
"";
379 #define APPEND_BUNDLING_INTRINSICS(name, flag) \
380 if (bundle_intrinsics & flag) { \
381 if (!bundling_message.empty()) { \
382 bundling_message += ", "; \
384 bundling_message += name; \
397 LG <<
"Bundling " << bundling_message <<
".";
406 map<int, Vec6> PackCamerasRotationAndTranslation(
408 map<int, Vec6> all_cameras_R_t;
411 for (
const EuclideanCamera&
camera : all_cameras) {
413 ceres::RotationMatrixToAngleAxis(&
camera.R(0, 0), &camera_R_t(0));
414 camera_R_t.tail<3>() =
camera.t;
415 all_cameras_R_t.insert(make_pair(
camera.image, camera_R_t));
418 return all_cameras_R_t;
422 void UnpackCamerasRotationAndTranslation(
423 const map<int, Vec6>& all_cameras_R_t,
425 for (map<int, Vec6>::value_type image_and_camera_R_T : all_cameras_R_t) {
426 const int image = image_and_camera_R_T.first;
427 const Vec6& camera_R_t = image_and_camera_R_T.second;
434 ceres::AngleAxisToRotationMatrix(&camera_R_t(0), &
camera->R(0, 0));
435 camera->t = camera_R_t.tail<3>();
444 void CRSMatrixToEigenMatrix(
const ceres::CRSMatrix& crs_matrix,
446 eigen_matrix->resize(crs_matrix.num_rows, crs_matrix.num_cols);
447 eigen_matrix->setZero();
449 for (
int row = 0; row < crs_matrix.num_rows; ++row) {
450 int start = crs_matrix.rows[row];
451 int end = crs_matrix.rows[row + 1] - 1;
453 for (
int i = start; i <= end; i++) {
454 int col = crs_matrix.cols[i];
455 double value = crs_matrix.values[i];
457 (*eigen_matrix)(row,
col) = value;
462 void EuclideanBundlerPerformEvaluation(
const Tracks&
tracks,
464 map<int, Vec6>* all_cameras_R_t,
465 ceres::Problem* problem,
466 BundleEvaluation* evaluation) {
467 int max_track =
tracks.MaxTrack();
469 int num_cameras = all_cameras_R_t->size();
473 for (
int i = 0; i <= max_track; i++) {
484 for (
int j = 0; j < markera_of_track.size(); j++) {
485 if (markera_of_track.at(j).weight != 0.0) {
486 minimized_points.push_back(
point);
494 LG <<
"Number of cameras " << num_cameras;
495 LG <<
"Number of points " << num_points;
497 evaluation->num_cameras = num_cameras;
498 evaluation->num_points = num_points;
500 if (evaluation->evaluate_jacobian) {
501 ceres::CRSMatrix evaluated_jacobian;
502 ceres::Problem::EvaluateOptions eval_options;
505 int max_image =
tracks.MaxImage();
506 for (
int i = 0; i <= max_image; i++) {
509 double* current_camera_R_t = &(*all_cameras_R_t)[i](0);
512 problem->SetParameterBlockVariable(current_camera_R_t);
514 eval_options.parameter_blocks.push_back(current_camera_R_t);
519 for (
int i = 0; i < minimized_points.size(); i++) {
520 EuclideanPoint*
point = minimized_points.at(i);
521 eval_options.parameter_blocks.push_back(&
point->X(0));
524 problem->Evaluate(eval_options,
NULL,
NULL,
NULL, &evaluated_jacobian);
526 CRSMatrixToEigenMatrix(evaluated_jacobian, &evaluation->jacobian);
530 template <
typename CostFunction>
531 void AddResidualBlockToProblemImpl(
const CameraIntrinsics* invariant_intrinsics,
535 double* intrinsics_block,
537 EuclideanPoint*
point,
538 ceres::Problem* problem) {
539 problem->AddResidualBlock(
540 new ceres::AutoDiffCostFunction<CostFunction,
545 invariant_intrinsics, observed_x, observed_y, weight)),
552 void AddResidualBlockToProblem(
const CameraIntrinsics* invariant_intrinsics,
553 const Marker& marker,
554 double marker_weight,
555 double* intrinsics_block,
557 EuclideanPoint*
point,
558 ceres::Problem* problem) {
559 if (NeedUseInvertIntrinsicsPipeline(invariant_intrinsics)) {
560 AddResidualBlockToProblemImpl<ReprojectionErrorInvertIntrinsics>(
561 invariant_intrinsics,
570 AddResidualBlockToProblemImpl<ReprojectionErrorApplyIntrinsics>(
571 invariant_intrinsics,
591 void EuclideanBundlePointsOnly(
const CameraIntrinsics* invariant_intrinsics,
593 map<int, Vec6>& all_cameras_R_t,
594 double* intrinsics_block,
596 ceres::Problem::Options problem_options;
597 ceres::Problem problem(problem_options);
598 int num_residuals = 0;
599 for (
int i = 0; i <
markers.size(); ++i) {
600 const Marker& marker =
markers[i];
609 double* current_camera_R_t = &all_cameras_R_t[
camera->image](0);
611 AddResidualBlockToProblem(invariant_intrinsics,
619 problem.SetParameterBlockConstant(current_camera_R_t);
623 LG <<
"Number of residuals: " << num_residuals;
624 if (!num_residuals) {
625 LG <<
"Skipping running minimizer with zero residuals";
629 problem.SetParameterBlockConstant(intrinsics_block);
632 ceres::Solver::Options
options;
633 options.use_nonmonotonic_steps =
true;
634 options.preconditioner_type = ceres::SCHUR_JACOBI;
635 options.linear_solver_type = ceres::ITERATIVE_SCHUR;
636 options.use_explicit_schur_complement =
true;
637 options.use_inner_iterations =
true;
638 options.max_num_iterations = 100;
639 options.num_threads = std::thread::hardware_concurrency();
642 ceres::Solver::Summary summary;
643 ceres::Solve(
options, &problem, &summary);
645 LG <<
"Final report:\n" << summary.FullReport();
662 const int bundle_intrinsics,
663 const int bundle_constraints,
667 LG <<
"Original intrinsics: " << *intrinsics;
676 intrinsics->
Pack(&packed_intrinsics);
684 map<int, Vec6> all_cameras_R_t =
688 ceres::SubsetManifold* constant_translation_manifold =
NULL;
690 std::vector<int> constant_translation;
693 constant_translation.push_back(3);
694 constant_translation.push_back(4);
695 constant_translation.push_back(5);
697 constant_translation_manifold =
698 new ceres::SubsetManifold(6, constant_translation);
702 ceres::Problem::Options problem_options;
703 ceres::Problem problem(problem_options);
704 int num_residuals = 0;
705 bool have_locked_camera =
false;
706 for (
int i = 0; i <
markers.size(); ++i) {
716 double* current_camera_R_t = &all_cameras_R_t[
camera->image](0);
721 if (marker.
weight != 0.0) {
722 AddResidualBlockToProblem(intrinsics,
732 if (!have_locked_camera) {
733 problem.SetParameterBlockConstant(current_camera_R_t);
734 have_locked_camera =
true;
738 problem.SetManifold(current_camera_R_t, constant_translation_manifold);
741 zero_weight_tracks_flags[marker.
track] =
false;
745 LG <<
"Number of residuals: " << num_residuals;
747 if (!num_residuals) {
748 LG <<
"Skipping running minimizer with zero residuals";
754 LOG(FATAL) <<
"Division model doesn't support bundling "
755 "of tangential distortion";
758 BundleIntrinsicsLogMessage(bundle_intrinsics);
763 problem.SetParameterBlockConstant(intrinsics_block);
768 std::vector<int> constant_intrinsics;
769 #define MAYBE_SET_CONSTANT(bundle_enum, offset) \
770 if (!(bundle_intrinsics & bundle_enum) || \
771 !packed_intrinsics.IsParameterDefined(offset)) { \
772 constant_intrinsics.push_back(offset); \
786 #undef MAYBE_SET_CONSTANT
788 if (!constant_intrinsics.empty()) {
789 ceres::SubsetManifold* subset_parameterization =
791 constant_intrinsics);
793 problem.SetManifold(intrinsics_block, subset_parameterization);
798 ceres::Solver::Options
options;
799 options.use_nonmonotonic_steps =
true;
800 options.preconditioner_type = ceres::SCHUR_JACOBI;
801 options.linear_solver_type = ceres::ITERATIVE_SCHUR;
802 options.use_explicit_schur_complement =
true;
803 options.use_inner_iterations =
true;
804 options.max_num_iterations = 100;
805 options.num_threads = std::thread::hardware_concurrency();
808 ceres::Solver::Summary summary;
809 ceres::Solve(
options, &problem, &summary);
811 LG <<
"Final report:\n" << summary.FullReport();
814 UnpackCamerasRotationAndTranslation(all_cameras_R_t,
reconstruction);
818 intrinsics->
Unpack(packed_intrinsics);
821 LG <<
"Final intrinsics: " << *intrinsics;
824 EuclideanBundlerPerformEvaluation(
831 for (
int track = 0; track <
tracks.MaxTrack(); ++track) {
832 if (zero_weight_tracks_flags[track]) {
834 zero_weight_markers.reserve(zero_weight_markers.size() +
835 current_markers.size());
836 for (
int i = 0; i < current_markers.size(); ++i) {
837 zero_weight_markers.push_back(current_markers[i]);
842 if (zero_weight_markers.size()) {
843 LG <<
"Refining position of constant zero-weighted tracks";
844 EuclideanBundlePointsOnly(intrinsics,
in reality light always falls off quadratically Particle Retrieve the data of the particle that spawned the object for example to give variation to multiple instances of an object Point Retrieve information about points in a point cloud Retrieve the edges of an object as it appears to Cycles topology will always appear triangulated Convert a blackbody temperature to an RGB value Normal Generate a perturbed normal from an RGB normal map image Typically used for faking highly detailed surfaces Generate an OSL shader from a file or text data block Image Sample an image file as a texture Sky Generate a procedural sky texture Noise Generate fractal Perlin noise Wave Generate procedural bands or rings with noise Voronoi Generate Worley noise based on the distance to random points Typically used to generate textures such as or biological cells Brick Generate a procedural texture producing bricks Texture Retrieve multiple types of texture coordinates nTypically used as inputs for texture nodes Vector Convert a point
in reality light always falls off quadratically Particle Retrieve the data of the particle that spawned the object for example to give variation to multiple instances of an object Point Retrieve information about points in a point cloud Retrieve the edges of an object as it appears to Cycles topology will always appear triangulated Convert a blackbody temperature to an RGB value Normal Generate a perturbed normal from an RGB normal map image Typically used for faking highly detailed surfaces Generate an OSL shader from a file or text data block Image Sample an image file as a texture Sky Generate a procedural sky texture Noise Generate fractal Perlin noise Wave Generate procedural bands or rings with noise Voronoi Generate Worley noise based on the distance to random points Typically used to generate textures such as or biological cells Brick Generate a procedural texture producing bricks Texture Retrieve multiple types of texture coordinates nTypically used as inputs for texture nodes Vector Convert a or normal between camera
#define MAYBE_SET_CONSTANT(bundle_enum, offset)
const double observed_distorted_x_
const CameraIntrinsics * invariant_intrinsics_
const double observed_distorted_y_
#define APPEND_BUNDLING_INTRINSICS(name, flag)
virtual void Unpack(const PackedIntrinsics &packed_intrinsics)
virtual void Pack(PackedIntrinsics *packed_intrinsics) const
virtual DistortionModelType GetDistortionModelType() const =0
double * GetParametersBlock()
@ OFFSET_PRINCIPAL_POINT_Y
@ OFFSET_PRINCIPAL_POINT_X
vector< ProjectiveCamera > AllCameras() const
Returns all cameras.
ProjectivePoint * PointForTrack(int track)
Returns a pointer to the point corresponding to track.
ProjectiveCamera * CameraForImage(int image)
Returns a pointer to the camera corresponding to image.
CCL_NAMESPACE_BEGIN struct Options options
depth_tx normal_tx diffuse_light_tx specular_light_tx volume_light_tx environment_tx ambient_occlusion_tx aov_value_tx in_weight_img image(1, GPU_R32F, Qualifier::WRITE, ImageType::FLOAT_2D_ARRAY, "out_weight_img") .image(3
const vector< Marker > & markers
const ProjectiveReconstruction & reconstruction
Eigen::Matrix< double, 6, 1 > Vec6
void ProjectiveBundle(const Tracks &, ProjectiveReconstruction *)
void InvertNukeDistortionModel(const T &focal_length_x, const T &focal_length_y, const T &principal_point_x, const T &principal_point_y, const int image_width, const int image_height, const T &k1, const T &k2, const T &image_x, const T &image_y, T *normalized_x, T *normalized_y)
void ApplyPolynomialDistortionModel(const T &focal_length_x, const T &focal_length_y, const T &principal_point_x, const T &principal_point_y, const T &k1, const T &k2, const T &k3, const T &p1, const T &p2, const T &normalized_x, const T &normalized_y, T *image_x, T *image_y)
void ApplyDivisionDistortionModel(const T &focal_length_x, const T &focal_length_y, const T &principal_point_x, const T &principal_point_y, const T &k1, const T &k2, const T &normalized_x, const T &normalized_y, T *image_x, T *image_y)
void EuclideanBundle(const Tracks &tracks, EuclideanReconstruction *reconstruction)
void EuclideanBundleCommonIntrinsics(const Tracks &tracks, const int bundle_intrinsics, const int bundle_constraints, EuclideanReconstruction *reconstruction, CameraIntrinsics *intrinsics, BundleEvaluation *evaluation)
@ DISTORTION_MODEL_POLYNOMIAL
@ DISTORTION_MODEL_DIVISION
void ApplyBrownDistortionModel(const T &focal_length_x, const T &focal_length_y, const T &principal_point_x, const T &principal_point_y, const T &k1, const T &k2, const T &k3, const T &k4, const T &p1, const T &p2, const T &normalized_x, const T &normalized_y, T *image_x, T *image_y)