From d2b55d1171af787f603c6c14129224dc1c038e85 Mon Sep 17 00:00:00 2001 From: Sergey Sharybin Date: Mon, 8 Apr 2013 17:05:52 +0000 Subject: Bundle adjustment improvements - Get rid of rotation matrix parameterization, use angle-axis instead. Also Joined rotation and translation into a single parameter block. This made minimization go significantly faster, like 1.3x times in average. - Fix first camera when bundling. This is to address orientation ambiguity. Reconstruction result could still vary in size, but that's another issue to be addressed later. Additional change: Split EuclideanBundleCommonIntrinsics into smaller functions, so it's now a bit easier to follow. --- extern/libmv/libmv/simple_pipeline/bundle.cc | 228 ++++++++++++++------------- 1 file changed, 117 insertions(+), 111 deletions(-) (limited to 'extern') diff --git a/extern/libmv/libmv/simple_pipeline/bundle.cc b/extern/libmv/libmv/simple_pipeline/bundle.cc index d84f7eb8215..ec0d2b1345b 100644 --- a/extern/libmv/libmv/simple_pipeline/bundle.cc +++ b/extern/libmv/libmv/simple_pipeline/bundle.cc @@ -61,8 +61,8 @@ struct OpenCVReprojectionError { template bool operator()(const T* const intrinsics, - const T* const R, // Rotation 3x3 column-major. - const T* const t, // Translation 3x1. + const T* const R_t, // Rotation denoted by angle axis + // followed with translation const T* const X, // Point coordinates 3x1. T* residuals) const { // Unpack the intrinsics. @@ -77,9 +77,11 @@ struct OpenCVReprojectionError { // Compute projective coordinates: x = RX + t. T x[3]; - x[0] = R[0]*X[0] + R[3]*X[1] + R[6]*X[2] + t[0]; - x[1] = R[1]*X[0] + R[4]*X[1] + R[7]*X[2] + t[1]; - x[2] = R[2]*X[0] + R[5]*X[1] + R[8]*X[2] + t[2]; + + ceres::AngleAxisRotatePoint(R_t, X, x); + x[0] += R_t[3]; + x[1] += R_t[4]; + x[2] += R_t[5]; // Compute normalized coordinates: x /= x[2]. T xn = x[0] / x[2]; @@ -121,70 +123,6 @@ struct OpenCVReprojectionError { double observed_y; }; -// TODO(keir): Get rid of the parameterization! Ceres will work much faster if -// the rotation block is angle-axis and also the translation is merged into a -// single parameter block. -struct RotationMatrixPlus { - template - bool operator()(const T* R_array, // Rotation 3x3 col-major. - const T* delta, // Angle-axis delta - T* R_plus_delta_array) const { - T angle_axis[3]; - - ceres::RotationMatrixToAngleAxis(R_array, angle_axis); - - angle_axis[0] += delta[0]; - angle_axis[1] += delta[1]; - angle_axis[2] += delta[2]; - - ceres::AngleAxisToRotationMatrix(angle_axis, R_plus_delta_array); - - return true; - } -}; - -// TODO(sergey): would be nice to have this in Ceres upstream -template -class AutodiffParameterization : public ceres::LocalParameterization { - public: - AutodiffParameterization(const PlusFunctor &plus_functor) - : plus_functor_(plus_functor) {} - - virtual ~AutodiffParameterization() {} - - virtual bool Plus(const double* x, - const double* delta, - double* x_plus_delta) const { - return plus_functor_(x, delta, x_plus_delta); - } - - virtual bool ComputeJacobian(const double* x, double* jacobian) const { - double zero_delta[kLocalSize] = { 0.0 }; - double x_plus_delta[kGlobalSize]; - const double* parameters[2] = { x, zero_delta }; - double* jacobians_array[2] = { NULL, jacobian }; - - Plus(x, zero_delta, x_plus_delta); - - return ceres::internal::AutoDiff - ::Differentiate(plus_functor_, - parameters, - kGlobalSize, - x_plus_delta, - jacobians_array); - - return true; - } - - virtual int GlobalSize() const { return kGlobalSize; } - virtual int LocalSize() const { return kLocalSize; } - - private: - const PlusFunctor &plus_functor_; -}; - void BundleIntrinsicsLogMessage(int bundle_intrinsics) { if (bundle_intrinsics == BUNDLE_NO_INTRINSICS) { LG << "Bundling only camera positions."; @@ -220,6 +158,83 @@ void BundleIntrinsicsLogMessage(int bundle_intrinsics) { } } +// Pack intrinsics from object to an array for easier +// and faster minimization +void PackIntrinisicsIntoArray(CameraIntrinsics *intrinsics, + double ceres_intrinsics[8]) { + ceres_intrinsics[OFFSET_FOCAL_LENGTH] = intrinsics->focal_length(); + ceres_intrinsics[OFFSET_PRINCIPAL_POINT_X] = intrinsics->principal_point_x(); + ceres_intrinsics[OFFSET_PRINCIPAL_POINT_Y] = intrinsics->principal_point_y(); + ceres_intrinsics[OFFSET_K1] = intrinsics->k1(); + ceres_intrinsics[OFFSET_K2] = intrinsics->k2(); + ceres_intrinsics[OFFSET_K3] = intrinsics->k3(); + ceres_intrinsics[OFFSET_P1] = intrinsics->p1(); + ceres_intrinsics[OFFSET_P2] = intrinsics->p2(); +} + +// Unpack intrinsics back from an array to an object +void UnpackIntrinsicsFromArray(CameraIntrinsics *intrinsics, + double ceres_intrinsics[8]) { + intrinsics->SetFocalLength(ceres_intrinsics[OFFSET_FOCAL_LENGTH], + ceres_intrinsics[OFFSET_FOCAL_LENGTH]); + + intrinsics->SetPrincipalPoint(ceres_intrinsics[OFFSET_PRINCIPAL_POINT_X], + ceres_intrinsics[OFFSET_PRINCIPAL_POINT_Y]); + + intrinsics->SetRadialDistortion(ceres_intrinsics[OFFSET_K1], + ceres_intrinsics[OFFSET_K2], + ceres_intrinsics[OFFSET_K3]); + + intrinsics->SetTangentialDistortion(ceres_intrinsics[OFFSET_P1], + ceres_intrinsics[OFFSET_P2]); +} + +// Get a vector of camera's rotations denoted by angle axis +// conjuncted with translations into single block +// +// Element with index i matches to a rotation+translation for +// camera at image i. +vector PackCamerasRotationAndTranslation( + const Tracks &tracks, + EuclideanReconstruction *reconstruction) { + vector cameras_R_t; + int max_image = tracks.MaxImage(); + + cameras_R_t.resize(max_image + 1); + + for (int i = 0; i <= max_image; i++) { + EuclideanCamera *camera = reconstruction->CameraForImage(i); + + if (!camera) + continue; + + ceres::RotationMatrixToAngleAxis(&camera->R(0, 0), + &cameras_R_t[i](0)); + cameras_R_t[i].tail<3>() = camera->t; + } + + return cameras_R_t; +} + +// Convert cameras rotations fro mangle axis back to rotation matrix +void UnpackCamerasRotationAndTranslation( + const Tracks &tracks, + EuclideanReconstruction *reconstruction, + vector cameras_R_t) { + int max_image = tracks.MaxImage(); + + for (int i = 0; i <= max_image; i++) { + EuclideanCamera *camera = reconstruction->CameraForImage(i); + + if (!camera) + continue; + + ceres::AngleAxisToRotationMatrix(&cameras_R_t[i](0), + &camera->R(0, 0)); + camera->t = cameras_R_t[i].tail<3>(); + } +} + } // namespace void EuclideanBundle(const Tracks &tracks, @@ -240,29 +255,24 @@ void EuclideanBundleCommonIntrinsics(const Tracks &tracks, vector markers = tracks.AllMarkers(); ceres::Problem::Options problem_options; - problem_options.local_parameterization_ownership = - ceres::DO_NOT_TAKE_OWNERSHIP; - ceres::Problem problem(problem_options); // Residual blocks with 10 parameters are unwieldly with Ceres, so pack the // intrinsics into a single block and rely on local parameterizations to // control which intrinsics are allowed to vary. double ceres_intrinsics[8]; - ceres_intrinsics[OFFSET_FOCAL_LENGTH] = intrinsics->focal_length(); - ceres_intrinsics[OFFSET_PRINCIPAL_POINT_X] = intrinsics->principal_point_x(); - ceres_intrinsics[OFFSET_PRINCIPAL_POINT_Y] = intrinsics->principal_point_y(); - ceres_intrinsics[OFFSET_K1] = intrinsics->k1(); - ceres_intrinsics[OFFSET_K2] = intrinsics->k2(); - ceres_intrinsics[OFFSET_K3] = intrinsics->k3(); - ceres_intrinsics[OFFSET_P1] = intrinsics->p1(); - ceres_intrinsics[OFFSET_P2] = intrinsics->p2(); + PackIntrinisicsIntoArray(intrinsics, ceres_intrinsics); - RotationMatrixPlus rotation_matrix_plus; - AutodiffParameterization - rotation_parameterization(rotation_matrix_plus); + // Convert cameras rotations to angle axis and merge with translation + // into single parameter block for maximal minimization speed + // + // Block for minimization has got the following structure: + // <3 elements for angle-axis> <3 elements for translation> + vector cameras_R_t = + PackCamerasRotationAndTranslation(tracks, reconstruction); int num_residuals = 0; + bool have_locked_camera = false; for (int i = 0; i < markers.size(); ++i) { const Marker &marker = markers[i]; EuclideanCamera *camera = reconstruction->CameraForImage(marker.image); @@ -271,24 +281,28 @@ void EuclideanBundleCommonIntrinsics(const Tracks &tracks, continue; } + // Rotation of camera denoted in angle axis + double *camera_R_t = &cameras_R_t[camera->image] (0); + problem.AddResidualBlock(new ceres::AutoDiffCostFunction< - OpenCVReprojectionError, 2, 8, 9, 3, 3>( + OpenCVReprojectionError, 2, 8, 6, 3>( new OpenCVReprojectionError( marker.x, marker.y)), NULL, ceres_intrinsics, - &camera->R(0, 0), - &camera->t(0), + camera_R_t, &point->X(0)); - // It's fine if the parameterization for one camera is set repeatedly. - problem.SetParameterization(&camera->R(0, 0), - &rotation_parameterization); + // We lock first camera for better deal with + // scene orientation ambiguity + if (!have_locked_camera) { + problem.SetParameterBlockConstant(camera_R_t); + have_locked_camera = true; + } - if (bundle_constraints & BUNDLE_NO_TRANSLATION) { + if (bundle_constraints & BUNDLE_NO_TRANSLATION) problem.SetParameterBlockConstant(&camera->t(0)); - } num_residuals++; } @@ -301,9 +315,6 @@ void EuclideanBundleCommonIntrinsics(const Tracks &tracks, BundleIntrinsicsLogMessage(bundle_intrinsics); - scoped_ptr - subset_parameterization(NULL); - if (bundle_intrinsics == BUNDLE_NO_INTRINSICS) { // No camera intrinsics are refining, // set the whole parameter block as constant for best performance @@ -328,12 +339,13 @@ void EuclideanBundleCommonIntrinsics(const Tracks &tracks, // Always set K3 constant, it's not used at the moment. constant_intrinsics.push_back(OFFSET_K3); - subset_parameterization.reset( - new ceres::SubsetParameterization(8, constant_intrinsics)); + ceres::SubsetParameterization *subset_parameterization = + new ceres::SubsetParameterization(8, constant_intrinsics); - problem.SetParameterization(ceres_intrinsics, subset_parameterization.get()); + problem.SetParameterization(ceres_intrinsics, subset_parameterization); } + // Configure the solver ceres::Solver::Options options; options.use_nonmonotonic_steps = true; options.preconditioner_type = ceres::SCHUR_JACOBI; @@ -346,26 +358,20 @@ void EuclideanBundleCommonIntrinsics(const Tracks &tracks, options.num_linear_solver_threads = omp_get_max_threads(); #endif + // Solve! ceres::Solver::Summary summary; ceres::Solve(options, &problem, &summary); LG << "Final report:\n" << summary.FullReport(); - // Copy intrinsics back. - if (bundle_intrinsics != BUNDLE_NO_INTRINSICS) { - intrinsics->SetFocalLength(ceres_intrinsics[OFFSET_FOCAL_LENGTH], - ceres_intrinsics[OFFSET_FOCAL_LENGTH]); - - intrinsics->SetPrincipalPoint(ceres_intrinsics[OFFSET_PRINCIPAL_POINT_X], - ceres_intrinsics[OFFSET_PRINCIPAL_POINT_Y]); + // Copy rotations and translations back. + UnpackCamerasRotationAndTranslation(tracks, + reconstruction, + cameras_R_t); - intrinsics->SetRadialDistortion(ceres_intrinsics[OFFSET_K1], - ceres_intrinsics[OFFSET_K2], - ceres_intrinsics[OFFSET_K3]); - - intrinsics->SetTangentialDistortion(ceres_intrinsics[OFFSET_P1], - ceres_intrinsics[OFFSET_P2]); - } + // Copy intrinsics back. + if (bundle_intrinsics != BUNDLE_NO_INTRINSICS) + UnpackIntrinsicsFromArray(intrinsics, ceres_intrinsics); LG << "Final intrinsics: " << *intrinsics; } -- cgit v1.2.3