Merge pull request #1430 from fspindle/feat_quality

New fixes for Sonarqube misra c++ quality

modules/core/include/visp3/core/vpImageMorphology.h

@@ -137,6 +137,39 @@ class VISP_EXPORT vpImageMorphology
 #endif
 
 private:
+  template <typename T>
+  class vpPixelOperation
+  {
+  public:
+    vpPixelOperation() { }
+
+    virtual T operator()(const T &, const T &) = 0;
+  };
+
+  template <typename T>
+  class vpPixelOperationMax : public vpPixelOperation<T>
+  {
+  public:
+    vpPixelOperationMax() { }
+
+    virtual T operator()(const T &a, const T &b) VP_OVERRIDE
+    {
+      return std::max<T>(a, b);
+    }
+  };
+
+  template <typename T>
+  class vpPixelOperationMin : public vpPixelOperation<T>
+  {
+  public:
+    vpPixelOperationMin() { }
+
+    T operator()(const T &a, const T &b) VP_OVERRIDE
+    {
+      return std::min<T>(a, b);
+    }
+  };
+
   /**
    * \brief Modify the image by applying the \b operation on each of its elements on a 3x3
    * grid.
@@ -149,7 +182,7 @@ class VISP_EXPORT vpImageMorphology
    * and vertical neighbors, or a 8-connexity, if we want to also take into account the diagonal neighbors.
    */
   template <typename T>
-  static void imageOperation(vpImage<T> &I, const T &null_value, const T &(*operation)(const T &, const T &), const vpConnexityType &connexity = CONNEXITY_4);
+  static void imageOperation(vpImage<T> &I, const T &null_value, vpPixelOperation<T> *operation, const vpConnexityType &connexity = CONNEXITY_4);
 
   /**
    * \brief Modify the image by applying the \b operation on each of its elements on a \b size x \b size
@@ -161,7 +194,7 @@ class VISP_EXPORT vpImageMorphology
    * \param[in] size Size of the kernel of the operation.
    */
   template <typename T>
-  static void imageOperation(vpImage<T> &I, const T &(*operation)(const T &, const T &), const int &size = 3);
+  static void imageOperation(vpImage<T> &I, vpPixelOperation<T> *operation, const int &size = 3);
 
 };
 
@@ -323,7 +356,7 @@ void vpImageMorphology::dilatation(vpImage<Type> &I, Type value, Type value_out,
 }
 
 template<typename T>
