From 7bceb0687037e2bf0307ff2281e4ad1e0fa50156 Mon Sep 17 00:00:00 2001 From: Fabien Spindler Date: Fri, 19 Apr 2024 11:57:17 +0200 Subject: [PATCH] Apply Misra c++ quality rules --- modules/core/src/display/vpDisplay_rgba.cpp | 6 +- .../core/src/tools/histogram/vpHistogram.cpp | 45 ++++---- .../tracking/forward-projection/vpCircle.cpp | 12 +-- .../detection/src/tag/vpDetectorAprilTag.cpp | 102 ++++++++++-------- modules/detection/src/vpDetectorBase.cpp | 43 +++++--- modules/imgproc/src/vpCLAHE.cpp | 2 +- .../imgproc/src/vpCircleHoughTransform.cpp | 2 +- modules/imgproc/src/vpConnectedComponents.cpp | 2 +- modules/imgproc/src/vpContours.cpp | 28 +++-- modules/imgproc/src/vpFloodFill.cpp | 6 +- modules/imgproc/src/vpImgproc.cpp | 4 +- modules/imgproc/src/vpThreshold.cpp | 12 +-- modules/tracker/blob/src/dots/vpDot.cpp | 21 ++-- modules/tracker/blob/src/dots/vpDot2.cpp | 33 +++--- .../private/vpLevenbergMarquartd.cpp | 19 ++-- modules/vision/src/pose-estimation/vpPose.cpp | 26 +++-- .../src/pose-estimation/vpPoseDementhon.cpp | 13 ++- .../src/pose-estimation/vpPoseLagrange.cpp | 10 +- .../vision/src/pose-estimation/vpPoseLowe.cpp | 3 +- .../vision/src/pose-estimation/vpPoseRGBD.cpp | 10 +- .../src/pose-estimation/vpPoseRansac.cpp | 34 +++--- .../vpPoseVirtualVisualServoing.cpp | 36 +++++-- 22 files changed, 275 insertions(+), 194 deletions(-) diff --git a/modules/core/src/display/vpDisplay_rgba.cpp b/modules/core/src/display/vpDisplay_rgba.cpp index 7cc8971f38..2a3a069823 100644 --- a/modules/core/src/display/vpDisplay_rgba.cpp +++ b/modules/core/src/display/vpDisplay_rgba.cpp @@ -287,7 +287,8 @@ void vpDisplay::displayDotLine(const vpImage &I, const std::list::const_iterator it = ips.begin(); vpImagePoint ip_prev = *(++it); - for (; it != ips.end(); ++it) { + std::list::const_iterator ips_end = ips.end(); + for (; it != ips_end; ++it) { if (vpImagePoint::distance(ip_prev, *it) > 1) { vp_display_display_dot_line(I, ip_prev, *it, color, thickness); ip_prev = *it; @@ -544,7 +545,8 @@ void vpDisplay::displayLine(const vpImage &I, const std::list::const_iterator it = ips.begin(); vpImagePoint ip_prev = *(++it); - for (; it != ips.end(); ++it) { + std::list::const_iterator ips_end = ips.end(); + for (; it != ips_end; ++it) { if (vpImagePoint::distance(ip_prev, *it) > 1) { vp_display_display_line(I, ip_prev, *it, color, thickness); ip_prev = *it; diff --git a/modules/core/src/tools/histogram/vpHistogram.cpp b/modules/core/src/tools/histogram/vpHistogram.cpp index d80ca2cc6d..aca247a683 100644 --- a/modules/core/src/tools/histogram/vpHistogram.cpp +++ b/modules/core/src/tools/histogram/vpHistogram.cpp @@ -343,7 +343,7 @@ void vpHistogram::calculate(const vpImage &I, unsigned int nbins, m_total = 0; while (ptrCurrent != ptrEnd) { if (*ptrMaskCurrent) { - m_histogram[lut[*ptrCurrent]]++; + ++m_histogram[lut[*ptrCurrent]]; ++m_total; } ++ptrCurrent; @@ -543,7 +543,7 @@ void vpHistogram::smooth(unsigned int fsize) // exploitation of the overflow to detect negative value... if (/*(i + j) >= 0 &&*/ (i + static_cast(j)) < m_size) { sum += h.m_histogram[i + static_cast(j)]; - nb++; + ++nb; } } m_histogram[i] = sum / nb; @@ -588,20 +588,20 @@ unsigned vpHistogram::getPeaks(std::list &peaks) if ((prev_slope > 0) && (next_slope == 0)) { sum_level += i + 1; - cpt++; + ++cpt; continue; } // Peak detection if ((prev_slope > 0) && (next_slope < 0)) { sum_level += i; - cpt++; + ++cpt; unsigned int level = sum_level / cpt; p.set(static_cast(level), m_histogram[level]); peaks.push_back(p); - nbpeaks++; + ++nbpeaks; } prev_slope = next_slope; @@ -611,7 +611,7 @@ unsigned vpHistogram::getPeaks(std::list &peaks) if (prev_slope > 0) { p.set(static_cast(m_size) - 1u, m_histogram[m_size - 1]); peaks.push_back(p); - nbpeaks++; + ++nbpeaks; } return nbpeaks; @@ -657,7 +657,8 @@ unsigned vpHistogram::getPeaks(unsigned char dist, vpHistogramPeak &peak1, vpHis // Parse the peaks list to get the peak with a distance greater // than dist to the highest peak1 = peaks.front(); - for (std::list::const_iterator it = peaks.begin(); it != peaks.end(); ++it) { + std::list::const_iterator peaks_end = peaks.end(); + for (std::list::const_iterator it = peaks.begin(); it != peaks_end; ++it) { vpHistogramPeak p = *it; if (abs(p.getLevel() - peak1.getLevel()) > dist) { // The second peak is found @@ -711,8 +712,9 @@ bool vpHistogram::getPeaks(unsigned char dist, vpHistogramPeak &peakl, vpHistogr prev_slope = 1; for (unsigned i = 0; i < (m_size - 1); ++i) { int next_slope = static_cast(m_histogram[i + 1]) - static_cast(m_histogram[i]); // Next histogram inclination - if (next_slope == 0) + if (next_slope == 0) { continue; + } // Peak detection if ((prev_slope > 0) && (next_slope < 0)) { peak[nbpeaks++] = static_cast(i); @@ -800,7 +802,7 @@ bool vpHistogram::getPeaks(unsigned char dist, vpHistogramPeak &peakl, vpHistogr } if (m_histogram[i] == mini) { sumindmini += i; - nbmini++; + ++nbmini; } } @@ -857,20 +859,20 @@ unsigned vpHistogram::getValey(std::list &valey) if ((prev_slope < 0) && (next_slope == 0)) { sum_level += i + 1; - cpt++; + ++cpt; continue; } // Valey detection if ((prev_slope < 0) && (next_slope > 0)) { sum_level += i; - cpt++; + ++cpt; unsigned int level = sum_level / cpt; p.set(static_cast(level), m_histogram[level]); valey.push_back(p); - nbvaley++; + ++nbvaley; } prev_slope = next_slope; @@ -880,7 +882,7 @@ unsigned vpHistogram::getValey(std::list &valey) if (prev_slope < 0) { p.set(static_cast(m_size) - 1u, m_histogram[m_size - 1]); valey.push_back(p); - nbvaley++; + ++nbvaley; } return nbvaley; @@ -932,7 +934,7 @@ bool vpHistogram::getValey(const vpHistogramPeak &peak1, const vpHistogramPeak & } if (m_histogram[i] == mini) { sumindmini += i; - nbmini++; + ++nbmini; } } @@ -998,12 +1000,13 @@ unsigned vpHistogram::getValey(unsigned char dist, const vpHistogramPeak &peak, // Go to the requested peak in the list std::list::const_iterator it; unsigned index = 0; - for (it = peaks.begin(); it != peaks.end(); ++it) { + std::list::const_iterator peaks_end = peaks.end(); + for (it = peaks.begin(); it != peaks_end; ++it) { if (peak == *it) { // we are on the peak. break; } - index++; + ++index; } bool found = false; @@ -1042,7 +1045,7 @@ unsigned vpHistogram::getValey(unsigned char dist, const vpHistogramPeak &peak, } if (m_histogram[i] == mini) { sumindmini += i; - nbmini++; + ++nbmini; } } if (nbmini == 0) { @@ -1063,7 +1066,8 @@ unsigned vpHistogram::getValey(unsigned char dist, const vpHistogramPeak &peak, } // Go to the requested peak in the list std::list::const_iterator it; - for (it = peaks.begin(); it != peaks.end(); ++it) { + std::list::const_iterator peaks_end = peaks.end(); + for (it = peaks.begin(); it != peaks_end; ++it) { if (peak == *it) { // we are on the peak. break; @@ -1073,7 +1077,8 @@ unsigned vpHistogram::getValey(unsigned char dist, const vpHistogramPeak &peak, bool found = false; // search for the nearest peak on the right that matches the distance std::list::const_iterator rit; // right iterator - for (rit = it; rit != peaks.end(); ++rit) { + std::list::const_iterator peaks_end_s = peaks.end(); + for (rit = it; rit != peaks_end_s; ++rit) { if (abs((*rit).getLevel() - peak.getLevel()) > dist) { // peakr found @@ -1101,7 +1106,7 @@ unsigned vpHistogram::getValey(unsigned char dist, const vpHistogramPeak &peak, } if (m_histogram[i] == mini) { sumindmini += i; - nbmini++; + ++nbmini; } } if (nbmini == 0) { diff --git a/modules/core/src/tracking/forward-projection/vpCircle.cpp b/modules/core/src/tracking/forward-projection/vpCircle.cpp index 9b9a43c156..a5674ba4fa 100644 --- a/modules/core/src/tracking/forward-projection/vpCircle.cpp +++ b/modules/core/src/tracking/forward-projection/vpCircle.cpp @@ -421,14 +421,14 @@ void vpCircle::computeIntersectionPoint(const vpCircle &circle, const vpCameraPa double ctheta2 = vpMath::sqr(ctheta); double m02xg = n02 * Xg; double m11stheta = n11 * stheta; - j = ((((((n11 * Xg * sctheta) - (n20 * Yg * sctheta)) + (n20 * rho * ctheta)) - m11yg) + (m11yg * ctheta2) + m02xg - - (m02xg * ctheta2) + (m11stheta * rho)) / - ((((n20 * ctheta2) + (2.0 * m11stheta * ctheta) + n02)) - (n02 * ctheta2))); + j = ((((((((n11 * Xg * sctheta) - (n20 * Yg * sctheta)) + (n20 * rho * ctheta)) - m11yg) + (m11yg * ctheta2) + m02xg) - + (m02xg * ctheta2)) + (m11stheta * rho)) / + (((n20 * ctheta2) + (2.0 * m11stheta * ctheta) + n02) - (n02 * ctheta2))); // Optimised calculation for Y double rhom02 = rho * n02; double sctheta2 = stheta * ctheta2; double ctheta3 = ctheta2 * ctheta; - i = (-((-rho * n11 * stheta * ctheta) - rhom02 + (rhom02 * ctheta2) + (n11 * Xg * sctheta2) - (n20 * Yg * sctheta2) - - (ctheta * n11 * Yg) + (ctheta3 * n11 * Yg) + (ctheta * n02 * Xg) - (ctheta3 * n02 * Xg)) / - ((n20 * ctheta2) + (2.0 * n11 * stheta * ctheta) + n02 - (n02 * ctheta2)) / stheta); + i = (-(((((((-rho * n11 * stheta * ctheta) - rhom02) + (rhom02 * ctheta2) + (n11 * Xg * sctheta2)) - (n20 * Yg * sctheta2)) - + (ctheta * n11 * Yg)) + (ctheta3 * n11 * Yg) + (ctheta * n02 * Xg)) - (ctheta3 * n02 * Xg)) / + (((n20 * ctheta2) + (2.0 * n11 * stheta * ctheta) + n02) - (n02 * ctheta2)) / stheta); } diff --git a/modules/detection/src/tag/vpDetectorAprilTag.cpp b/modules/detection/src/tag/vpDetectorAprilTag.cpp index 4ad66fe131..eeea0d89b8 100644 --- a/modules/detection/src/tag/vpDetectorAprilTag.cpp +++ b/modules/detection/src/tag/vpDetectorAprilTag.cpp @@ -124,7 +124,7 @@ class vpDetectorAprilTag::Impl throw vpException(vpException::fatalError, "Unknown Tag family!"); } - if (m_tagFamily != TAG_36ARTOOLKIT && m_tf) { + if ((m_tagFamily != TAG_36ARTOOLKIT) && m_tf) { m_td = apriltag_detector_create(); apriltag_detector_add_family(m_td, m_tf); } @@ -193,7 +193,7 @@ class vpDetectorAprilTag::Impl throw vpException(vpException::fatalError, "Unknown Tag family!"); } - if (m_tagFamily != TAG_36ARTOOLKIT && m_tf) { + if ((m_tagFamily != TAG_36ARTOOLKIT) && m_tf) { m_td = apriltag_detector_create(); apriltag_detector_add_family(m_td, m_tf); } @@ -278,8 +278,8 @@ class vpDetectorAprilTag::Impl void convertHomogeneousMatrix(const apriltag_pose_t &pose, vpHomogeneousMatrix &cMo) { - for (unsigned int i = 0; i < 3; i++) { - for (unsigned int j = 0; j < 3; j++) { + for (unsigned int i = 0; i < 3; ++i) { + for (unsigned int j = 0; j < 3; ++j) { cMo[i][j] = MATD_EL(pose.R, i, j); } cMo[i][3] = MATD_EL(pose.t, i, 0); @@ -298,8 +298,8 @@ class vpDetectorAprilTag::Impl return false; } #if !defined(VISP_HAVE_APRILTAG_BIG_FAMILY) - if (m_tagFamily == TAG_CIRCLE49h12 || m_tagFamily == TAG_CUSTOM48h12 || m_tagFamily == TAG_STANDARD41h12 || - m_tagFamily == TAG_STANDARD52h13) { + if ((m_tagFamily == TAG_CIRCLE49h12) || (m_tagFamily == TAG_CUSTOM48h12) || (m_tagFamily == TAG_STANDARD41h12) || + (m_tagFamily == TAG_STANDARD52h13)) { std::cerr << "TAG_CIRCLE49h12, TAG_CUSTOM48h12, TAG_STANDARD41h12 and TAG_STANDARD52h13 are disabled." << std::endl; return false; @@ -308,9 +308,9 @@ class vpDetectorAprilTag::Impl const bool computePose = (cMo_vec != nullptr); - image_u8_t im = {/*.width =*/(int32_t)I.getWidth(), - /*.height =*/(int32_t)I.getHeight(), - /*.stride =*/(int32_t)I.getWidth(), + image_u8_t im = {/*.width =*/static_cast(I.getWidth()), + /*.height =*/static_cast(I.getHeight()), + /*.stride =*/static_cast(I.getWidth()), /*.buf =*/I.bitmap }; if (m_detections) { @@ -326,7 +326,8 @@ class vpDetectorAprilTag::Impl messages.resize(static_cast(nb_detections)); m_tagsId.resize(static_cast(nb_detections)); - for (int i = 0; i < zarray_size(m_detections); ++i) { + int zarray_size_m_detections = zarray_size(m_detections); + for (int i = 0; i < zarray_size_m_detections; ++i) { apriltag_detection_t *det; zarray_get(m_detections, i, &det); @@ -346,13 +347,13 @@ class vpDetectorAprilTag::Impl vpColor Ox2 = (color == vpColor::none) ? vpColor::yellow : color; vpColor Oy2 = (color == vpColor::none) ? vpColor::blue : color; - vpDisplay::displayLine(I, (int)det->p[0][1], (int)det->p[0][0], (int)det->p[1][1], (int)det->p[1][0], Ox, + vpDisplay::displayLine(I, static_cast(det->p[0][1]), static_cast(det->p[0][0]), static_cast(det->p[1][1]), static_cast(det->p[1][0]), Ox, thickness); - vpDisplay::displayLine(I, (int)det->p[0][1], (int)det->p[0][0], (int)det->p[3][1], (int)det->p[3][0], Oy, + vpDisplay::displayLine(I, static_cast(det->p[0][1]), static_cast(det->p[0][0]), static_cast(det->p[3][1]), static_cast(det->p[3][0]), Oy, thickness); - vpDisplay::displayLine(I, (int)det->p[1][1], (int)det->p[1][0], (int)det->p[2][1], (int)det->p[2][0], Ox2, + vpDisplay::displayLine(I, static_cast(det->p[1][1]), static_cast(det->p[1][0]), static_cast(det->p[2][1]), static_cast(det->p[2][0]), Ox2, thickness); - vpDisplay::displayLine(I, (int)det->p[2][1], (int)det->p[2][0], (int)det->p[3][1], (int)det->p[3][0], Oy2, + vpDisplay::displayLine(I, static_cast(det->p[2][1]), static_cast(det->p[2][0]), static_cast(det->p[3][1]), static_cast(det->p[3][0]), Oy2, thickness); } @@ -382,7 +383,8 @@ class vpDetectorAprilTag::Impl void displayFrames(const vpImage &I, const std::vector &cMo_vec, const vpCameraParameters &cam, double size, const vpColor &color, unsigned int thickness) const { - for (size_t i = 0; i < cMo_vec.size(); i++) { + size_t cmo_vec_size = cMo_vec.size(); + for (size_t i = 0; i < cmo_vec_size; ++i) { const vpHomogeneousMatrix &cMo = cMo_vec[i]; vpDisplay::displayFrame(I, cMo, cam, size, color, thickness); } @@ -391,7 +393,8 @@ class vpDetectorAprilTag::Impl void displayFrames(const vpImage &I, const std::vector &cMo_vec, const vpCameraParameters &cam, double size, const vpColor &color, unsigned int thickness) const { - for (size_t i = 0; i < cMo_vec.size(); i++) { + size_t cmo_vec_size = cMo_vec.size(); + for (size_t i = 0; i < cmo_vec_size; ++i) { const vpHomogeneousMatrix &cMo = cMo_vec[i]; vpDisplay::displayFrame(I, cMo, cam, size, color, thickness); } @@ -400,7 +403,8 @@ class vpDetectorAprilTag::Impl void displayTags(const vpImage &I, const std::vector > &tagsCorners, const vpColor &color, unsigned int thickness) const { - for (size_t i = 0; i < tagsCorners.size(); i++) { + size_t tagscorners_size = tagsCorners.size(); + for (size_t i = 0; i < tagscorners_size; ++i) { const vpColor Ox = (color == vpColor::none) ? vpColor::red : color; const vpColor Oy = (color == vpColor::none) ? vpColor::green : color; const vpColor Ox2 = (color == vpColor::none) ? vpColor::yellow : color; @@ -409,21 +413,22 @@ class vpDetectorAprilTag::Impl const std::vector &corners = tagsCorners[i]; assert(corners.size() == 4); - vpDisplay::displayLine(I, (int)corners[0].get_i(), (int)corners[0].get_j(), (int)corners[1].get_i(), (int)corners[1].get_j(), - Ox, thickness); - vpDisplay::displayLine(I, (int)corners[0].get_i(), (int)corners[0].get_j(), (int)corners[3].get_i(), (int)corners[3].get_j(), - Oy, thickness); - vpDisplay::displayLine(I, (int)corners[1].get_i(), (int)corners[1].get_j(), (int)corners[2].get_i(), (int)corners[2].get_j(), - Ox2, thickness); - vpDisplay::displayLine(I, (int)corners[2].get_i(), (int)corners[2].get_j(), (int)corners[3].get_i(), (int)corners[3].get_j(), - Oy2, thickness); + vpDisplay::displayLine(I, static_cast(corners[0].get_i()), static_cast(corners[0].get_j()), static_cast(corners[1].get_i()), static_cast(corners[1].get_j()), + Ox, thickness); + vpDisplay::displayLine(I, static_cast(corners[0].get_i()), static_cast(corners[0].get_j()), static_cast(corners[3].get_i()), static_cast(corners[3].get_j()), + Oy, thickness); + vpDisplay::displayLine(I, static_cast(corners[1].get_i()), static_cast(corners[1].get_j()), static_cast(corners[2].get_i()), static_cast(corners[2].get_j()), + Ox2, thickness); + vpDisplay::displayLine(I, static_cast(corners[2].get_i()), static_cast(corners[2].get_j()), static_cast(corners[3].get_i()), static_cast(corners[3].get_j()), + Oy2, thickness); } } void displayTags(const vpImage &I, const std::vector > &tagsCorners, const vpColor &color, unsigned int thickness) const { - for (size_t i = 0; i < tagsCorners.size(); i++) { + size_t tagscorners_size = tagsCorners.size(); + for (size_t i = 0; i < tagscorners_size; ++i) { const vpColor Ox = (color == vpColor::none) ? vpColor::red : color; const vpColor Oy = (color == vpColor::none) ? vpColor::green : color; const vpColor Ox2 = (color == vpColor::none) ? vpColor::yellow : color; @@ -432,14 +437,14 @@ class vpDetectorAprilTag::Impl const std::vector &corners = tagsCorners[i]; assert(corners.size() == 4); - vpDisplay::displayLine(I, (int)corners[0].get_i(), (int)corners[0].get_j(), (int)corners[1].get_i(), (int)corners[1].get_j(), - Ox, thickness); - vpDisplay::displayLine(I, (int)corners[0].get_i(), (int)corners[0].get_j(), (int)corners[3].get_i(), (int)corners[3].get_j(), - Oy, thickness); - vpDisplay::displayLine(I, (int)corners[1].get_i(), (int)corners[1].get_j(), (int)corners[2].get_i(), (int)corners[2].get_j(), - Ox2, thickness); - vpDisplay::displayLine(I, (int)corners[2].get_i(), (int)corners[2].get_j(), (int)corners[3].get_i(), (int)corners[3].get_j(), - Oy2, thickness); + vpDisplay::displayLine(I, static_cast(corners[0].get_i()), static_cast(corners[0].get_j()), static_cast(corners[1].get_i()), static_cast(corners[1].get_j()), + Ox, thickness); + vpDisplay::displayLine(I, static_cast(corners[0].get_i()), static_cast(corners[0].get_j()), static_cast(corners[3].get_i()), static_cast(corners[3].get_j()), + Oy, thickness); + vpDisplay::displayLine(I, static_cast(corners[1].get_i()), static_cast(corners[1].get_j()), static_cast(corners[2].get_i()), static_cast(corners[2].get_j()), + Ox2, thickness); + vpDisplay::displayLine(I, static_cast(corners[2].get_i()), static_cast(corners[2].get_j()), static_cast(corners[3].get_i()), static_cast(corners[3].get_j()), + Oy2, thickness); } } @@ -455,8 +460,8 @@ class vpDetectorAprilTag::Impl return false; } #if !defined(VISP_HAVE_APRILTAG_BIG_FAMILY) - if (m_tagFamily == TAG_CIRCLE49h12 || m_tagFamily == TAG_CUSTOM48h12 || m_tagFamily == TAG_STANDARD41h12 || - m_tagFamily == TAG_STANDARD52h13) { + if ((m_tagFamily == TAG_CIRCLE49h12) || (m_tagFamily == TAG_CUSTOM48h12) || (m_tagFamily == TAG_STANDARD41h12) || + (m_tagFamily == TAG_STANDARD52h13)) { std::cerr << "TAG_CIRCLE49h12, TAG_CUSTOM48h12, TAG_STANDARD41h12 and TAG_STANDARD52h13 are disabled." << std::endl; return false; @@ -467,7 +472,7 @@ class vpDetectorAprilTag::Impl zarray_get(m_detections, static_cast(tagIndex), &det); int nb_detections = zarray_size(m_detections); - if (tagIndex >= (size_t)nb_detections) { + if (tagIndex >= static_cast(nb_detections)) { return false; } @@ -478,8 +483,8 @@ class vpDetectorAprilTag::Impl // Under the hood, we use aligned frames everywhere and transform the pose according to the option. vpHomogeneousMatrix cMo_homography_ortho_iter; - if (m_poseEstimationMethod == HOMOGRAPHY_ORTHOGONAL_ITERATION || - m_poseEstimationMethod == BEST_RESIDUAL_VIRTUAL_VS) { + if ((m_poseEstimationMethod == HOMOGRAPHY_ORTHOGONAL_ITERATION) || + (m_poseEstimationMethod == BEST_RESIDUAL_VIRTUAL_VS)) { double fx = cam.get_px(), fy = cam.get_py(); double cx = cam.get_u0(), cy = cam.get_v0(); @@ -497,8 +502,8 @@ class vpDetectorAprilTag::Impl } vpHomogeneousMatrix cMo_homography; - if (m_poseEstimationMethod == HOMOGRAPHY || m_poseEstimationMethod == HOMOGRAPHY_VIRTUAL_VS || - m_poseEstimationMethod == BEST_RESIDUAL_VIRTUAL_VS) { + if ((m_poseEstimationMethod == HOMOGRAPHY) || (m_poseEstimationMethod == HOMOGRAPHY_VIRTUAL_VS) || + (m_poseEstimationMethod == BEST_RESIDUAL_VIRTUAL_VS)) { double fx = cam.get_px(), fy = cam.get_py(); double cx = cam.get_u0(), cy = cam.get_v0(); @@ -557,8 +562,8 @@ class vpDetectorAprilTag::Impl pose.addPoints(pts); - if (m_poseEstimationMethod != HOMOGRAPHY && m_poseEstimationMethod != HOMOGRAPHY_VIRTUAL_VS && - m_poseEstimationMethod != HOMOGRAPHY_ORTHOGONAL_ITERATION) { + if ((m_poseEstimationMethod != HOMOGRAPHY) && (m_poseEstimationMethod != HOMOGRAPHY_VIRTUAL_VS) && + (m_poseEstimationMethod != HOMOGRAPHY_ORTHOGONAL_ITERATION)) { if (m_poseEstimationMethod == BEST_RESIDUAL_VIRTUAL_VS) { vpHomogeneousMatrix cMo_dementhon, cMo_lagrange; @@ -594,7 +599,7 @@ class vpDetectorAprilTag::Impl } } - if (m_poseEstimationMethod != HOMOGRAPHY && m_poseEstimationMethod != HOMOGRAPHY_ORTHOGONAL_ITERATION) { + if ((m_poseEstimationMethod != HOMOGRAPHY) && (m_poseEstimationMethod != HOMOGRAPHY_ORTHOGONAL_ITERATION)) { // Compute final pose using VVS pose.computePose(vpPose::VIRTUAL_VS, cMo); } @@ -701,10 +706,12 @@ class vpDetectorAprilTag::Impl } matd_destroy(pose2.R); - if (err1) + if (err1) { *err1 = err_1; - if (err2) + } + if (err2) { *err2 = err_2; + } } bool getZAlignedWithCameraAxis() { return m_zAlignedWithCameraFrame; } @@ -1019,7 +1026,8 @@ std::vector > vpDetectorAprilTag::getTagsPoints3D(const std default_size = it->second; // Default size } } - for (size_t i = 0; i < tagsId.size(); i++) { + size_t tagsid_size = tagsId.size(); + for (size_t i = 0; i < tagsid_size; ++i) { std::map::const_iterator it = tagsSize.find(tagsId[i]); double tagSize = default_size; // Default size if (it == tagsSize.end()) { diff --git a/modules/detection/src/vpDetectorBase.cpp b/modules/detection/src/vpDetectorBase.cpp index adb8f9b3e7..f69d44149f 100644 --- a/modules/detection/src/vpDetectorBase.cpp +++ b/modules/detection/src/vpDetectorBase.cpp @@ -39,23 +39,24 @@ /*! Default constructor. */ -vpDetectorBase::vpDetectorBase() : m_polygon(), m_message(), m_nb_objects(0), m_timeout_ms(0) {} +vpDetectorBase::vpDetectorBase() : m_polygon(), m_message(), m_nb_objects(0), m_timeout_ms(0) { } vpDetectorBase::vpDetectorBase(const vpDetectorBase &o) : m_polygon(o.m_polygon), m_message(o.m_message), m_nb_objects(o.m_nb_objects), m_timeout_ms(o.m_timeout_ms) -{ -} +{ } /*! Returns ith object container box as a vector of points. */ std::vector &vpDetectorBase::getPolygon(size_t i) { - if (i < m_polygon.size()) + if (i < m_polygon.size()) { return m_polygon[i]; - else + } + else { throw(vpException(vpException::badValue, "Bad index to retrieve object %d. Only %d objects are detected.", i, m_polygon.size())); + } } /*! @@ -63,11 +64,13 @@ std::vector &vpDetectorBase::getPolygon(size_t i) */ std::string &vpDetectorBase::getMessage(size_t i) { - if (i < m_polygon.size()) + if (i < m_polygon.size()) { return m_message[i]; - else + } + else { throw(vpException(vpException::badValue, "Bad index to retrieve object %d . Only %d objects are detected.", i, m_polygon.size())); + } } /*! @@ -76,10 +79,11 @@ std::string &vpDetectorBase::getMessage(size_t i) vpImagePoint vpDetectorBase::getCog(size_t i) const { vpImagePoint cog(0, 0); - for (size_t j = 0; j < m_polygon[i].size(); j++) { + size_t m_polygon_i_size = m_polygon[i].size(); + for (size_t j = 0; j < m_polygon_i_size; ++j) { cog += m_polygon[i][j]; } - cog /= (double)m_polygon[i].size(); + cog /= static_cast(m_polygon[i].size()); return cog; } @@ -92,19 +96,26 @@ vpRect vpDetectorBase::getBBox(size_t i) const double left, right; double top, bottom; - left = right = m_polygon[i][0].get_u(); - top = bottom = m_polygon[i][0].get_v(); - for (size_t j = 0; j < m_polygon[i].size(); j++) { + left = m_polygon[i][0].get_u(); + right = m_polygon[i][0].get_u(); + top = m_polygon[i][0].get_v(); + bottom = m_polygon[i][0].get_v(); + size_t m_polygon_i_size = m_polygon[i].size(); + for (size_t j = 0; j < m_polygon_i_size; ++j) { double u = m_polygon[i][j].get_u(); double v = m_polygon[i][j].get_v(); - if (u < left) + if (u < left) { left = u; - if (u > right) + } + if (u > right) { right = u; - if (v < top) + } + if (v < top) { top = v; - if (v > bottom) + } + if (v > bottom) { bottom = v; + } } vpRect roi(vpImagePoint(top, left), vpImagePoint(bottom, right)); return roi; diff --git a/modules/imgproc/src/vpCLAHE.cpp b/modules/imgproc/src/vpCLAHE.cpp index f9de470271..7f6b9f4be2 100644 --- a/modules/imgproc/src/vpCLAHE.cpp +++ b/modules/imgproc/src/vpCLAHE.cpp @@ -478,7 +478,7 @@ void clahe(const vpImage &I1, vpImage &I2, int blockRadius, int *ptrCurrent = pa.bitmap[cpt]; ++ptrCurrent; - cpt++; + ++cpt; } } }; diff --git a/modules/imgproc/src/vpCircleHoughTransform.cpp b/modules/imgproc/src/vpCircleHoughTransform.cpp index 39b0a6b051..a4f62b694c 100644 --- a/modules/imgproc/src/vpCircleHoughTransform.cpp +++ b/modules/imgproc/src/vpCircleHoughTransform.cpp @@ -186,7 +186,7 @@ vpCircleHoughTransform::initGradientFilters() filter[r][c] = filter[r][c] * scale; } } - }; + }; #endif if ((m_algoParams.m_gradientFilterKernelSize % 2) != 1) { diff --git a/modules/imgproc/src/vpConnectedComponents.cpp b/modules/imgproc/src/vpConnectedComponents.cpp index 63e2f24d32..be93d2e98e 100644 --- a/modules/imgproc/src/vpConnectedComponents.cpp +++ b/modules/imgproc/src/vpConnectedComponents.cpp @@ -150,7 +150,7 @@ void connectedComponents(const vpImage &I, vpImage &labels, visitNeighbors(I_copy, listOfNeighbors, labels_copy, current_label, connexity); // Increment label - current_label++; + ++current_label; } } } diff --git a/modules/imgproc/src/vpContours.cpp b/modules/imgproc/src/vpContours.cpp index 8aba1ed56c..530f99b96a 100644 --- a/modules/imgproc/src/vpContours.cpp +++ b/modules/imgproc/src/vpContours.cpp @@ -235,7 +235,8 @@ void getContoursList(const vp::vpContour &root, int level, vp::vpContour &contou contour_list.m_children.push_back(contour_node); } - for (std::vector::const_iterator it = root.m_children.begin(); it != root.m_children.end(); ++it) { + std::vector::const_iterator root_m_children_end = root.m_children.end(); + for (std::vector::const_iterator it = root.m_children.begin(); it != root_m_children_end; ++it) { getContoursList(**it, level + 1, contour_list); } } @@ -249,8 +250,10 @@ void drawContours(vpImage &I, const std::vector >::const_iterator it1 = contours.begin(); it1 != contours.end(); ++it1) { - for (std::vector::const_iterator it2 = it1->begin(); it2 != it1->end(); ++it2) { + std::vector >::const_iterator contours_end = contours.end(); + for (std::vector >::const_iterator it1 = contours.begin(); it1 != contours_end; ++it1) { + std::vector::const_iterator it1_end = it1->end(); + for (std::vector::const_iterator it2 = it1->begin(); it2 != it1_end; ++it2) { unsigned int i = static_cast(it2->get_i()); unsigned int j = static_cast(it2->get_j()); I[i][j] = grayValue; @@ -264,8 +267,10 @@ void drawContours(vpImage &I, const std::vector >::const_iterator it1 = contours.begin(); it1 != contours.end(); ++it1) { - for (std::vector::const_iterator it2 = it1->begin(); it2 != it1->end(); ++it2) { + std::vector >::const_iterator contours_end = contours.end(); + for (std::vector >::const_iterator it1 = contours.begin(); it1 != contours_end; ++it1) { + std::vector::const_iterator it1_end = it1->end(); + for (std::vector::const_iterator it2 = it1->begin(); it2 != it1_end; ++it2) { unsigned int i = static_cast(it2->get_i()); unsigned int j = static_cast(it2->get_j()); I[i][j] = vpRGBa(color.R, color.G, color.B); @@ -332,7 +337,7 @@ void findContours(const vpImage &I_original, vpContour &contours, if (isOuter) { //(1) (a) - nbd++; + ++nbd; from.set_j(from.get_j() - 1); border->m_contourType = vp::CONTOUR_OUTER; borderPrime = borderMap[lnbd]; @@ -353,7 +358,7 @@ void findContours(const vpImage &I_original, vpContour &contours, } else { //(1) (b) - nbd++; + ++nbd; if (fji > 1) { lnbd = fji; @@ -406,7 +411,8 @@ void findContours(const vpImage &I_original, vpContour &contours, // Delete contours content contours.m_parent = nullptr; - for (std::vector::iterator it = contours.m_children.begin(); it != contours.m_children.end(); ++it) { + std::vector::iterator contours_m_children_end = contours.m_children.end(); + for (std::vector::iterator it = contours.m_children.begin(); it != contours_m_children_end; ++it) { (*it)->m_parent = nullptr; if (*it != nullptr) { delete *it; @@ -419,7 +425,8 @@ void findContours(const vpImage &I_original, vpContour &contours, if (retrievalMode == CONTOUR_RETR_EXTERNAL) { // Add only external contours - for (std::vector::const_iterator it = root->m_children.begin(); it != root->m_children.end(); ++it) { + std::vector::iterator root_m_children_end = root->m_children.end(); + for (std::vector::const_iterator it = root->m_children.begin(); it != root_m_children_end; ++it) { // Save children std::vector children_copy = (*it)->m_children; // Erase children @@ -440,7 +447,8 @@ void findContours(const vpImage &I_original, vpContour &contours, getContoursList(*root, 0, contours); // Set parent to root - for (std::vector::iterator it = contours.m_children.begin(); it != contours.m_children.end(); ++it) { + std::vector::iterator contours_m_children_end = contours.m_children.end(); + for (std::vector::iterator it = contours.m_children.begin(); it != contours_m_children_end; ++it) { (*it)->m_parent = &contours; } } diff --git a/modules/imgproc/src/vpFloodFill.cpp b/modules/imgproc/src/vpFloodFill.cpp index 0257338462..34cf9c037f 100644 --- a/modules/imgproc/src/vpFloodFill.cpp +++ b/modules/imgproc/src/vpFloodFill.cpp @@ -92,9 +92,9 @@ void floodFill(vpImage &I, const vpImagePoint &seedPoint, const u // Find most left pixel while ((x1 >= 0) && (I[y][x1] == oldValue)) { - x1--; + --x1; } - x1++; + ++x1; bool spanAbove = false, spanBelow = false; @@ -155,7 +155,7 @@ void floodFill(vpImage &I, const vpImagePoint &seedPoint, const u spanAbove = false; } - x1++; + ++x1; } } } diff --git a/modules/imgproc/src/vpImgproc.cpp b/modules/imgproc/src/vpImgproc.cpp index 2ac9a74467..762170f012 100644 --- a/modules/imgproc/src/vpImgproc.cpp +++ b/modules/imgproc/src/vpImgproc.cpp @@ -276,7 +276,7 @@ void equalizeHistogram(vpImage &I, bool useHSV) *ptrCurrent = pa.bitmap[cpt]; ++ptrCurrent; - cpt++; + ++cpt; } } else { @@ -363,7 +363,7 @@ void gammaCorrectionNonLinearMethod(vpImage &I, const vpImage &hist_float) size_t hist_float_size_m_1 = hist_float.size() - 1; for (size_t cpt = 1; cpt < hist_float_size_m_1; ++cpt) { if ((hist_float[cpt - 1] < hist_float[cpt]) && (hist_float[cpt] > hist_float[cpt + 1])) { - modes++; + ++modes; } if (modes > 2) { @@ -73,11 +73,11 @@ int computeThresholdHuang(const vpHistogram &hist) // Find first and last non-empty bin size_t first, last; size_t hist_size = hist.getSize(); - for (first = 0; (first < hist_size) && (hist[static_cast(first)] == 0); first++) { + for (first = 0; (first < hist_size) && (hist[static_cast(first)] == 0); ++first) { // do nothing } - for (last = (hist_size - 1); (last > first) && (hist[static_cast(last)] == 0); last--) { + for (last = (hist_size - 1); (last > first) && (hist[static_cast(last)] == 0); --last) { // do nothing } @@ -172,7 +172,7 @@ int computeThresholdIntermodes(const vpHistogram &hist) // Last value hist_float[hist_float.size() - 1] = (((hist_float.size() - 2) + hist_float.size()) - 1) / 2.0f; - iter++; + ++iter; if (iter > 10000) { std::cerr << "Intermodes Threshold not found after 10000 iterations!" << std::endl; @@ -329,7 +329,7 @@ int computeThresholdTriangle(vpHistogram &hist) int cpt_left = 0; int cpt_right = static_cast(hist.getSize()) - 1; - for (; cpt_left < cpt_right; cpt_left++, cpt_right--) { + for (; cpt_left < cpt_right; ++cpt_left, --cpt_right) { unsigned int temp = hist[cpt_left]; hist.set(cpt_left, hist[cpt_right]); hist.set(cpt_right, temp); @@ -358,7 +358,7 @@ int computeThresholdTriangle(vpHistogram &hist) threshold = cpt; } } - threshold--; + --threshold; if (flip) { threshold = static_cast(hist.getSize()) - 1 - threshold; diff --git a/modules/tracker/blob/src/dots/vpDot.cpp b/modules/tracker/blob/src/dots/vpDot.cpp index 67954c398b..c87d4949d7 100644 --- a/modules/tracker/blob/src/dots/vpDot.cpp +++ b/modules/tracker/blob/src/dots/vpDot.cpp @@ -297,7 +297,7 @@ bool vpDot::connexe(const vpImage &I, unsigned int u, unsigned in // Mean value of the dot intensities mean_value = ((mean_value * (n - 1)) + I[v][u]) / n; if (compute_moment == true) { - m00++; + ++m00; m10 += u; m01 += v; m11 += (u * v); @@ -335,7 +335,7 @@ bool vpDot::connexe(const vpImage &I, unsigned int u, unsigned in } } - if (v + 1 < I.getHeight()) { + if ((v + 1) < I.getHeight()) { if (!checkTab[u + ((v + 1) * I.getWidth())]) { if (!connexe(I, u, v + 1, mean_value, u_cog, v_cog, n, checkTab)) { edge = true; @@ -345,14 +345,14 @@ bool vpDot::connexe(const vpImage &I, unsigned int u, unsigned in if (connexityType == CONNEXITY_8) { if ((v >= 1) && (u >= 1)) { - if (!checkTab[u - 1 + ((v - 1) * I.getWidth())]) { + if (!checkTab[(u - 1) + ((v - 1) * I.getWidth())]) { if (!connexe(I, u - 1, v - 1, mean_value, u_cog, v_cog, n, checkTab)) { edge = true; } } } - if (v >= 1 && (u + 1) < I.getWidth()) { + if ((v >= 1) && ((u + 1) < I.getWidth())) { if (!checkTab[u + 1 + ((v - 1) * I.getWidth())]) { if (!connexe(I, u + 1, v - 1, mean_value, u_cog, v_cog, n, checkTab)) { edge = true; @@ -360,7 +360,7 @@ bool vpDot::connexe(const vpImage &I, unsigned int u, unsigned in } } - if ((v + 1) < I.getHeight() && u >= 1) { + if (((v + 1) < I.getHeight()) && (u >= 1)) { if (!checkTab[(u - 1) + ((v + 1) * I.getWidth())]) { if (!connexe(I, u - 1, v + 1, mean_value, u_cog, v_cog, n, checkTab)) { edge = true; @@ -368,7 +368,7 @@ bool vpDot::connexe(const vpImage &I, unsigned int u, unsigned in } } - if ((v + 1) < I.getHeight() && (u + 1) < I.getWidth()) { + if (((v + 1) < I.getHeight()) && ((u + 1) < I.getWidth())) { if (!checkTab[u + 1 + ((v + 1) * I.getWidth())]) { if (!connexe(I, u + 1, v + 1, mean_value, u_cog, v_cog, n, checkTab)) { edge = true; @@ -851,7 +851,8 @@ void vpDot::display(const vpImage &I, vpColor color, unsigned int vpDisplay::displayCross(I, cog, (3 * thickness) + 8, color, thick); std::list::const_iterator it; - for (it = ip_edges_list.begin(); it != ip_edges_list.end(); ++it) { + std::list::const_iterator ip_edges_list_end = ip_edges_list.end(); + for (it = ip_edges_list.begin(); it != ip_edges_list_end; ++it) { vpDisplay::displayPoint(I, *it, color); } } @@ -908,7 +909,8 @@ void vpDot::display(const vpImage &I, const vpImagePoint &cog, co vpDisplay::displayCross(I, cog, (3 * thickness) + 8, color, thickness); std::list::const_iterator it; - for (it = edges_list.begin(); it != edges_list.end(); ++it) { + std::list::const_iterator edges_list_end = edges_list.end(); + for (it = edges_list.begin(); it != edges_list_end; ++it) { vpDisplay::displayPoint(I, *it, color); } } @@ -933,7 +935,8 @@ void vpDot::display(const vpImage &I, const vpImagePoint &cog, const std vpDisplay::displayCross(I, cog, (3 * thickness) + 8, color, thickness); std::list::const_iterator it; - for (it = edges_list.begin(); it != edges_list.end(); ++it) { + std::list::const_iterator edges_list_end = edges_list.end(); + for (it = edges_list.begin(); it != edges_list_end; ++it) { vpDisplay::displayPoint(I, *it, color); } } diff --git a/modules/tracker/blob/src/dots/vpDot2.cpp b/modules/tracker/blob/src/dots/vpDot2.cpp index 7ff973e046..fe9f9ed377 100644 --- a/modules/tracker/blob/src/dots/vpDot2.cpp +++ b/modules/tracker/blob/src/dots/vpDot2.cpp @@ -214,7 +214,8 @@ void vpDot2::display(const vpImage &I, vpColor color, unsigned in vpDisplay::displayCross(I, cog, (3 * t) + 8, color, t); std::list::const_iterator it; - for (it = ip_edges_list.begin(); it != ip_edges_list.end(); ++it) { + std::list::const_iterator ip_edges_list_end = ip_edges_list.end(); + for (it = ip_edges_list.begin(); it != ip_edges_list_end; ++it) { vpDisplay::displayPoint(I, *it, color); } } @@ -253,6 +254,7 @@ void vpDot2::display(const vpImage &I, vpColor color, unsigned in void vpDot2::initTracking(const vpImage &I, unsigned int size) { while (vpDisplay::getClick(I, cog) != true) { + // block empty waiting user interaction } unsigned int i = static_cast(cog.get_i()); @@ -1073,11 +1075,11 @@ void vpDot2::searchDotsInArea(const vpImage &I, int area_u, int a // position to the list of pixels of previously detected dots cogBadDot = *it_edges; if ((std::fabs(border_u - cogBadDot.get_u()) <= - vpMath::maximum(std::fabs(static_cast(border_u)), std::fabs(cogBadDot.get_u())) * - std::numeric_limits::epsilon()) && + (vpMath::maximum(std::fabs(static_cast(border_u)), std::fabs(cogBadDot.get_u())) * + std::numeric_limits::epsilon())) && (std::fabs(v - cogBadDot.get_v()) <= - vpMath::maximum(std::fabs(static_cast(v)), std::fabs(cogBadDot.get_v())) * - std::numeric_limits::epsilon())) { + (vpMath::maximum(std::fabs(static_cast(v)), std::fabs(cogBadDot.get_v())) * + std::numeric_limits::epsilon()))) { good_germ = false; } ++it_edges; @@ -1358,7 +1360,7 @@ bool vpDot2::isValid(const vpImage &I, const vpDot2 &wantedDot) "%d not in [%u, %u]\n", u, v, cog_u, cog_v, I[v][u], gray_level_min, gray_level_max); #endif - nb_bad_points++; + ++nb_bad_points; } if (graphics) { for (unsigned int t = 0; t < thickness; ++t) { @@ -1406,7 +1408,7 @@ bool vpDot2::isValid(const vpImage &I, const vpDot2 &wantedDot) "%g): %d not in [%u, %u]\n", u, v, cog_u, cog_v, I[v][u], gray_level_min, gray_level_max); #endif - nb_bad_points++; + ++nb_bad_points; } if (graphics) { for (unsigned int t = 0; t < thickness; ++t) { @@ -1793,7 +1795,7 @@ bool vpDot2::findFirstBorder(const vpImage &I, const unsigned int vpDisplay::flush(I); #endif - border_u++; + ++border_u; } return true; } @@ -2231,14 +2233,14 @@ void vpDot2::computeMeanGrayLevel(const vpImage &I) unsigned int pixel_gray = static_cast(I[static_cast(cog_v)][i]); if ((pixel_gray >= getGrayLevelMin()) && (pixel_gray <= getGrayLevelMax())) { sum_value += pixel_gray; - nb_pixels++; + ++nb_pixels; } } for (unsigned int i = static_cast(this->bbox_v_min); i <= static_cast(this->bbox_v_max); ++i) { unsigned char pixel_gray = I[i][static_cast(cog_u)]; if ((pixel_gray >= getGrayLevelMin()) && (pixel_gray <= getGrayLevelMax())) { sum_value += pixel_gray; - nb_pixels++; + ++nb_pixels; } } if (nb_pixels < 10) { // could be good to choose the min nb points from area of dot @@ -2260,7 +2262,7 @@ void vpDot2::computeMeanGrayLevel(const vpImage &I) unsigned int pixel_gray = static_cast(I[static_cast(cog_v + i)][static_cast(cog_u + i)]); if ((pixel_gray >= getGrayLevelMin()) && (pixel_gray <= getGrayLevelMax())) { sum_value += pixel_gray; - nb_pixels++; + ++nb_pixels; } } @@ -2281,7 +2283,7 @@ void vpDot2::computeMeanGrayLevel(const vpImage &I) unsigned char pixel_gray = I[static_cast(cog_v - i)][static_cast(cog_u + i)]; if ((pixel_gray >= getGrayLevelMin()) && (pixel_gray <= getGrayLevelMax())) { sum_value += pixel_gray; - nb_pixels++; + ++nb_pixels; } } } @@ -2464,7 +2466,8 @@ void vpDot2::display(const vpImage &I, const vpImagePoint &cog, vpDisplay::displayCross(I, cog, (3 * thickness) + 8, color, thickness); std::list::const_iterator it; - for (it = edges_list.begin(); it != edges_list.end(); ++it) { + std::list::const_iterator edges_list_end = edges_list.end(); + for (it = edges_list.begin(); it != edges_list_end; ++it) { vpDisplay::displayPoint(I, *it, color); } } @@ -2488,8 +2491,8 @@ void vpDot2::display(const vpImage &I, const vpImagePoint &cog, const st { vpDisplay::displayCross(I, cog, (3 * thickness) + 8, color, thickness); std::list::const_iterator it; - - for (it = edges_list.begin(); it != edges_list.end(); ++it) { + std::list::const_iterator edges_list_end = edges_list.end(); + for (it = edges_list.begin(); it != edges_list_end; ++it) { vpDisplay::displayPoint(I, *it, color); } } diff --git a/modules/vision/src/pose-estimation/private/vpLevenbergMarquartd.cpp b/modules/vision/src/pose-estimation/private/vpLevenbergMarquartd.cpp index f47769667f..ed70e010df 100644 --- a/modules/vision/src/pose-estimation/private/vpLevenbergMarquartd.cpp +++ b/modules/vision/src/pose-estimation/private/vpLevenbergMarquartd.cpp @@ -263,7 +263,7 @@ int lmpar(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, doubl */ for (;;) // iter >= 0) { - iter++; + ++iter; /* * evaluation de la fonction a la valeur courant @@ -296,9 +296,10 @@ int lmpar(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, doubl * ou parl est nul et ou le nombre d'iteration a * atteint 10. */ - if ((std::fabs(fp) <= (tol1 * (*delta))) || - ((std::fabs(parl) <= std::numeric_limits::epsilon()) && ((fp <= temp) && (temp < 0.0))) || - (iter == 10)) { + bool cond_part_1 = (std::fabs(fp) <= (tol1 * (*delta))); + bool cond_part_2 = (std::fabs(parl) <= std::numeric_limits::epsilon()) && ((fp <= temp) && (temp < 0.0)); + bool cond_part_3 = (iter == 10); + if (cond_part_1 || cond_part_2 || cond_part_3) { // terminaison. return 0; @@ -685,8 +686,10 @@ int lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, if (factor <= 0.0) { return 0; } - if ((n <= 0) || (m < n) || (ldfjac < m) || (ftol < 0.0) || (xtol < 0.0) || (gtol < 0.0) || (maxfev == 0) || - (factor <= 0.0)) { + bool cond_part_one = (n <= 0) || (m < n) || (ldfjac < m); + bool cond_part_two = (ftol < 0.0) || (xtol < 0.0) || (gtol < 0.0); + bool cond_part_three = (maxfev == 0) || (factor <= 0.0); + if (cond_part_one || cond_part_two || cond_part_three) { /* * termination, normal ou imposee par l'utilisateur. */ @@ -765,7 +768,7 @@ int lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, * debut de la boucle la plus externe. */ while (count < static_cast(maxfev)) { - count++; + ++count; /* * calcul de la matrice jacobienne. */ @@ -1086,7 +1089,7 @@ int lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, xnorm = enorm(wa2, n); fnorm = fnorm1; - iter++; + ++iter; } /* diff --git a/modules/vision/src/pose-estimation/vpPose.cpp b/modules/vision/src/pose-estimation/vpPose.cpp index 710264ba3c..feebc4522c 100644 --- a/modules/vision/src/pose-estimation/vpPose.cpp +++ b/modules/vision/src/pose-estimation/vpPose.cpp @@ -94,7 +94,7 @@ void vpPose::addPoint(const vpPoint &newP) { listP.push_back(newP); listOfPoints.push_back(newP); - npt++; + ++npt; } void vpPose::addPoints(const std::vector &lP) @@ -141,7 +141,8 @@ bool vpPose::coplanar(int &coplanar_plane_type, double *p_a, double *p_b, double std::vector::const_iterator it_tmp; std::vector::const_iterator it_i, it_j, it_k; - for (it_i = shuffled_listP.begin(); it_i != shuffled_listP.end(); ++it_i) { + std::vector::const_iterator shuffled_listp_end = shuffled_listP.end(); + for (it_i = shuffled_listP.begin(); it_i != shuffled_listp_end; ++it_i) { if (degenerate == false) { // --comment: print "Found a non degenerate configuration" break; @@ -159,7 +160,8 @@ bool vpPose::coplanar(int &coplanar_plane_type, double *p_a, double *p_b, double if (not_on_origin) { it_tmp = it_i; ++it_tmp; // j = i+1 - for (it_j = it_tmp; it_j != shuffled_listP.end(); ++it_j) { + std::vector::const_iterator shuffled_listp_end_1 = shuffled_listP.end(); + for (it_j = it_tmp; it_j != shuffled_listp_end_1; ++it_j) { if (degenerate == false) { // --comment: cout "Found a non degenerate configuration" break; @@ -176,7 +178,8 @@ bool vpPose::coplanar(int &coplanar_plane_type, double *p_a, double *p_b, double if (not_on_origin) { it_tmp = it_j; ++it_tmp; // k = j+1 - for (it_k = it_tmp; it_k != shuffled_listP.end(); ++it_k) { + std::vector::const_iterator shuffled_listp_end_2 = shuffled_listP.end(); + for (it_k = it_tmp; it_k != shuffled_listp_end_2; ++it_k) { P3 = *it_k; if ((std::fabs(P3.get_oX()) <= std::numeric_limits::epsilon()) && (std::fabs(P3.get_oY()) <= std::numeric_limits::epsilon()) && @@ -287,7 +290,8 @@ double vpPose::computeResidual(const vpHomogeneousMatrix &cMo) const { double squared_error = 0; vpPoint P; - for (std::list::const_iterator it = listP.begin(); it != listP.end(); ++it) { + std::list::const_iterator listp_end = listP.end(); + for (std::list::const_iterator it = listP.begin(); it != listp_end; ++it) { P = *it; double x = P.get_x(); double y = P.get_y(); @@ -311,7 +315,8 @@ double vpPose::computeResidual(const vpHomogeneousMatrix &cMo, const vpCameraPar residuals.resize(static_cast(listP.size())); vpPoint P; unsigned int i = 0; - for (std::list::const_iterator it = listP.begin(); it != listP.end(); ++it) { + std::list::const_iterator listp_end = listP.end(); + for (std::list::const_iterator it = listP.begin(); it != listp_end; ++it) { P = *it; double x = P.get_x(); double y = P.get_y(); @@ -539,7 +544,8 @@ bool vpPose::computePoseDementhonLagrangeVVS(vpHomogeneousMatrix &cMo) void vpPose::printPoint() { vpPoint P; - for (std::list::const_iterator it = listP.begin(); it != listP.end(); ++it) { + std::list::const_iterator listp_end = listP.end(); + for (std::list::const_iterator it = listP.begin(); it != listp_end; ++it) { P = *it; std::cout << "3D oP " << P.oP.t(); @@ -563,7 +569,8 @@ void vpPose::displayModel(vpImage &I, vpCameraParameters &cam, vp { vpPoint P; vpImagePoint ip; - for (std::list::const_iterator it = listP.begin(); it != listP.end(); ++it) { + std::list::const_iterator listp_end = listP.end(); + for (std::list::const_iterator it = listP.begin(); it != listp_end; ++it) { P = *it; vpMeterPixelConversion::convertPoint(cam, P.p[0], P.p[1], ip); vpDisplay::displayCross(I, ip, 5, col); @@ -574,7 +581,8 @@ void vpPose::displayModel(vpImage &I, vpCameraParameters &cam, vpColor c { vpPoint P; vpImagePoint ip; - for (std::list::const_iterator it = listP.begin(); it != listP.end(); ++it) { + std::list::const_iterator listp_end = listP.end(); + for (std::list::const_iterator it = listP.begin(); it != listp_end; ++it) { P = *it; vpMeterPixelConversion::convertPoint(cam, P.p[0], P.p[1], ip); vpDisplay::displayCross(I, ip, 5, col); diff --git a/modules/vision/src/pose-estimation/vpPoseDementhon.cpp b/modules/vision/src/pose-estimation/vpPoseDementhon.cpp index 3dc46180be..5429c651c2 100644 --- a/modules/vision/src/pose-estimation/vpPoseDementhon.cpp +++ b/modules/vision/src/pose-estimation/vpPoseDementhon.cpp @@ -102,7 +102,8 @@ void vpPose::poseDementhonNonPlan(vpHomogeneousMatrix &cMo) cdg[0] = 0.0; cdg[1] = 0.0; cdg[2] = 0.0; - for (std::list::const_iterator it = listP.begin(); it != listP.end(); ++it) { + std::list::const_iterator listp_end = listP.end(); + for (std::list::const_iterator it = listP.begin(); it != listp_end; ++it) { P = (*it); cdg[0] += P.get_oX(); cdg[1] += P.get_oY(); @@ -115,7 +116,8 @@ void vpPose::poseDementhonNonPlan(vpHomogeneousMatrix &cMo) c3d.clear(); /* translate the 3D points wrt cog */ - for (std::list::const_iterator it = listP.begin(); it != listP.end(); ++it) { + std::list::const_iterator listp_end_s = listP.end(); + for (std::list::const_iterator it = listP.begin(); it != listp_end_s; ++it) { P = (*it); P.set_oX(P.get_oX() - cdg[0]); P.set_oY(P.get_oY() - cdg[1]); @@ -216,7 +218,7 @@ void vpPose::poseDementhonNonPlan(vpHomogeneousMatrix &cMo) #endif cMo = cMo_old; } - cpt++; + ++cpt; } // go back to the initial frame cMo[0][3] -= ((cdg[0] * cMo[0][0]) + (cdg[1] * cMo[0][1]) + (cdg[2] * cMo[0][2])); @@ -410,7 +412,7 @@ int vpPose::calculArbreDementhon(vpMatrix &Ap, vpColVector &U, vpHomogeneousMatr #endif cMo = cMo_old; } - cpt++; + ++cpt; } /* end of while */ #if (DEBUG_LEVEL1) @@ -448,7 +450,8 @@ void vpPose::poseDementhonPlan(vpHomogeneousMatrix &cMo) c3d.clear(); /* translate the 3D points wrt cog */ - for (std::list::const_iterator it = listP.begin(); it != listP.end(); ++it) { + std::list::const_iterator listp_end = listP.end(); + for (std::list::const_iterator it = listP.begin(); it != listp_end; ++it) { P = (*it); P.set_oX(P.get_oX() - cdg[0]); P.set_oY(P.get_oY() - cdg[1]); diff --git a/modules/vision/src/pose-estimation/vpPoseLagrange.cpp b/modules/vision/src/pose-estimation/vpPoseLagrange.cpp index ed528d5437..8e073977f3 100644 --- a/modules/vision/src/pose-estimation/vpPoseLagrange.cpp +++ b/modules/vision/src/pose-estimation/vpPoseLagrange.cpp @@ -261,7 +261,9 @@ void vpPose::poseLagrangePlan(vpHomogeneousMatrix &cMo, bool *p_isPlan, double * double a, b, c, d; // Checking if coplanar has already been called and if the plan coefficients have been given - if ((p_isPlan != nullptr) && (p_a != nullptr) && (p_b != nullptr) && (p_c != nullptr) && (p_d != nullptr)) { + bool p_isplan_and_p_a_no_null = (p_isPlan != nullptr) && (p_a != nullptr); + bool p_b_p_c_p_d_no_null = (p_b != nullptr) && (p_c != nullptr) && (p_d != nullptr); + if (p_isplan_and_p_a_no_null && p_b_p_c_p_d_no_null) { if (*p_isPlan) { // All the pointers towards the plan coefficients are different from nullptr => using them in the rest of the method a = *p_a; @@ -340,7 +342,8 @@ void vpPose::poseLagrangePlan(vpHomogeneousMatrix &cMo, bool *p_isPlan, double * vpMatrix B(nl, 6); vpPoint P; - for (std::list::const_iterator it = listP.begin(); it != listP.end(); ++it) { + std::list::const_iterator listp_end = listP.end(); + for (std::list::const_iterator it = listP.begin(); it != listp_end; ++it) { P = *it; // Transform each point in plane Z = 0 @@ -485,7 +488,8 @@ void vpPose::poseLagrangeNonPlan(vpHomogeneousMatrix &cMo) vpPoint P; i = 0; - for (std::list::const_iterator it = listP.begin(); it != listP.end(); ++it) { + std::list::const_iterator listp_end = listP.end(); + for (std::list::const_iterator it = listP.begin(); it != listp_end; ++it) { P = *it; a[k][0] = -P.get_oX(); a[k][1] = 0.0; diff --git a/modules/vision/src/pose-estimation/vpPoseLowe.cpp b/modules/vision/src/pose-estimation/vpPoseLowe.cpp index 1cd16e3ade..4c4a89ea6b 100644 --- a/modules/vision/src/pose-estimation/vpPoseLowe.cpp +++ b/modules/vision/src/pose-estimation/vpPoseLowe.cpp @@ -288,7 +288,8 @@ void vpPose::poseLowe(vpHomogeneousMatrix &cMo) vpPoint P; unsigned int i_ = 0; - for (std::list::const_iterator it = listP.begin(); it != listP.end(); ++it) { + std::list::const_iterator listp_end = listP.end(); + for (std::list::const_iterator it = listP.begin(); it != listp_end; ++it) { P = *it; XI[i_] = P.get_x(); // --comment: *cam.px plus cam.xc YI[i_] = P.get_y(); // --comment: *cam.py plus cam.yc diff --git a/modules/vision/src/pose-estimation/vpPoseRGBD.cpp b/modules/vision/src/pose-estimation/vpPoseRGBD.cpp index d207f1a52b..bd519fccc4 100644 --- a/modules/vision/src/pose-estimation/vpPoseRGBD.cpp +++ b/modules/vision/src/pose-estimation/vpPoseRGBD.cpp @@ -55,8 +55,8 @@ void estimatePlaneEquationSVD(const std::vector &point_cloud_face, vpPla tukey.setMinMedianAbsoluteDeviation(1e-4); vpColVector normal; - - for (unsigned int iter = 0; (iter < max_iter) && (std::fabs(error - prev_error) > 1e-6); ++iter) { + double fabs_error_m_prev_error = std::fabs(error - prev_error); + for (unsigned int iter = 0; (iter < max_iter) && (fabs_error_m_prev_error > 1e-6); ++iter) { if (iter != 0) { tukey.MEstimator(vpRobust::TUKEY, residues, weights); } @@ -114,6 +114,8 @@ void estimatePlaneEquationSVD(const std::vector &point_cloud_face, vpPla error += weights[i] * residues[i]; } error /= total_w; + // evaluate one of the end conditions of the for + fabs_error_m_prev_error = std::fabs(error - prev_error); } // Update final weights @@ -355,7 +357,7 @@ bool vpPose::computePlanarObjectPoseFromRGBD(const vpImage &depthMap, p.push_back(vpPoint(x * Z, y * Z, Z)); pose_points[count].set_x(x); pose_points[count].set_y(y); - count++; + ++count; } } } @@ -389,7 +391,7 @@ bool vpPose::computePlanarObjectPoseFromRGBD(const vpImage &depthMap, p.push_back(vpPoint(x * Z, y * Z, Z)); pose_points[count].set_x(x); pose_points[count].set_y(y); - count++; + ++count; } } else { diff --git a/modules/vision/src/pose-estimation/vpPoseRansac.cpp b/modules/vision/src/pose-estimation/vpPoseRansac.cpp index ab8cbc05af..89ebc2ffb8 100644 --- a/modules/vision/src/pose-estimation/vpPoseRansac.cpp +++ b/modules/vision/src/pose-estimation/vpPoseRansac.cpp @@ -140,9 +140,10 @@ struct FindDegeneratePoint bool operator()(const vpPoint &pt) { - return (((std::fabs(m_pt.oP[0] - pt.oP[0]) < EPS) && (std::fabs(m_pt.oP[1] - pt.oP[1]) < EPS) && - (std::fabs(m_pt.oP[2] - pt.oP[2]) < EPS)) || - ((std::fabs(m_pt.p[0] - pt.p[0]) < EPS) && (std::fabs(m_pt.p[1] - pt.p[1]) < EPS))); + bool result_cond1 = ((std::fabs(m_pt.oP[0] - pt.oP[0]) < EPS) && (std::fabs(m_pt.oP[1] - pt.oP[1]) < EPS) + && (std::fabs(m_pt.oP[2] - pt.oP[2]) < EPS)); + bool result_cond2 = (std::fabs(m_pt.p[0] - pt.p[0]) < EPS) && (std::fabs(m_pt.p[1] - pt.p[1]) < EPS); + return result_cond1 || result_cond2; } vpPoint m_pt; @@ -209,12 +210,12 @@ bool vpPose::vpRansacFunctor::poseRansacImpl() poseMin.addPoint(pt); cur_randoms.push_back(r_); // Increment the number of points picked - i++; + ++i; } } if (poseMin.npt < nbMinRandom) { - nbTrials++; + ++nbTrials; continue; } @@ -254,7 +255,8 @@ bool vpPose::vpRansacFunctor::poseRansacImpl() if (isPoseValid && (r < m_ransacThreshold)) { unsigned int nbInliersCur = 0; unsigned int iter = 0; - for (std::vector::const_iterator it = m_listOfUniquePoints.begin(); it != m_listOfUniquePoints.end(); + std::vector::const_iterator m_listofuniquepoints_end = m_listOfUniquePoints.end(); + for (std::vector::const_iterator it = m_listOfUniquePoints.begin(); it != m_listofuniquepoints_end; ++it, ++iter) { p.setWorldCoordinates(it->get_oX(), it->get_oY(), it->get_oZ()); p.track(m_cMo); @@ -271,7 +273,7 @@ bool vpPose::vpRansacFunctor::poseRansacImpl() if (!degenerate) { // the point is considered as inlier if the error is below the // threshold - nbInliersCur++; + ++nbInliersCur; cur_consensus.push_back(iter); cur_inliers.push_back(*it); } @@ -290,18 +292,18 @@ bool vpPose::vpRansacFunctor::poseRansacImpl() m_nbInliers = nbInliersCur; } - nbTrials++; + ++nbTrials; if (nbTrials >= m_ransacMaxTrials) { foundSolution = true; } } else { - nbTrials++; + ++nbTrials; } } else { - nbTrials++; + ++nbTrials; } } @@ -340,7 +342,8 @@ bool vpPose::poseRansac(vpHomogeneousMatrix &cMo, bool (*func)(const vpHomogeneo // Remove degenerate object points std::map filterObjectPointMap; size_t index_pt = 0; - for (std::vector::const_iterator it_pt = listOfPoints.begin(); it_pt != listOfPoints.end(); + std::vector::const_iterator listofpoints_end = listOfPoints.end(); + for (std::vector::const_iterator it_pt = listOfPoints.begin(); it_pt != listofpoints_end; ++it_pt, ++index_pt) { if (filterObjectPointMap.find(*it_pt) == filterObjectPointMap.end()) { filterObjectPointMap[*it_pt] = index_pt; @@ -348,8 +351,9 @@ bool vpPose::poseRansac(vpHomogeneousMatrix &cMo, bool (*func)(const vpHomogeneo } std::map filterImagePointMap; + std::map::const_iterator filterobjectpointmap_end = filterObjectPointMap.end(); for (std::map::const_iterator it = filterObjectPointMap.begin(); - it != filterObjectPointMap.end(); ++it) { + it != filterobjectpointmap_end; ++it) { if (filterImagePointMap.find(it->first) == filterImagePointMap.end()) { filterImagePointMap[it->first] = it->second; @@ -363,7 +367,8 @@ bool vpPose::poseRansac(vpHomogeneousMatrix &cMo, bool (*func)(const vpHomogeneo listOfUniquePoints = listOfPoints; size_t index_pt = 0; - for (std::vector::const_iterator it_pt = listOfPoints.begin(); it_pt != listOfPoints.end(); + std::vector::const_iterator listofpoints_end = listOfPoints.end(); + for (std::vector::const_iterator it_pt = listOfPoints.begin(); it_pt != listofpoints_end; ++it_pt, ++index_pt) { mapOfUniquePointIndex[index_pt] = index_pt; } @@ -482,8 +487,9 @@ bool vpPose::poseRansac(vpHomogeneousMatrix &cMo, bool (*func)(const vpHomogeneo } // Update the list of inlier index + std::vector::const_iterator best_consensus_end = best_consensus.end(); for (std::vector::const_iterator it_index = best_consensus.begin(); - it_index != best_consensus.end(); ++it_index) { + it_index != best_consensus_end; ++it_index) { ransacInlierIndex.push_back(static_cast(mapOfUniquePointIndex[*it_index])); } diff --git a/modules/vision/src/pose-estimation/vpPoseVirtualVisualServoing.cpp b/modules/vision/src/pose-estimation/vpPoseVirtualVisualServoing.cpp index 20965d81b4..65198c9f99 100644 --- a/modules/vision/src/pose-estimation/vpPoseVirtualVisualServoing.cpp +++ b/modules/vision/src/pose-estimation/vpPoseVirtualVisualServoing.cpp @@ -64,12 +64,13 @@ void vpPose::poseVirtualVS(vpHomogeneousMatrix &cMo) // create sd unsigned int k = 0; - for (std::list::const_iterator it = listP.begin(); it != listP.end(); ++it) { + std::list::const_iterator listp_end = listP.end(); + for (std::list::const_iterator it = listP.begin(); it != listp_end; ++it) { P = *it; sd[2 * k] = P.get_x(); sd[(2 * k) + 1] = P.get_y(); lP.push_back(P); - k++; + ++k; } vpHomogeneousMatrix cMoPrev = cMo; @@ -81,7 +82,8 @@ void vpPose::poseVirtualVS(vpHomogeneousMatrix &cMo) // Compute the interaction matrix and the error k = 0; - for (std::list::const_iterator it = lP.begin(); it != lP.end(); ++it) { + std::list::const_iterator lp_end = lP.end(); + for (std::list::const_iterator it = lP.begin(); it != lp_end; ++it) { P = *it; // forward projection of the 3D model for a given pose // change frame coordinates @@ -124,9 +126,12 @@ void vpPose::poseVirtualVS(vpHomogeneousMatrix &cMo) cMoPrev = cMo; cMo = vpExponentialMap::direct(v).inverse() * cMo; - if ((iter++) > vvsIterMax) { + if (iter> vvsIterMax) { break; } + else { + ++iter; + } } if (computeCovariance) { @@ -164,12 +169,13 @@ void vpPose::poseVirtualVSrobust(vpHomogeneousMatrix &cMo) // create sd unsigned int k_ = 0; - for (std::list::const_iterator it = listP.begin(); it != listP.end(); ++it) { + std::list::const_iterator listp_end = listP.end(); + for (std::list::const_iterator it = listP.begin(); it != listp_end; ++it) { P = *it; sd[2 * k_] = P.get_x(); sd[(2 * k_) + 1] = P.get_y(); lP.push_back(P); - k_++; + ++k_; } int iter = 0; res.resize(s.getRows() / 2); @@ -177,13 +183,14 @@ void vpPose::poseVirtualVSrobust(vpHomogeneousMatrix &cMo) W.resize(s.getRows(), s.getRows()); w = 1; - // --comment: while((int)((residu_1 - r)*1e12) !=0) + // --comment: while (residu_1 - r) times 1e12 diff 0 while (std::fabs((residu_1 - r) * 1e12) > std::numeric_limits::epsilon()) { residu_1 = r; // Compute the interaction matrix and the error k_ = 0; - for (std::list::const_iterator it = lP.begin(); it != lP.end(); ++it) { + std::list::const_iterator lp_end = lP.end(); + for (std::list::const_iterator it = lP.begin(); it != lp_end; ++it) { P = *it; // forward projection of the 3D model for a given pose // change frame coordinates @@ -207,7 +214,7 @@ void vpPose::poseVirtualVSrobust(vpHomogeneousMatrix &cMo) L[(2 * k_) + 1][4] = -x * y; L[(2 * k_) + 1][5] = -x; - k_++; + ++k_; } error = s - sd; @@ -234,8 +241,12 @@ void vpPose::poseVirtualVSrobust(vpHomogeneousMatrix &cMo) v = -m_lambda * Lp * W * error; cMo = vpExponentialMap::direct(v).inverse() * cMo; - if ((iter++) > vvsIterMax) + if (iter > vvsIterMax) { break; + } + else { + ++iter; + } } if (computeCovariance) { @@ -326,9 +337,12 @@ std::optional vpPose::poseVirtualVSWithDepth(const std::vec // update the pose cMoPrev = vpExponentialMap::direct(v).inverse() * cMoPrev; - if ((iter++) > vvsIterMax) { + if (iter > vvsIterMax) { return std::nullopt; } + else { + ++iter; + } } return cMoPrev; }