feat(ceres_intrinsic_camera_calibrator): add FOV regularization

calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/ceres_camera_intrinsics_optimizer.hpp

@@ -48,6 +48,10 @@ class CeresCameraIntrinsicsOptimizer
 
   static constexpr int RESIDUAL_DIM = 2;
 
+  static constexpr int SOLVE_MAX_ATTEMPTS = 5;
+  static constexpr double REPR_THR = 0.1;
+  static constexpr double FOV_THR = 1e-6;
+
   /*!
    * Sets the number of radial distortion coefficients
    * @param[in] radial_distortion_coefficients number of radial distortion coefficients
@@ -70,9 +74,22 @@ class CeresCameraIntrinsicsOptimizer
 
   /*!
    * Sets the regularization weight for the distortion coefficients
-   * @param[in] regularization_weight the regularization weight
+   * @param[in] coeffs_regularization_weight the regularization weight
+   */
+  void setCoeffsRegularizationWeight(double coeffs_regularization_weight);
+
+  /*!
+   * Sets the regularization weight for the field of view
+   * @param[in] fov_regularization_weight the regularization weight
    */
-  void setRegularizationWeight(double regularization_weight);
+  void setFovRegularizationWeight(double fov_regularization_weight);
+
+  /*!
+   * Sets the source dimensions
+   * @param[in] width the width of the source image
+   * @param[in] height the height of the source image
+   */
+  void setSourceDimensions(int width, int height);
 
   /*!
    * Sets the verbose mode
@@ -107,6 +124,18 @@ class CeresCameraIntrinsicsOptimizer
     cv::Mat_<double> & camera_matrix, cv::Mat_<double> & distortion_coeffs,
     std::vector<cv::Mat> & rvecs, std::vector<cv::Mat> & tvecs);
 
+  /*!
+   * Calculates the ceres total error
+   * @return the ceres total error
+   */
+  double getTotalCeresError();
+
+  /*!
+   * Calculates the average ceres error
+   * @return the ceres average error
+   */
+  double getAvgCeresError();
+
   /*!
    * Formats the input data into optimization placeholders
    */
@@ -123,16 +152,26 @@ class CeresCameraIntrinsicsOptimizer
    */
   void evaluate();
 
+  /*!
+   * Evaluates the current optimization variables with the ceres cost function for the field of view
+   * @return the Ceres total field of view error
+   */
+  double evaluateFov();
+
   /*!
    * Formulates and solves the optimization problem
+   * @param[in] use_fov_block whether or not to use the field of view regularization
    */
-  void solve();
+  void solve(bool use_fov_block = false);
 
 protected:
   int radial_distortion_coefficients_;
   bool use_tangential_distortion_;
   int rational_distortion_coefficients_;
-  double regularization_weight_;
+  double coeffs_regularization_weight_;
+  double fov_regularization_weight_;
+  int width_;
+  int height_;
   bool verbose_;
   cv::Mat_<double> camera_matrix_;
   cv::Mat_<double> distortion_coeffs_;