-void vpImageMorphology::imageOperation(vpImage<T> &I, const T &null_value, const T &(*operation)(const T &, const T &), const vpConnexityType &connexity)
+void vpImageMorphology::imageOperation(vpImage<T> &I, const T &null_value, vpPixelOperation<T> *operation, const vpConnexityType &connexity)
 {
   const int width_in = I.getWidth();
   const int height_in = I.getHeight();
@@ -343,7 +376,7 @@ void vpImageMorphology::imageOperation(vpImage<T> &I, const T &null_value, const
       for (int j = 0; j < width_in; ++j) {
         T value = null_value;
         for (int k = 0; k < nbOffset; ++k) {
-          value = operation(value, J[i + 1 + offset_y[k]][j + 1 + offset_x[k]]);
+          value = (*operation)(value, J[i + 1 + offset_y[k]][j + 1 + offset_x[k]]);
         }
 
         I[i][j] = value;
@@ -359,7 +392,7 @@ void vpImageMorphology::imageOperation(vpImage<T> &I, const T &null_value, const
       for (int j = 0; j < width_in; ++j) {
         T value = null_value;
         for (int k = 0; k < nbOffset; ++k) {
-          value = operation(value, J[i + 1 + offset_y[k]][j + 1 + offset_x[k]]);
+          value = (*operation)(value, J[i + 1 + offset_y[k]][j + 1 + offset_x[k]]);
         }
 
         I[i][j] = value;
@@ -394,8 +427,8 @@ void vpImageMorphology::imageOperation(vpImage<T> &I, const T &null_value, const
 template <typename T>
 void vpImageMorphology::erosion(vpImage<T> &I, const vpConnexityType &connexity)
 {
-  const T &(*operation)(const T & a, const T & b) = std::min;
-  vpImageMorphology::imageOperation(I, std::numeric_limits<T>::max(), operation, connexity);
+  vpPixelOperationMin<T> operation;
+  vpImageMorphology::imageOperation(I, std::numeric_limits<T>::max(), &operation, connexity);
 }
 
 /*!
@@ -424,12 +457,12 @@ void vpImageMorphology::erosion(vpImage<T> &I, const vpConnexityType &connexity)
 template <typename T>
 void vpImageMorphology::dilatation(vpImage<T> &I, const vpConnexityType &connexity)
 {
-  const T &(*operation)(const T & a, const T & b) = std::max;
-  vpImageMorphology::imageOperation(I, std::numeric_limits<T>::min(), operation, connexity);
+  vpPixelOperationMax<T> operation;
+  vpImageMorphology::imageOperation(I, std::numeric_limits<T>::min(), &operation, connexity);
 }
 
 template<typename T>
-void vpImageMorphology::imageOperation(vpImage<T> &I, const T &(*operation)(const T &, const T &), const int &size)
+void vpImageMorphology::imageOperation(vpImage<T> &I, vpPixelOperation<T> *operation, const int &size)
 {
   if ((size % 2) != 1) {
     throw(vpException(vpException::badValue, "Dilatation/erosion kernel must be odd."));
@@ -461,7 +494,7 @@ void vpImageMorphology::imageOperation(vpImage<T> &I, const T &(*operation)(cons
       }
       for (int r_iterator = r_iterator_start; r_iterator < r_iterator_stop; ++r_iterator) {
         for (int c_iterator = c_iterator_start; c_iterator < c_iterator_stop; ++c_iterator) {
-          value = operation(value, J[r + r_iterator][c + c_iterator]);
+          value = (*operation)(value, J[r + r_iterator][c + c_iterator]);
         }
       }
       I[r][c] = value;
@@ -498,8 +531,8 @@ void vpImageMorphology::imageOperation(vpImage<T> &I, const T &(*operation)(cons
 template <typename T>
 void vpImageMorphology::erosion(vpImage<T> &I, const int &size)
 {
-  const T &(*operation)(const T & a, const T & b) = std::min;
-  vpImageMorphology::imageOperation(I, operation, size);
+  vpPixelOperationMin<T> operation;
+  vpImageMorphology::imageOperation(I, &operation, size);
 }
 
 /**
@@ -530,8 +563,8 @@ void vpImageMorphology::erosion(vpImage<T> &I, const int &size)
 template<typename T>
 void vpImageMorphology::dilatation(vpImage<T> &I, const int &size)
 {
-  const T &(*operation)(const T & a, const T & b) = std::max;
-  vpImageMorphology::imageOperation(I, operation, size);
+  vpPixelOperationMax<T> operation;
+  vpImageMorphology::imageOperation(I, &operation, size);
 }
 END_VISP_NAMESPACE
 #endif

modules/core/include/visp3/core/vpMath.h

@@ -356,9 +356,11 @@ class VISP_EXPORT vpMath
   }
 
   static std::vector<std::pair<double, double> > computeRegularPointsOnSphere(unsigned int maxPoints);
+
+  typedef vpHomogeneousMatrix(*LongLattToHomogeneous)(double lonDeg, double latDeg, double radius);
   static std::vector<vpHomogeneousMatrix>
     getLocalTangentPlaneTransformations(const std::vector<std::pair<double, double> > &lonlatVec, double radius,
-      vpHomogeneousMatrix(*toECEF)(double lonDeg, double latDeg, double radius));
+      LongLattToHomogeneous func);
 
   static vpHomogeneousMatrix lookAt(const vpColVector &from, const vpColVector &to, vpColVector tmp);
 

modules/core/include/visp3/core/vpMatrix.h

@@ -581,6 +581,8 @@ class VISP_EXPORT vpMatrix : public vpArray2D<double>
   int pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, int rank_in) const;
   int pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, int rank_in, vpMatrix &imA, vpMatrix &imAt, vpMatrix &kerAt) const;
 #endif
+
+  vpMatrix dampedInverse(const double &ratioOfMaxSvd = 1e-4) const;
   //@}
 
   //-------------------------------------------------

modules/core/src/camera/vpXmlParserCamera.cpp

@@ -308,9 +308,10 @@ class vpXmlParserCamera::Impl
     // if same name && same projection model && same image size camera already exists, we return SEQUENCE_OK
     // otherwise it is a new camera that need to be updated and we return SEQUENCE_OK
     bool same_name = (cam_name.empty() || (cam_name == camera_name_tmp));
-    bool same_img_size = (abs(static_cast<int>(im_width) - static_cast<int>(image_width_tmp)) < allowedPixelDiffOnImageSize || im_width == 0) &&
-      (abs(static_cast<int>(im_height) - static_cast<int>(image_height_tmp)) < allowedPixelDiffOnImageSize || im_height == 0) &&
-      (test_subsampling_width) && (test_subsampling_height);
+    bool imWidthOk = (abs(static_cast<int>(im_width) - static_cast<int>(image_width_tmp)) < allowedPixelDiffOnImageSize) || (im_width == 0);
+    bool imHeightOk = (abs(static_cast<int>(im_height) - static_cast<int>(image_height_tmp)) < allowedPixelDiffOnImageSize) || (im_height == 0);
+    bool imSizeOk = imWidthOk && imHeightOk;
+    bool same_img_size = imSizeOk && test_subsampling_width && test_subsampling_height;
     if (same_name && same_img_size && same_proj_model) {
       back = SEQUENCE_OK; // Camera exists
       camera = cam_tmp;
@@ -761,10 +762,13 @@ class vpXmlParserCamera::Impl
         }
       }
     }
-    if (!((cam_name == camera_name_tmp) && (im_width == image_width_tmp || im_width == 0) &&
-          (im_height == image_height_tmp || im_height == 0) &&
-          (subsampl_width == subsampling_width_tmp || subsampl_width == 0) &&
-          (subsampl_height == subsampling_height_tmp || subsampl_height == 0))) {
+    bool imHeightOK = (im_height == image_height_tmp) || (im_height == 0);
+    bool imWidthOK = (im_width == image_width_tmp) || (im_width == 0);
+    bool imSizeEqual = imHeightOK && imWidthOK;
+    bool subsampleHeightOK = (subsampl_height == subsampling_height_tmp) || (subsampl_height == 0);
+    bool subsampleWidthOK = (subsampl_width == subsampling_width_tmp) || (subsampl_width == 0);
+    bool subsampleOK = subsampleHeightOK && subsampleWidthOK;
+    if (!((cam_name == camera_name_tmp) && imSizeEqual && subsampleOK)) {
       back = SEQUENCE_ERROR;
     }
     return back;

modules/core/src/display/vpDisplay_impl.h

@@ -571,8 +571,9 @@ template <class Type> void vp_display_display_roi(const vpImage<Type> &I, const
   double roiwidth = floor(roi.getWidth());
   double iheight = static_cast<double>(I.getHeight());
   double iwidth = static_cast<double>(I.getWidth());
-
-  if (top < 0 || top > iheight || left < 0 || left > iwidth || top + roiheight > iheight || left + roiwidth > iwidth) {
+  bool vertNOK = (top < 0) || (top > iheight) || ((top + roiheight) > iheight);
+  bool horNOK = (left < 0) || (left > iwidth) || ((left + roiwidth) > iwidth);
+  if (vertNOK || horNOK) {
     throw(vpException(vpException::dimensionError, "Region of interest outside of the image"));
   }
 

modules/core/src/image/vpImageCircle.cpp

@@ -577,11 +577,14 @@ void computeIntersTopRightBottom(const float &u_c, const float &v_c, const float
   float theta_u_max_bottom = crossing_theta_u_max.first;
   float u_umin_bottom = crossing_theta_u_min.second;
   float u_umax_bottom = crossing_theta_u_max.second;
+  bool crossOnceTopHor = (u_umin_top <= umax_roi) && (u_umax_top > umax_roi);
+  bool dontCrossVert = (v_vmin <= vmin_roi) && (v_vmax >= vmax_roi);
+  bool crossOnceBotHor = (u_umin_bottom <= umax_roi) && (u_umax_bottom > umax_roi);
   if ((u_umax_top <= umax_roi) && (u_umax_bottom <= umax_roi) && (v_vmin >= vmin_roi) && (v_vmax <= vmax_roi)) {
     // case intersection top + right + bottom twice
     delta_theta = (2.f * M_PI_FLOAT) - ((theta_u_min_top - theta_u_max_top) + (theta_v_min - theta_v_max) + (theta_u_max_bottom - theta_u_min_bottom));
   }
-  else if ((u_umin_top <= umax_roi) && (u_umax_top > umax_roi) && (v_vmin <= vmin_roi) && (u_umin_bottom <= umax_roi) && (u_umax_bottom > umax_roi) && (v_vmax >= vmax_roi)) {
+  else if (crossOnceTopHor && crossOnceBotHor  && dontCrossVert) {
     // case intersection top and bottom
     delta_theta = (theta_u_max_top - theta_u_max_bottom);
   }

modules/core/src/math/matrix/vpMatrix_pseudo_inverse.cpp

@@ -1010,4 +1010,29 @@ int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, int rank_in, vpMatrix
 #endif
 }
 
+/*!
+ * Permits to compute an approximated inverse of a matrix that is ill-conditionned.
+ * If the matrix is well-conditionned, the damped inverse is close to the Moore-Penrose pseudo-inverse.
+ *
+ * The corresponding equation is the following, assuming that \f$ \textbf{A} \f$
+ * is the matrix we want to compute the damped inverse:
+ *
+ * \f$ \textbf{A} \approx ( \lambda \textbf{I} + \textbf{A}^T \textbf{A})^{-1} \textbf{A}^T \f$
+ *
+ * \param[in] ratioOfMaxSvd The ratio of the maximum singular value of \f$ M \f$ we want to set \f$ \lambda \f$.
+ * @return vpMatrix The damped inverse.
+ */
+vpMatrix vpMatrix::dampedInverse(const double &ratioOfMaxSvd) const
+{
+  vpColVector w;
+  vpMatrix V, Mcpy(*this);
+  Mcpy.svd(w, V);
+  double maxSingularValue = w.getMaxValue();
+  vpMatrix I;
+  I.eye(this->colNum);
+  double lambda = ratioOfMaxSvd * maxSingularValue;
+  vpMatrix dampedInv = (lambda * I + this->transpose() * *this).inverseByLU()* this->transpose();
+  return dampedInv;
+}
+
 END_VISP_NAMESPACE

modules/core/src/math/misc/vpMath.cpp

@@ -640,7 +640,7 @@ std::vector<std::pair<double, double> > vpMath::computeRegularPointsOnSphere(uns
   \sa enu2ecef(), ned2ecef()
 */
 std::vector<vpHomogeneousMatrix> vpMath::getLocalTangentPlaneTransformations(const std::vector<std::pair<double, double> > &lonlatVec, double radius,
-                                                                             vpHomogeneousMatrix(*toECEF)(double lonDeg_, double latDeg_, double radius_))
+                                                                             LongLattToHomogeneous toECEF)
 {
   std::vector<vpHomogeneousMatrix> ecef_M_local_vec;
   ecef_M_local_vec.reserve(lonlatVec.size());

modules/core/src/math/transformation/vpHomogeneousMatrix.cpp

@@ -995,8 +995,9 @@ bool vpHomogeneousMatrix::isAnHomogeneousMatrix(double threshold) const
   const unsigned int index_2 = 2;
   const unsigned int index_3 = 3;
   const double epsilon = std::numeric_limits<double>::epsilon();
-  return R.isARotationMatrix(threshold) && vpMath::nul((*this)[index_3][index_0], epsilon) && vpMath::nul((*this)[index_3][index_1], epsilon) &&
-    vpMath::nul((*this)[index_3][index_2], epsilon) && vpMath::equal((*this)[index_3][index_3], 1.0, epsilon);
+  bool isLastRowOK = vpMath::nul((*this)[index_3][index_0], epsilon) && vpMath::nul((*this)[index_3][index_1], epsilon) &&
+    vpMath::nul((*this)[index_3][index_2], epsilon);
+  return R.isARotationMatrix(threshold) && isLastRowOK && vpMath::equal((*this)[index_3][index_3], 1.0, epsilon);
 }
 
 /*!

modules/vision/include/visp3/vision/vpPose.h

@@ -117,7 +117,7 @@ class VISP_EXPORT vpPose
 
 public:
   // Typedef a function that checks if a pose is valid.
-  typedef bool (*funcCheckValidityPose)(const vpHomogeneousMatrix &);
+  typedef bool (*FuncCheckValidityPose)(const vpHomogeneousMatrix &);
 
   /*!
    * Default constructor.
@@ -183,7 +183,7 @@ class VISP_EXPORT vpPose
    *   has the smallest residual.
    * - vpPose::RANSAC: Robust Ransac aproach (doesn't need an initialization)
    */
-  bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, funcCheckValidityPose func = nullptr);
+  bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, FuncCheckValidityPose func = nullptr);
 
   /*!
    * @brief Method that first computes the pose \b cMo using the linear approaches of Dementhon and Lagrange
@@ -326,7 +326,7 @@ class VISP_EXPORT vpPose
    * The number of threads used can then be set with setNbParallelRansacThreads().
    * Filter flag can be used  with setRansacFilterFlag().
    */
-  bool poseRansac(vpHomogeneousMatrix &cMo, funcCheckValidityPose func = nullptr);
+  bool poseRansac(vpHomogeneousMatrix &cMo, FuncCheckValidityPose func = nullptr);
 
   /*!
    * Compute the pose using virtual visual servoing approach and
@@ -646,7 +646,7 @@ class VISP_EXPORT vpPose
                         const unsigned int &numberOfInlierToReachAConsensus, const double &threshold,
                         unsigned int &ninliers, std::vector<vpPoint> &listInliers, vpHomogeneousMatrix &cMo,
                         const int &maxNbTrials = 10000, bool useParallelRansac = true, unsigned int nthreads = 0,
-                        funcCheckValidityPose func = nullptr);
+                        FuncCheckValidityPose func = nullptr);
 
 #ifdef VISP_HAVE_HOMOGRAPHY
   /*!
@@ -833,7 +833,7 @@ class VISP_EXPORT vpPose
      */
     vpRansacFunctor(const vpHomogeneousMatrix &cMo_, unsigned int ransacNbInlierConsensus_, const int ransacMaxTrials_,
       double ransacThreshold_, unsigned int initial_seed_, bool checkDegeneratePoints_,
-      const std::vector<vpPoint> &listOfUniquePoints_, funcCheckValidityPose func_)
+      const std::vector<vpPoint> &listOfUniquePoints_, FuncCheckValidityPose func_)
       : m_best_consensus(), m_checkDegeneratePoints(checkDegeneratePoints_), m_cMo(cMo_), m_foundSolution(false),
       m_func(func_), m_listOfUniquePoints(listOfUniquePoints_), m_nbInliers(0), m_ransacMaxTrials(ransacMaxTrials_),
       m_ransacNbInlierConsensus(ransacNbInlierConsensus_), m_ransacThreshold(ransacThreshold_),
@@ -870,7 +870,7 @@ class VISP_EXPORT vpPose
     bool m_checkDegeneratePoints; //!< Flag to check for degenerate points
     vpHomogeneousMatrix m_cMo; //!< Estimated pose
     bool m_foundSolution; //!< Solution found
-    funcCheckValidityPose m_func; //!< Pointer to ransac function
+    FuncCheckValidityPose m_func; //!< Pointer to ransac function
     std::vector<vpPoint> m_listOfUniquePoints; //!< List of unique points
     unsigned int m_nbInliers; //!< Number of inliers
     int m_ransacMaxTrials; //!< Ransac max trial number

modules/vision/src/pose-estimation/private/vpLevenbergMarquartd.cpp

@@ -643,7 +643,7 @@ int qrsolv(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, doub
   return 0;
 }
 
-bool lmderMostInnerLoop(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, int ldfjac, int iflag), int m, int n,
+bool lmderMostInnerLoop(ComputeFunctionAndJacobian ptr_fcn, int m, int n,
           double *x, double *fvec, double *fjac, int ldfjac, double ftol, double xtol, unsigned int maxfev,
           double *diag, int nprint, int *info, unsigned int *nfev, int *ipvt,
           double *qtf, double *wa1, double *wa2, double *wa3, double *wa4, const double &gnorm, int &iter, double &delta, double &par, double &pnorm,
@@ -874,7 +874,7 @@ bool lmderMostInnerLoop(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc,
   return false;
 }
 
-int lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, int ldfjac, int iflag), int m, int n,
+int lmder(ComputeFunctionAndJacobian ptr_fcn, int m, int n,
           double *x, double *fvec, double *fjac, int ldfjac, double ftol, double xtol, double gtol, unsigned int maxfev,
           double *diag, int mode, const double factor, int nprint, int *info, unsigned int *nfev, int *njev, int *ipvt,
           double *qtf, double *wa1, double *wa2, double *wa3, double *wa4)
@@ -1183,7 +1183,7 @@ int lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac,
   return 0;
 }
 
-int lmder1(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, int ldfjac, int iflag), int m, int n,
+int lmder1(ComputeFunctionAndJacobian ptr_fcn, int m, int n,
            double *x, double *fvec, double *fjac, int ldfjac, double tol, int *info, int *ipvt, int lwa, double *wa)
 {
   const double factor = 100.0;

modules/vision/src/pose-estimation/private/vpLevenbergMarquartd.h

@@ -267,6 +267,11 @@ double VISP_EXPORT pythag(double a, double b);
 int VISP_EXPORT qrfac(int m, int n, double *a, int lda, int *pivot, int *ipvt, int lipvt, double *rdiag, double *acnorm,
                       double *wa);
 
+/**
+ * \brief Function pointer towards a method that evaluates a function and its Jacobian.
+ */
+typedef void (*ComputeFunctionAndJacobian)(int m, int n, double *xc, double *fvecc, double *jac, int ldfjac, int iflag);
+
 /*
  * PROCEDURE    : lmder
  *
@@ -372,7 +377,7 @@ int VISP_EXPORT qrfac(int m, int n, double *a, int lda, int *pivot, int *ipvt, i
  * En cas de succes, la valeur zero est retournee.
  * Sinon la valeur -1 est retournee.
  */
-int VISP_EXPORT lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, int ldfjac, int iflag),
+int VISP_EXPORT lmder(ComputeFunctionAndJacobian ptr_fcn,
                       int m, int n, double *x, double *fvec, double *fjac, int ldfjac, double ftol, double xtol,
                       double gtol, unsigned int maxfev, double *diag, int mode, const double factor, int nprint,
                       int *info, unsigned int *nfev, int *njev, int *ipvt, double *qtf, double *wa1, double *wa2,
@@ -457,7 +462,7 @@ int VISP_EXPORT lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, d
  * En cas de succes, la valeur zero est retournee.
  * Sinon, la valeur -1.
  */
-int VISP_EXPORT lmder1(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, int ldfjac, int iflag),
+int VISP_EXPORT lmder1(ComputeFunctionAndJacobian ptr_fcn,
                        int m, int n, double *x, double *fvec, double *fjac, int ldfjac, double tol, int *info,
                        int *ipvt, int lwa, double *wa);