26#include "ceres/ceres.h"
27#include "ceres/rotation.h"
63void ApplyDistortionModelUsingIntrinsicsBlock(
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 =
122 LOG(FATAL) <<
"Unsupported distortion model.";
152 LOG(FATAL) <<
"Unknown distortion model.";
168void InvertDistortionModelUsingIntrinsicsBlock(
170 const T*
const intrinsics_block,
176 const T& focal_length =
178 const T& principal_point_x =
180 const T& principal_point_y =
189 LOG(FATAL) <<
"Unsupported distortion model.";
216 LOG(FATAL) <<
"Unknown distortion model.";
220void NormalizedToImageSpace(
const T*
const intrinsics_block,
221 const T& normalized_x,
222 const T& normalized_y,
226 const T& focal_length =
228 const T& principal_point_x =
230 const T& principal_point_y =
233 *image_x = normalized_x * focal_length + principal_point_x;
234 *image_y = normalized_y * focal_length + principal_point_y;
243struct ReprojectionErrorApplyIntrinsics {
244 ReprojectionErrorApplyIntrinsics(
const CameraIntrinsics* invariant_intrinsics,
245 const double observed_distorted_x,
246 const double observed_distorted_y,
248 : invariant_intrinsics_(invariant_intrinsics),
249 observed_distorted_x_(observed_distorted_x),
250 observed_distorted_y_(observed_distorted_y),
253 template <
typename T>
258 T* residuals)
const {
262 ceres::AngleAxisRotatePoint(R_t,
X,
x);
276 T predicted_distorted_x, predicted_distorted_y;
277 ApplyDistortionModelUsingIntrinsicsBlock(invariant_intrinsics_,
281 &predicted_distorted_x,
282 &predicted_distorted_y);
285 residuals[0] = (predicted_distorted_x -
T(observed_distorted_x_)) * weight_;
286 residuals[1] = (predicted_distorted_y -
T(observed_distorted_y_)) * weight_;
290 const CameraIntrinsics* invariant_intrinsics_;
291 const double observed_distorted_x_;
292 const double observed_distorted_y_;
293 const double weight_;
302struct ReprojectionErrorInvertIntrinsics {
303 ReprojectionErrorInvertIntrinsics(
304 const CameraIntrinsics* invariant_intrinsics,
305 const double observed_distorted_x,
306 const double observed_distorted_y,
308 : invariant_intrinsics_(invariant_intrinsics),
309 observed_distorted_x_(observed_distorted_x),
310 observed_distorted_y_(observed_distorted_y),
313 template <
typename T>
318 T* residuals)
const {
321 const T& principal_point_x =
323 const T& principal_point_y =
329 ceres::AngleAxisRotatePoint(R_t,
X,
x);
344 T predicted_x = focal_length * xn + principal_point_x;
345 T predicted_y = focal_length * yn + principal_point_y;
347 T observed_undistorted_normalized_x, observed_undistorted_normalized_y;
348 InvertDistortionModelUsingIntrinsicsBlock(
349 invariant_intrinsics_,
351 T(observed_distorted_x_),
352 T(observed_distorted_y_),
353 &observed_undistorted_normalized_x,
354 &observed_undistorted_normalized_y);
356 T observed_undistorted_image_x, observed_undistorted_image_y;
357 NormalizedToImageSpace(intrinsics,
358 observed_undistorted_normalized_x,
359 observed_undistorted_normalized_y,
360 &observed_undistorted_image_x,
361 &observed_undistorted_image_y);
364 residuals[0] = (predicted_x - observed_undistorted_image_x) * weight_;
365 residuals[1] = (predicted_y - observed_undistorted_image_y) * weight_;
370 const CameraIntrinsics* invariant_intrinsics_;
371 const double observed_distorted_x_;
372 const double observed_distorted_y_;
373 const double weight_;
377void BundleIntrinsicsLogMessage(
const int bundle_intrinsics) {
379 LG <<
"Bundling only camera positions.";
381 std::string bundling_message =
"";
383#define APPEND_BUNDLING_INTRINSICS(name, flag) \
384 if (bundle_intrinsics & flag) { \
385 if (!bundling_message.empty()) { \
386 bundling_message += ", "; \
388 bundling_message += name; \
401 LG <<
"Bundling " << bundling_message <<
".";
410map<int, Vec6> PackCamerasRotationAndTranslation(
412 map<int, Vec6> all_cameras_R_t;
417 ceres::RotationMatrixToAngleAxis(&
camera.R(0, 0), &camera_R_t(0));
418 camera_R_t.tail<3>() =
camera.t;
419 all_cameras_R_t.insert(make_pair(
camera.image, camera_R_t));
422 return all_cameras_R_t;
426void UnpackCamerasRotationAndTranslation(
427 const map<int, Vec6>& all_cameras_R_t,
429 for (map<int, Vec6>::value_type image_and_camera_R_T : all_cameras_R_t) {
430 const int image = image_and_camera_R_T.first;
431 const Vec6& camera_R_t = image_and_camera_R_T.second;
438 ceres::AngleAxisToRotationMatrix(&camera_R_t(0), &
camera->R(0, 0));
439 camera->t = camera_R_t.tail<3>();
448void CRSMatrixToEigenMatrix(
const ceres::CRSMatrix& crs_matrix,
450 eigen_matrix->resize(crs_matrix.num_rows, crs_matrix.num_cols);
451 eigen_matrix->setZero();
453 for (
int row = 0; row < crs_matrix.num_rows; ++row) {
454 int start = crs_matrix.rows[row];
455 int end = crs_matrix.rows[row + 1] - 1;
457 for (
int i = start; i <= end; i++) {
458 int col = crs_matrix.cols[i];
459 double value = crs_matrix.values[i];
461 (*eigen_matrix)(row, col) = value;
466void EuclideanBundlerPerformEvaluation(
const Tracks& tracks,
468 map<int, Vec6>* all_cameras_R_t,
469 ceres::Problem* problem,
473 int num_cameras = all_cameras_R_t->size();
477 for (
int i = 0; i <= max_track; i++) {
488 for (
int j = 0; j < markera_of_track.size(); j++) {
489 if (markera_of_track.at(j).weight != 0.0) {
490 minimized_points.push_back(point);
498 LG <<
"Number of cameras " << num_cameras;
499 LG <<
"Number of points " << num_points;
501 evaluation->num_cameras = num_cameras;
502 evaluation->num_points = num_points;
504 if (evaluation->evaluate_jacobian) {
505 ceres::CRSMatrix evaluated_jacobian;
506 ceres::Problem::EvaluateOptions eval_options;
510 for (
int i = 0; i <= max_image; i++) {
513 double* current_camera_R_t = &(*all_cameras_R_t)[i](0);
516 problem->SetParameterBlockVariable(current_camera_R_t);
518 eval_options.parameter_blocks.push_back(current_camera_R_t);
523 for (
int i = 0; i < minimized_points.size(); i++) {
525 eval_options.parameter_blocks.push_back(&point->
X(0));
528 problem->Evaluate(eval_options, NULL, NULL, NULL, &evaluated_jacobian);
530 CRSMatrixToEigenMatrix(evaluated_jacobian, &evaluation->jacobian);
534template <
typename CostFunction>
535void AddResidualBlockToProblemImpl(
const CameraIntrinsics* invariant_intrinsics,
539 double* intrinsics_block,
542 ceres::Problem* problem) {
543 problem->AddResidualBlock(
544 new ceres::AutoDiffCostFunction<CostFunction,
549 invariant_intrinsics, observed_x, observed_y, weight)),
558 double marker_weight,
559 double* intrinsics_block,
562 ceres::Problem* problem) {
563 if (NeedUseInvertIntrinsicsPipeline(invariant_intrinsics)) {
564 AddResidualBlockToProblemImpl<ReprojectionErrorInvertIntrinsics>(
565 invariant_intrinsics,
574 AddResidualBlockToProblemImpl<ReprojectionErrorApplyIntrinsics>(
575 invariant_intrinsics,
597 map<int, Vec6>& all_cameras_R_t,
598 double* intrinsics_block,
600 ceres::Problem::Options problem_options;
601 ceres::Problem problem(problem_options);
602 int num_residuals = 0;
603 for (
int i = 0; i < markers.size(); ++i) {
604 const Marker& marker = markers[i];
607 if (
camera == NULL || point == NULL) {
613 double* current_camera_R_t = &all_cameras_R_t[
camera->image](0);
615 AddResidualBlockToProblem(invariant_intrinsics,
623 problem.SetParameterBlockConstant(current_camera_R_t);
627 LG <<
"Number of residuals: " << num_residuals;
628 if (!num_residuals) {
629 LG <<
"Skipping running minimizer with zero residuals";
633 problem.SetParameterBlockConstant(intrinsics_block);
636 ceres::Solver::Options
options;
637 options.use_nonmonotonic_steps =
true;
638 options.preconditioner_type = ceres::SCHUR_JACOBI;
639 options.linear_solver_type = ceres::ITERATIVE_SCHUR;
640 options.use_explicit_schur_complement =
true;
641 options.use_inner_iterations =
true;
642 options.max_num_iterations = 100;
643 options.num_threads = std::thread::hardware_concurrency();
646 ceres::Solver::Summary summary;
647 ceres::Solve(
options, &problem, &summary);
649 LG <<
"Final report:\n" << summary.FullReport();
666 const int bundle_intrinsics,
667 const int bundle_constraints,
671 LG <<
"Original intrinsics: " << *intrinsics;
680 intrinsics->
Pack(&packed_intrinsics);
688 map<int, Vec6> all_cameras_R_t =
689 PackCamerasRotationAndTranslation(*reconstruction);
692 ceres::SubsetManifold* constant_translation_manifold = NULL;
694 std::vector<int> constant_translation;
697 constant_translation.push_back(3);
698 constant_translation.push_back(4);
699 constant_translation.push_back(5);
701 constant_translation_manifold =
702 new ceres::SubsetManifold(6, constant_translation);
706 ceres::Problem::Options problem_options;
707 ceres::Problem problem(problem_options);
708 int num_residuals = 0;
709 bool have_locked_camera =
false;
710 for (
int i = 0; i < markers.size(); ++i) {
711 const Marker& marker = markers[i];
714 if (
camera == NULL || point == NULL) {
720 double* current_camera_R_t = &all_cameras_R_t[
camera->image](0);
725 if (marker.
weight != 0.0) {
726 AddResidualBlockToProblem(intrinsics,
736 if (!have_locked_camera) {
737 problem.SetParameterBlockConstant(current_camera_R_t);
738 have_locked_camera =
true;
742 problem.SetManifold(current_camera_R_t, constant_translation_manifold);
745 zero_weight_tracks_flags[marker.
track] =
false;
749 LG <<
"Number of residuals: " << num_residuals;
751 if (!num_residuals) {
752 LG <<
"Skipping running minimizer with zero residuals";
758 LOG(FATAL) <<
"Division model doesn't support bundling "
759 "of tangential distortion";
762 BundleIntrinsicsLogMessage(bundle_intrinsics);
767 problem.SetParameterBlockConstant(intrinsics_block);
772 std::vector<int> constant_intrinsics;
773#define MAYBE_SET_CONSTANT(bundle_enum, offset) \
774 if (!(bundle_intrinsics & bundle_enum) || \
775 !packed_intrinsics.IsParameterDefined(offset)) { \
776 constant_intrinsics.push_back(offset); \
790#undef MAYBE_SET_CONSTANT
792 if (!constant_intrinsics.empty()) {
793 ceres::SubsetManifold* subset_parameterization =
795 constant_intrinsics);
797 problem.SetManifold(intrinsics_block, subset_parameterization);
802 ceres::Solver::Options
options;
803 options.use_nonmonotonic_steps =
true;
804 options.preconditioner_type = ceres::SCHUR_JACOBI;
805 options.linear_solver_type = ceres::ITERATIVE_SCHUR;
806 options.use_explicit_schur_complement =
true;
807 options.use_inner_iterations =
true;
808 options.max_num_iterations = 100;
809 options.num_threads = std::thread::hardware_concurrency();
812 ceres::Solver::Summary summary;
813 ceres::Solve(
options, &problem, &summary);
815 LG <<
"Final report:\n" << summary.FullReport();
818 UnpackCamerasRotationAndTranslation(all_cameras_R_t, reconstruction);
822 intrinsics->
Unpack(packed_intrinsics);
825 LG <<
"Final intrinsics: " << *intrinsics;
828 EuclideanBundlerPerformEvaluation(
829 tracks, reconstruction, &all_cameras_R_t, &problem, evaluation);
835 for (
int track = 0; track < tracks.
MaxTrack(); ++track) {
836 if (zero_weight_tracks_flags[track]) {
838 zero_weight_markers.reserve(zero_weight_markers.size() +
839 current_markers.size());
840 for (
int i = 0; i < current_markers.size(); ++i) {
841 zero_weight_markers.push_back(current_markers[i]);
846 if (zero_weight_markers.size()) {
847 LG <<
"Refining position of constant zero-weighted tracks";
848 EuclideanBundlePointsOnly(intrinsics,
#define MAYBE_SET_CONSTANT(bundle_enum, offset)
#define APPEND_BUNDLING_INTRINSICS(name, flag)
virtual DistortionModelType GetDistortionModelType() const =0
vector< EuclideanCamera > AllCameras() const
Returns all cameras.
EuclideanPoint * PointForTrack(int track)
Returns a pointer to the point corresponding to track.
EuclideanCamera * CameraForImage(int image)
Returns a pointer to the camera corresponding to image.
int MaxTrack() const
Returns the maximum track identifier used.
int MaxImage() const
Returns the maximum image identifier used.
vector< Marker > MarkersForTrack(int track) const
Returns all the markers belonging to a track.
virtual void Unpack(const PackedIntrinsics &packed_intrinsics)
virtual void Pack(PackedIntrinsics *packed_intrinsics) const
virtual DistortionModelType GetDistortionModelType() const =0
EuclideanPoint * PointForTrack(int track)
Returns a pointer to the point corresponding to track.
EuclideanCamera * CameraForImage(int image)
Returns a pointer to the camera corresponding to image.
double * GetParametersBlock()
@ OFFSET_PRINCIPAL_POINT_Y
@ OFFSET_PRINCIPAL_POINT_X
int MaxTrack() const
Returns the maximum track identifier used.
vector< Marker > AllMarkers() const
Returns all the markers.
vector< Marker > MarkersForTrack(int track) const
Returns all the markers belonging to a track.
CCL_NAMESPACE_BEGIN struct Options options
#define image(slot, qualifiers, format)
void camera(KernelGlobals &kg, const ShadingData &sd, float3 &outview, float &outdepth, float &outdist)
Eigen::Matrix< double, 6, 1 > Vec6
void ProjectiveBundle(const Tracks &, ProjectiveReconstruction *)
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)
std::vector< ElementType, Eigen::aligned_allocator< ElementType > > vector
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 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 &p1, const T &p2, const T &image_x, const T &image_y, T *normalized_x, T *normalized_y)
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)