diff options
Diffstat (limited to 'intern/libmv')
| -rw-r--r-- | intern/libmv/libmv/simple_pipeline/bundle.cc | 275 |
1 files changed, 69 insertions, 206 deletions
diff --git a/intern/libmv/libmv/simple_pipeline/bundle.cc b/intern/libmv/libmv/simple_pipeline/bundle.cc index d06942c230b..3df39ab8936 100644 --- a/intern/libmv/libmv/simple_pipeline/bundle.cc +++ b/intern/libmv/libmv/simple_pipeline/bundle.cc @@ -35,34 +35,10 @@ #include "libmv/simple_pipeline/reconstruction.h" #include "libmv/simple_pipeline/tracks.h" #include "libmv/simple_pipeline/distortion_models.h" +#include "libmv/simple_pipeline/packed_intrinsics.h" namespace libmv { -// The intrinsics need to get combined into a single parameter block; use these -// enums to index instead of numeric constants. -enum { - // Camera calibration values. - OFFSET_FOCAL_LENGTH, - OFFSET_PRINCIPAL_POINT_X, - OFFSET_PRINCIPAL_POINT_Y, - - // Distortion model coefficients. - OFFSET_K1, - OFFSET_K2, - OFFSET_K3, - OFFSET_K4, - OFFSET_P1, - OFFSET_P2, - - // Maximal possible offset. - OFFSET_MAX, -}; - -#define FIRST_DISTORTION_COEFFICIENT OFFSET_K1 -#define LAST_DISTORTION_COEFFICIENT OFFSET_P2 -#define NUM_DISTORTION_COEFFICIENTS \ - (LAST_DISTORTION_COEFFICIENT - FIRST_DISTORTION_COEFFICIENT + 1) - namespace { bool NeedUseInvertIntrinsicsPipeline(const CameraIntrinsics *intrinsics) { @@ -90,20 +66,23 @@ void ApplyDistortionModelUsingIntrinsicsBlock( const T& normalized_x, const T& normalized_y, T* distorted_x, T* distorted_y) { // Unpack the intrinsics. - const T& focal_length = intrinsics_block[OFFSET_FOCAL_LENGTH]; - const T& principal_point_x = intrinsics_block[OFFSET_PRINCIPAL_POINT_X]; - const T& principal_point_y = intrinsics_block[OFFSET_PRINCIPAL_POINT_Y]; + const T& focal_length = + intrinsics_block[PackedIntrinsics::OFFSET_FOCAL_LENGTH]; + const T& principal_point_x = + intrinsics_block[PackedIntrinsics::OFFSET_PRINCIPAL_POINT_X]; + const T& principal_point_y = + intrinsics_block[PackedIntrinsics::OFFSET_PRINCIPAL_POINT_Y]; // TODO(keir): Do early bailouts for zero distortion; these are expensive // jet operations. switch (invariant_intrinsics->GetDistortionModelType()) { case DISTORTION_MODEL_POLYNOMIAL: { - const T& k1 = intrinsics_block[OFFSET_K1]; - const T& k2 = intrinsics_block[OFFSET_K2]; - const T& k3 = intrinsics_block[OFFSET_K3]; - const T& p1 = intrinsics_block[OFFSET_P1]; - const T& p2 = intrinsics_block[OFFSET_P2]; + const T& k1 = intrinsics_block[PackedIntrinsics::OFFSET_K1]; + const T& k2 = intrinsics_block[PackedIntrinsics::OFFSET_K2]; + const T& k3 = intrinsics_block[PackedIntrinsics::OFFSET_K3]; + const T& p1 = intrinsics_block[PackedIntrinsics::OFFSET_P1]; + const T& p2 = intrinsics_block[PackedIntrinsics::OFFSET_P2]; ApplyPolynomialDistortionModel(focal_length, focal_length, @@ -118,8 +97,8 @@ void ApplyDistortionModelUsingIntrinsicsBlock( case DISTORTION_MODEL_DIVISION: { - const T& k1 = intrinsics_block[OFFSET_K1]; - const T& k2 = intrinsics_block[OFFSET_K2]; + const T& k1 = intrinsics_block[PackedIntrinsics::OFFSET_K1]; + const T& k2 = intrinsics_block[PackedIntrinsics::OFFSET_K2]; ApplyDivisionDistortionModel(focal_length, focal_length, @@ -139,12 +118,12 @@ void ApplyDistortionModelUsingIntrinsicsBlock( case DISTORTION_MODEL_BROWN: { - const T& k1 = intrinsics_block[OFFSET_K1]; - const T& k2 = intrinsics_block[OFFSET_K2]; - const T& k3 = intrinsics_block[OFFSET_K3]; - const T& k4 = intrinsics_block[OFFSET_K4]; - const T& p1 = intrinsics_block[OFFSET_P1]; - const T& p2 = intrinsics_block[OFFSET_P2]; + const T& k1 = intrinsics_block[PackedIntrinsics::OFFSET_K1]; + const T& k2 = intrinsics_block[PackedIntrinsics::OFFSET_K2]; + const T& k3 = intrinsics_block[PackedIntrinsics::OFFSET_K3]; + const T& k4 = intrinsics_block[PackedIntrinsics::OFFSET_K4]; + const T& p1 = intrinsics_block[PackedIntrinsics::OFFSET_P1]; + const T& p2 = intrinsics_block[PackedIntrinsics::OFFSET_P2]; ApplyBrownDistortionModel(focal_length, focal_length, @@ -180,9 +159,12 @@ void InvertDistortionModelUsingIntrinsicsBlock( const T& image_x, const T& image_y, T* normalized_x, T* normalized_y) { // Unpack the intrinsics. - const T& focal_length = intrinsics_block[OFFSET_FOCAL_LENGTH]; - const T& principal_point_x = intrinsics_block[OFFSET_PRINCIPAL_POINT_X]; - const T& principal_point_y = intrinsics_block[OFFSET_PRINCIPAL_POINT_Y]; + const T& focal_length = + intrinsics_block[PackedIntrinsics::OFFSET_FOCAL_LENGTH]; + const T& principal_point_x = + intrinsics_block[PackedIntrinsics::OFFSET_PRINCIPAL_POINT_X]; + const T& principal_point_y = + intrinsics_block[PackedIntrinsics::OFFSET_PRINCIPAL_POINT_Y]; // TODO(keir): Do early bailouts for zero distortion; these are expensive // jet operations. @@ -195,8 +177,8 @@ void InvertDistortionModelUsingIntrinsicsBlock( case DISTORTION_MODEL_NUKE: { - const T& k1 = intrinsics_block[OFFSET_K1]; - const T& k2 = intrinsics_block[OFFSET_K2]; + const T& k1 = intrinsics_block[PackedIntrinsics::OFFSET_K1]; + const T& k2 = intrinsics_block[PackedIntrinsics::OFFSET_K2]; InvertNukeDistortionModel(focal_length, focal_length, @@ -219,9 +201,12 @@ void NormalizedToImageSpace(const T* const intrinsics_block, const T& normalized_x, const T& normalized_y, T* image_x, T* image_y) { // Unpack the intrinsics. - const T& focal_length = intrinsics_block[OFFSET_FOCAL_LENGTH]; - const T& principal_point_x = intrinsics_block[OFFSET_PRINCIPAL_POINT_X]; - const T& principal_point_y = intrinsics_block[OFFSET_PRINCIPAL_POINT_Y]; + const T& focal_length = + intrinsics_block[PackedIntrinsics::OFFSET_FOCAL_LENGTH]; + const T& principal_point_x = + intrinsics_block[PackedIntrinsics::OFFSET_PRINCIPAL_POINT_X]; + const T& principal_point_y = + intrinsics_block[PackedIntrinsics::OFFSET_PRINCIPAL_POINT_Y]; *image_x = normalized_x * focal_length + principal_point_x; *image_y = normalized_y * focal_length + principal_point_y; @@ -310,9 +295,12 @@ struct ReprojectionErrorInvertIntrinsics { const T* const X, // Point coordinates 3x1. T* residuals) const { // Unpack the intrinsics. - const T& focal_length = intrinsics[OFFSET_FOCAL_LENGTH]; - const T& principal_point_x = intrinsics[OFFSET_PRINCIPAL_POINT_X]; - const T& principal_point_y = intrinsics[OFFSET_PRINCIPAL_POINT_Y]; + const T& focal_length = + intrinsics[PackedIntrinsics::OFFSET_FOCAL_LENGTH]; + const T& principal_point_x = + intrinsics[PackedIntrinsics::OFFSET_PRINCIPAL_POINT_X]; + const T& principal_point_y = + intrinsics[PackedIntrinsics::OFFSET_PRINCIPAL_POINT_Y]; // Compute projective coordinates: x = RX + t. T x[3]; @@ -387,139 +375,6 @@ void BundleIntrinsicsLogMessage(const int bundle_intrinsics) { } } -// Pack intrinsics from object to an array for easier -// and faster minimization. -void PackIntrinisicsIntoArray(const CameraIntrinsics &intrinsics, - double intrinsics_block[OFFSET_MAX]) { - // Pack common intrinsics part. - intrinsics_block[OFFSET_FOCAL_LENGTH] = intrinsics.focal_length(); - intrinsics_block[OFFSET_PRINCIPAL_POINT_X] = intrinsics.principal_point_x(); - intrinsics_block[OFFSET_PRINCIPAL_POINT_Y] = intrinsics.principal_point_y(); - - // Per-model intrinsics block. - // - // The goal here is to get named parameters from the intrinsics object and - // place them into well-defined position within the intrinsics block. This - // simplifies logic of marking parameters constant. - // - // TODO(sergey): The code is very much similar to what is goping on in the - // cost functors. With some templates and helper functions it will be - // possible to reduce level of duplication. - switch (intrinsics.GetDistortionModelType()) { - case DISTORTION_MODEL_POLYNOMIAL: - { - const PolynomialCameraIntrinsics& polynomial_intrinsics = - static_cast<const PolynomialCameraIntrinsics&>(intrinsics); - intrinsics_block[OFFSET_K1] = polynomial_intrinsics.k1(); - intrinsics_block[OFFSET_K2] = polynomial_intrinsics.k2(); - intrinsics_block[OFFSET_K3] = polynomial_intrinsics.k3(); - intrinsics_block[OFFSET_P1] = polynomial_intrinsics.p1(); - intrinsics_block[OFFSET_P2] = polynomial_intrinsics.p2(); - return; - } - - case DISTORTION_MODEL_DIVISION: - { - const DivisionCameraIntrinsics& division_intrinsics = - static_cast<const DivisionCameraIntrinsics&>(intrinsics); - intrinsics_block[OFFSET_K1] = division_intrinsics.k1(); - intrinsics_block[OFFSET_K2] = division_intrinsics.k2(); - return; - } - - case DISTORTION_MODEL_NUKE: - { - const NukeCameraIntrinsics& nuke_intrinsics = - static_cast<const NukeCameraIntrinsics&>(intrinsics); - intrinsics_block[OFFSET_K1] = nuke_intrinsics.k1(); - intrinsics_block[OFFSET_K2] = nuke_intrinsics.k2(); - return; - } - - case DISTORTION_MODEL_BROWN: - { - const BrownCameraIntrinsics& brown_intrinsics = - static_cast<const BrownCameraIntrinsics&>(intrinsics); - intrinsics_block[OFFSET_K1] = brown_intrinsics.k1(); - intrinsics_block[OFFSET_K2] = brown_intrinsics.k2(); - intrinsics_block[OFFSET_K3] = brown_intrinsics.k3(); - intrinsics_block[OFFSET_K4] = brown_intrinsics.k4(); - intrinsics_block[OFFSET_P1] = brown_intrinsics.p1(); - intrinsics_block[OFFSET_P2] = brown_intrinsics.p2(); - return; - } - } - - LOG(FATAL) << "Unknown distortion model."; -} - -// Unpack intrinsics back from an array to an object. -void UnpackIntrinsicsFromArray(const double intrinsics_block[OFFSET_MAX], - CameraIntrinsics *intrinsics) { - // Unpack common intrinsics part. - intrinsics->SetFocalLength(intrinsics_block[OFFSET_FOCAL_LENGTH], - intrinsics_block[OFFSET_FOCAL_LENGTH]); - - intrinsics->SetPrincipalPoint(intrinsics_block[OFFSET_PRINCIPAL_POINT_X], - intrinsics_block[OFFSET_PRINCIPAL_POINT_Y]); - - // Per-model intrinsics block. - // - // The goal here is to get named parameters from the intrinsics object and - // place them into well-defined position within the intrinsics block. This - // simplifies logic of marking parameters constant. - // - // TODO(sergey): The code is very much similar to what is goping on in the - // cost functors. With some templates and helper functions it will be - // possible to reduce level of duplication. - switch (intrinsics->GetDistortionModelType()) { - case DISTORTION_MODEL_POLYNOMIAL: - { - PolynomialCameraIntrinsics* polynomial_intrinsics = - static_cast<PolynomialCameraIntrinsics*>(intrinsics); - polynomial_intrinsics->SetRadialDistortion(intrinsics_block[OFFSET_K1], - intrinsics_block[OFFSET_K2], - intrinsics_block[OFFSET_K3]); - polynomial_intrinsics->SetTangentialDistortion( - intrinsics_block[OFFSET_P1], intrinsics_block[OFFSET_P2]); - return; - } - - case DISTORTION_MODEL_DIVISION: - { - DivisionCameraIntrinsics* division_intrinsics = - static_cast<DivisionCameraIntrinsics*>(intrinsics); - division_intrinsics->SetDistortion(intrinsics_block[OFFSET_K1], - intrinsics_block[OFFSET_K2]); - return; - } - - case DISTORTION_MODEL_NUKE: - { - NukeCameraIntrinsics* nuke_intrinsics = - static_cast<NukeCameraIntrinsics*>(intrinsics); - nuke_intrinsics->SetDistortion(intrinsics_block[OFFSET_K1], - intrinsics_block[OFFSET_K2]); - return; - } - - case DISTORTION_MODEL_BROWN: - { - BrownCameraIntrinsics* brown_intrinsics = - static_cast<BrownCameraIntrinsics*>(intrinsics); - brown_intrinsics->SetRadialDistortion(intrinsics_block[OFFSET_K1], - intrinsics_block[OFFSET_K2], - intrinsics_block[OFFSET_K3], - intrinsics_block[OFFSET_K4]); - brown_intrinsics->SetTangentialDistortion(intrinsics_block[OFFSET_P1], - intrinsics_block[OFFSET_P2]); - return; - } - } - - LOG(FATAL) << "Unknown distortion model."; -} - // Get a vector of camera's rotations denoted by angle axis // conjuncted with translations into single block // @@ -656,12 +511,12 @@ template<typename CostFunction> void AddResidualBlockToProblemImpl(const CameraIntrinsics *invariant_intrinsics, double observed_x, double observed_y, double weight, - double intrinsics_block[OFFSET_MAX], + double *intrinsics_block, double *camera_R_t, EuclideanPoint *point, ceres::Problem* problem) { problem->AddResidualBlock(new ceres::AutoDiffCostFunction< - CostFunction, 2, OFFSET_MAX, 6, 3>( + CostFunction, 2, PackedIntrinsics::NUM_PARAMETERS, 6, 3>( new CostFunction( invariant_intrinsics, observed_x, observed_y, @@ -675,7 +530,7 @@ void AddResidualBlockToProblemImpl(const CameraIntrinsics *invariant_intrinsics, void AddResidualBlockToProblem(const CameraIntrinsics *invariant_intrinsics, const Marker &marker, double marker_weight, - double intrinsics_block[OFFSET_MAX], + double* intrinsics_block, double *camera_R_t, EuclideanPoint *point, ceres::Problem* problem) { @@ -712,7 +567,7 @@ void AddResidualBlockToProblem(const CameraIntrinsics *invariant_intrinsics, void EuclideanBundlePointsOnly(const CameraIntrinsics *invariant_intrinsics, const vector<Marker> &markers, map<int, Vec6> &all_cameras_R_t, - double intrinsics_block[OFFSET_MAX], + double* intrinsics_block, EuclideanReconstruction *reconstruction) { ceres::Problem::Options problem_options; ceres::Problem problem(problem_options); @@ -792,11 +647,11 @@ void EuclideanBundleCommonIntrinsics( // N-th element denotes whether track N is a constant zero-weighted track. vector<bool> zero_weight_tracks_flags(tracks.MaxTrack() + 1, true); - // 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 intrinsics_block[OFFSET_MAX]; - PackIntrinisicsIntoArray(*intrinsics, intrinsics_block); + // Pack all intrinsics parameters into a single block and rely on local + // parameterizations to control which intrinsics are allowed to vary. + PackedIntrinsics packed_intrinsics; + intrinsics->Pack(&packed_intrinsics); + double* intrinsics_block = packed_intrinsics.GetParametersBlock(); // Convert cameras rotations to angle axis and merge with translation // into single parameter block for maximal minimization speed. @@ -892,21 +747,28 @@ void EuclideanBundleCommonIntrinsics( if (!(bundle_intrinsics & bundle_enum)) { \ constant_intrinsics.push_back(offset); \ } - MAYBE_SET_CONSTANT(BUNDLE_FOCAL_LENGTH, OFFSET_FOCAL_LENGTH); - MAYBE_SET_CONSTANT(BUNDLE_PRINCIPAL_POINT, OFFSET_PRINCIPAL_POINT_X); - MAYBE_SET_CONSTANT(BUNDLE_PRINCIPAL_POINT, OFFSET_PRINCIPAL_POINT_Y); - MAYBE_SET_CONSTANT(BUNDLE_RADIAL_K1, OFFSET_K1); - MAYBE_SET_CONSTANT(BUNDLE_RADIAL_K2, OFFSET_K2); - MAYBE_SET_CONSTANT(BUNDLE_TANGENTIAL_P1, OFFSET_P1); - MAYBE_SET_CONSTANT(BUNDLE_TANGENTIAL_P2, OFFSET_P2); + MAYBE_SET_CONSTANT(BUNDLE_FOCAL_LENGTH, + PackedIntrinsics::OFFSET_FOCAL_LENGTH); + MAYBE_SET_CONSTANT(BUNDLE_PRINCIPAL_POINT, + PackedIntrinsics::OFFSET_PRINCIPAL_POINT_X); + MAYBE_SET_CONSTANT(BUNDLE_PRINCIPAL_POINT, + PackedIntrinsics::OFFSET_PRINCIPAL_POINT_Y); + MAYBE_SET_CONSTANT(BUNDLE_RADIAL_K1, PackedIntrinsics::OFFSET_K1); + MAYBE_SET_CONSTANT(BUNDLE_RADIAL_K2, PackedIntrinsics::OFFSET_K2); + MAYBE_SET_CONSTANT(BUNDLE_TANGENTIAL_P1, PackedIntrinsics::OFFSET_P1); + MAYBE_SET_CONSTANT(BUNDLE_TANGENTIAL_P2, PackedIntrinsics::OFFSET_P2); #undef MAYBE_SET_CONSTANT // Always set K3 and K4 constant, it's not used at the moment. - constant_intrinsics.push_back(OFFSET_K3); - constant_intrinsics.push_back(OFFSET_K4); + constant_intrinsics.push_back(PackedIntrinsics::OFFSET_K3); + constant_intrinsics.push_back(PackedIntrinsics::OFFSET_K4); + + // TODO(sergey): Mark all parameters which are not used by the distortion + // model as constant. ceres::SubsetParameterization *subset_parameterization = - new ceres::SubsetParameterization(OFFSET_MAX, constant_intrinsics); + new ceres::SubsetParameterization(PackedIntrinsics::NUM_PARAMETERS, + constant_intrinsics); problem.SetParameterization(intrinsics_block, subset_parameterization); } @@ -931,8 +793,9 @@ void EuclideanBundleCommonIntrinsics( UnpackCamerasRotationAndTranslation(all_cameras_R_t, reconstruction); // Copy intrinsics back. - if (bundle_intrinsics != BUNDLE_NO_INTRINSICS) - UnpackIntrinsicsFromArray(intrinsics_block, intrinsics); + if (bundle_intrinsics != BUNDLE_NO_INTRINSICS) { + intrinsics->Unpack(packed_intrinsics); + } LG << "Final intrinsics: " << *intrinsics; |