From f0e896509024f036f9383ed121c6e170fd27a8b3 Mon Sep 17 00:00:00 2001 From: Sergey Sharybin Date: Mon, 20 Apr 2020 11:41:01 +0200 Subject: Libmv: Cleanup reprojection cost function Make it smaller and more clear how and what it operates on. --- intern/libmv/libmv/simple_pipeline/bundle.cc | 154 +++++++++++++++------------ 1 file changed, 88 insertions(+), 66 deletions(-) (limited to 'intern/libmv') diff --git a/intern/libmv/libmv/simple_pipeline/bundle.cc b/intern/libmv/libmv/simple_pipeline/bundle.cc index b7d1e1e57fb..7a0451213f7 100644 --- a/intern/libmv/libmv/simple_pipeline/bundle.cc +++ b/intern/libmv/libmv/simple_pipeline/bundle.cc @@ -66,18 +66,82 @@ enum { namespace { -// Cost functor which computes reprojection error of 3D point X -// on camera defined by angle-axis rotation and it's translation -// (which are in the same block due to optimization reasons). +// Apply distorion model (aka distort the input) for the given input in the +// normalized space. +// Only use for distortion models which are analytically defined for their +// Apply() function. // -// This functor uses a radial distortion model. -struct OpenCVReprojectionError { - OpenCVReprojectionError(const CameraIntrinsics *invariant_intrinsics, - const double observed_x, - const double observed_y, - const double weight) +// The invariant_intrinsics are used to access intrinsics which are never +// packed into their parameter block: for example, image dimension. +template +void ApplyIntrinsicsOnNormalizedPoint( + const CameraIntrinsics * invariant_intrinsics, + const T* const intrinsics, + const T& normalized_x, const T& normalized_y, + T* distorted_x, T* distorted_y) { + // 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]; + + // Apply distortion to the normalized points to get (xd, yd). + // + // 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[OFFSET_K1]; + const T& k2 = intrinsics[OFFSET_K2]; + const T& k3 = intrinsics[OFFSET_K3]; + const T& p1 = intrinsics[OFFSET_P1]; + const T& p2 = intrinsics[OFFSET_P2]; + + ApplyPolynomialDistortionModel(focal_length, + focal_length, + principal_point_x, + principal_point_y, + k1, k2, k3, + p1, p2, + normalized_x, normalized_y, + distorted_x, distorted_y); + return; + } + + case DISTORTION_MODEL_DIVISION: + { + const T& k1 = intrinsics[OFFSET_K1]; + const T& k2 = intrinsics[OFFSET_K2]; + + ApplyDivisionDistortionModel(focal_length, + focal_length, + principal_point_x, + principal_point_y, + k1, k2, + normalized_x, normalized_y, + distorted_x, distorted_y); + return; + } + } + + LOG(FATAL) << "Unknown distortion model."; +} + +// Cost functor which computes reprojection error of 3D point X on camera +// defined by angle-axis rotation and it's translation (which are in the same +// block due to optimization reasons). +// +// This functor can only be used for distortion models which have analytically +// defined Apply() function. +struct OpenCVReprojectionErrorApplyIntrinsics { + OpenCVReprojectionErrorApplyIntrinsics( + const CameraIntrinsics *invariant_intrinsics, + const double observed_distorted_x, + const double observed_distorted_y, + const double weight) : invariant_intrinsics_(invariant_intrinsics), - observed_x_(observed_x), observed_y_(observed_y), + observed_distorted_x_(observed_distorted_x), + observed_distorted_y_(observed_distorted_y), weight_(weight) {} template @@ -86,11 +150,6 @@ struct OpenCVReprojectionError { // followed with translation 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]; - // Compute projective coordinates: x = RX + t. T x[3]; @@ -108,59 +167,22 @@ struct OpenCVReprojectionError { T xn = x[0] / x[2]; T yn = x[1] / x[2]; - T predicted_x, predicted_y; - - // Apply distortion to the normalized points to get (xd, yd). - // 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[OFFSET_K1]; - const T& k2 = intrinsics[OFFSET_K2]; - const T& k3 = intrinsics[OFFSET_K3]; - const T& p1 = intrinsics[OFFSET_P1]; - const T& p2 = intrinsics[OFFSET_P2]; - - ApplyPolynomialDistortionModel(focal_length, - focal_length, - principal_point_x, - principal_point_y, - k1, k2, k3, - p1, p2, - xn, yn, - &predicted_x, - &predicted_y); - break; - } - case DISTORTION_MODEL_DIVISION: - { - const T& k1 = intrinsics[OFFSET_K1]; - const T& k2 = intrinsics[OFFSET_K2]; - - ApplyDivisionDistortionModel(focal_length, - focal_length, - principal_point_x, - principal_point_y, - k1, k2, - xn, yn, - &predicted_x, - &predicted_y); - break; - } - default: - LOG(FATAL) << "Unknown distortion model"; - } + T predicted_distorted_x, predicted_distorted_y; + ApplyIntrinsicsOnNormalizedPoint( + invariant_intrinsics_, + intrinsics, + xn, yn, + &predicted_distorted_x, &predicted_distorted_y); // The error is the difference between the predicted and observed position. - residuals[0] = (predicted_x - T(observed_x_)) * weight_; - residuals[1] = (predicted_y - T(observed_y_)) * weight_; + residuals[0] = (predicted_distorted_x - T(observed_distorted_x_)) * weight_; + residuals[1] = (predicted_distorted_y - T(observed_distorted_y_)) * weight_; return true; } const CameraIntrinsics *invariant_intrinsics_; - const double observed_x_; - const double observed_y_; + const double observed_distorted_x_; + const double observed_distorted_y_; const double weight_; }; @@ -397,8 +419,8 @@ void EuclideanBundlePointsOnly(const CameraIntrinsics *invariant_intrinsics, double *current_camera_R_t = &all_cameras_R_t[camera->image](0); problem.AddResidualBlock(new ceres::AutoDiffCostFunction< - OpenCVReprojectionError, 2, OFFSET_MAX, 6, 3>( - new OpenCVReprojectionError( + OpenCVReprojectionErrorApplyIntrinsics, 2, OFFSET_MAX, 6, 3>( + new OpenCVReprojectionErrorApplyIntrinsics( invariant_intrinsics, marker.x, marker.y, @@ -517,8 +539,8 @@ void EuclideanBundleCommonIntrinsics( // This way ceres is not gonna to go crazy. if (marker.weight != 0.0) { problem.AddResidualBlock(new ceres::AutoDiffCostFunction< - OpenCVReprojectionError, 2, OFFSET_MAX, 6, 3>( - new OpenCVReprojectionError( + OpenCVReprojectionErrorApplyIntrinsics, 2, OFFSET_MAX, 6, 3>( + new OpenCVReprojectionErrorApplyIntrinsics( intrinsics, marker.x, marker.y, -- cgit v1.2.3